initial modification for 5 axis, two extruders only one active

src/libs/Pwm.cpp

@@ -1,6 +1,6 @@
 #include "Pwm.h"
 
-#include "nuts_bolts.h"
+#include "utils.h"
 
 #define PID_PWM_MAX 256
 

src/libs/StepperMotor.cpp

@@ -16,8 +16,6 @@
 StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_pin(dir), en_pin(en)
 {
     set_high_on_debug(en.port_number, en.pin);
-    // register this motor with the step ticker, and get its index in that array and bit position
-    this->index= THEKERNEL->step_ticker->register_motor(this);
 
     steps_per_mm         = 1.0F;
     max_rate             = 50.0F;
@@ -27,6 +25,8 @@ StepperMotor::StepperMotor(Pin &step, Pin &dir, Pin &en) : step_pin(step), dir_p
     current_position_steps= 0;
     enable(false);
     moving= false;
+    acceleration= NAN;
+    selected= true;
 
     this->register_for_event(ON_HALT);
     this->register_for_event(ON_ENABLE);

src/libs/StepperMotor.h

@@ -40,6 +40,10 @@ class StepperMotor  : public Module {
         uint32_t get_current_step(void) const { return current_position_steps; }
         float get_max_rate(void) const { return max_rate; }
         void set_max_rate(float mr) { max_rate= mr; }
+        void set_acceleration(float a) { acceleration= a; }
+        float get_acceleration() const { return acceleration; }
+        bool is_selected() const { return selected; }
+        void set_selected(bool b) { selected= b; }
 
         int  steps_to_target(float);
 
@@ -61,6 +65,7 @@ class StepperMotor  : public Module {
         float steps_per_second;
         float steps_per_mm;
         float max_rate; // this is not really rate it is in mm/sec, misnamed used in Robot and Extruder
+        float acceleration;
 
         volatile int32_t current_position_steps;
         int32_t last_milestone_steps;
@@ -69,6 +74,7 @@ class StepperMotor  : public Module {
         volatile struct {
             volatile bool direction:1;
             volatile bool moving:1;
+            bool selected:1;
         };
 };
 

src/libs/nuts_bolts.h

@@ -24,6 +24,7 @@ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
 #define X_AXIS 0
 #define Y_AXIS 1
 #define Z_AXIS 2
+#define E_AXIS 3
 
 #define ALPHA_STEPPER 0
 #define BETA_STEPPER 1
@@ -32,13 +33,5 @@ along with Grbl. If not, see <http://www.gnu.org/licenses/>.
 #define EPSILON_STEPPER 4
 #define ZETA_STEPPER 5
 
-#define clear_vector(a) memset(a, 0, sizeof(a))
-#define clear_vector_float(a) memset(a, 0, sizeof(a))
-
-#define confine(value, min, max) (((value) < (min))?(min):(((value) > (max))?(max):(value)))
-
-#define dd(...) LPC_GPIO2->FIODIR = 0xffff; LPC_GPIO2->FIOCLR = 0xffff; LPC_GPIO2->FIOSET = __VA_ARGS__
-
-
 #endif
 

src/libs/utils.h

@@ -5,40 +5,42 @@
 #include <string>
 #include <vector>
 
-using std::string;
-using std::vector;
-
-string lc(const string& str);
+std::string lc(const std::string& str);
 
 bool is_alpha( int );
 bool is_digit( int );
 bool is_numeric( int );
 bool is_alphanum( int );
 bool is_whitespace( int );
 
-vector<string> split(const char *str, char c = ',');
-vector<float> parse_number_list(const char *str);
-vector<uint32_t> parse_number_list(const char *str, uint8_t radix);
+std::vector<std::string> split(const char *str, char c = ',');
+std::vector<float> parse_number_list(const char *str);
+std::vector<uint32_t> parse_number_list(const char *str, uint8_t radix);
 
-string remove_non_number( string str );
+std::string remove_non_number( std::string str );
 
-uint16_t get_checksum(const string& to_check);
+uint16_t get_checksum(const std::string& to_check);
 uint16_t get_checksum(const char* to_check);
 
-void get_checksums(uint16_t check_sums[], const string& key);
+void get_checksums(uint16_t check_sums[], const std::string& key);
 
-string shift_parameter( string &parameters );
+std::string shift_parameter( std::string &parameters );
 
-string get_arguments( const string& possible_command );
+std::string get_arguments( const std::string& possible_command );
 
-bool file_exists( const string file_name );
+bool file_exists( const std::string file_name );
 
 void system_reset( bool dfu= false );
 
-string absolute_from_relative( string path );
+std::string absolute_from_relative( std::string path );
 
 int append_parameters(char *buf, std::vector<std::pair<char,float>> params, size_t bufsize);
-string wcs2gcode(int wcs);
+std::string wcs2gcode(int wcs);
 void safe_delay(uint32_t delay);
 
+#define confine(value, min, max) (((value) < (min))?(min):(((value) > (max))?(max):(value)))
+
+//#define dd(...) LPC_GPIO2->FIODIR = 0xffff; LPC_GPIO2->FIOCLR = 0xffff; LPC_GPIO2->FIOSET = __VA_ARGS__
+
+
 #endif

src/modules/communication/utils/Gcode.cpp

@@ -23,7 +23,6 @@ Gcode::Gcode(const string &command, StreamOutput *stream, bool strip)
     this->add_nl= false;
     this->is_error= false;
     this->stream= stream;
-    this->millimeters_of_travel = 0.0F;
     prepare_cached_values(strip);
     this->stripped= strip;
 }
@@ -39,7 +38,6 @@ Gcode::~Gcode()
 Gcode::Gcode(const Gcode &to_copy)
 {
     this->command               = strdup(to_copy.command); // TODO we can reference count this so we share copies, may save more ram than the extra count we need to store
-    this->millimeters_of_travel = to_copy.millimeters_of_travel;
     this->has_m                 = to_copy.has_m;
     this->has_g                 = to_copy.has_g;
     this->m                     = to_copy.m;
@@ -55,7 +53,6 @@ Gcode &Gcode::operator= (const Gcode &to_copy)
 {
     if( this != &to_copy ) {
         this->command               = strdup(to_copy.command); // TODO we can reference count this so we share copies, may save more ram than the extra count we need to store
-        this->millimeters_of_travel = to_copy.millimeters_of_travel;
         this->has_m                 = to_copy.has_m;
         this->has_g                 = to_copy.has_g;
         this->m                     = to_copy.m;

src/modules/communication/utils/Gcode.h

@@ -36,7 +36,6 @@ class Gcode {
         // FIXME these should be private
         unsigned int m;
         unsigned int g;
-        float millimeters_of_travel;
 
         struct {
             bool add_nl:1;

src/modules/robot/ActuatorCoordinates.h

@@ -5,17 +5,19 @@
       You should have received a copy of the GNU General Public License along with Smoothie. If not, see <http://www.gnu.org/licenses/>.
 */
 
-#ifndef ACTUATOR_COORDINATES_H
-#define ACTUATOR_COORDINATES_H
+#pragma once
+
 #include <array>
 
 #ifndef MAX_ROBOT_ACTUATORS
-#define MAX_ROBOT_ACTUATORS 3
+    #ifdef CNC
+    #define MAX_ROBOT_ACTUATORS 3
+    #else
+    // includes 2 extruders
+    #define MAX_ROBOT_ACTUATORS 5
+    #endif
 #endif
 
-//The subset in use is determined by the arm solution's get_actuator_count().
-//Keep MAX_ROBOT_ACTUATORS as small as practical it impacts block size and therefore free memory.
+// Keep MAX_ROBOT_ACTUATORS as small as practical it impacts block size and therefore free memory.
 const size_t k_max_actuators = MAX_ROBOT_ACTUATORS;
 typedef struct std::array<float, k_max_actuators> ActuatorCoordinates;
-
-#endif

src/modules/robot/Planner.cpp

@@ -25,8 +25,6 @@ using namespace std;
 
 #include <math.h>
 
-#define acceleration_checksum          CHECKSUM("acceleration")
-#define z_acceleration_checksum        CHECKSUM("z_acceleration")
 #define junction_deviation_checksum    CHECKSUM("junction_deviation")
 #define z_junction_deviation_checksum  CHECKSUM("z_junction_deviation")
 #define minimum_planner_speed_checksum CHECKSUM("minimum_planner_speed")
@@ -37,33 +35,30 @@ using namespace std;
 
 Planner::Planner()
 {
-    clear_vector_float(this->previous_unit_vec);
+    memset(this->previous_unit_vec, 0, sizeof this->previous_unit_vec);
     config_load();
 }
 
 // Configure acceleration
 void Planner::config_load()
 {
-    this->acceleration = THEKERNEL->config->value(acceleration_checksum)->by_default(100.0F )->as_number(); // Acceleration is in mm/s^2
-    this->z_acceleration = THEKERNEL->config->value(z_acceleration_checksum)->by_default(0.0F )->as_number(); // disabled by default
-
     this->junction_deviation = THEKERNEL->config->value(junction_deviation_checksum)->by_default(0.05F)->as_number();
-    this->z_junction_deviation = THEKERNEL->config->value(z_junction_deviation_checksum)->by_default(-1)->as_number(); // disabled by default
+    this->z_junction_deviation = THEKERNEL->config->value(z_junction_deviation_checksum)->by_default(NAN)->as_number(); // disabled by default
     this->minimum_planner_speed = THEKERNEL->config->value(minimum_planner_speed_checksum)->by_default(0.0f)->as_number();
 }
 
 
 // Append a block to the queue, compute it's speed factors
-void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s, float distance, float *unit_vec)
+void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s, float distance, float *unit_vec, float acceleration)
 {
-    float acceleration, junction_deviation;
+    float junction_deviation;
 
     // Create ( recycle ) a new block
     Block* block = THEKERNEL->conveyor->queue.head_ref();
 
 
     // Direction bits
-    for (size_t i = 0; i < THEROBOT->actuators.size(); i++) {
+    for (size_t i = 0; i < THEROBOT->n_motors; i++) {
         int steps = THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]);
 
         block->direction_bits[i] = (steps < 0) ? 1 : 0;
@@ -75,26 +70,19 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s,
         block->steps[i] = labs(steps);
     }
 
-    acceleration = this->acceleration;
     junction_deviation = this->junction_deviation;
 
-    // use either regular acceleration or a z only move accleration
+    // use either regular junction deviation or z specific
     if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0) {
         // z only move
-        if(this->z_acceleration > 0.0F) acceleration = this->z_acceleration;
-        if(this->z_junction_deviation >= 0.0F) junction_deviation = this->z_junction_deviation;
+        if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation;
     }
 
     block->acceleration = acceleration; // save in block
 
-    // if it is a SOLO move from extruder, zprobe or endstops we do not use junction deviation
-    if(unit_vec == nullptr) {
-        junction_deviation= 0.0F;
-    }
-
     // Max number of steps, for all axes
     uint32_t steps_event_count = 0;
-    for (size_t s = 0; s < THEROBOT->actuators.size(); s++) {
+    for (size_t s = 0; s < THEROBOT->n_motors; s++) {
         steps_event_count = std::max(steps_event_count, block->steps[s]);
     }
     block->steps_event_count = steps_event_count;
@@ -129,10 +117,11 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s,
     // and this allows one to stop with little to no decleration in many cases. This is particualrly bad on leadscrew based systems that will skip steps.
     float vmax_junction = minimum_planner_speed; // Set default max junction speed
 
-    if (!THEKERNEL->conveyor->is_queue_empty()) {
+    // if unit_vec was null then it was not a primary axis move so we skip the junction deviation stuff
+    if (unit_vec != nullptr && !THEKERNEL->conveyor->is_queue_empty()) {
         float previous_nominal_speed = THEKERNEL->conveyor->queue.item_ref(THEKERNEL->conveyor->queue.prev(THEKERNEL->conveyor->queue.head_i))->nominal_speed;
 
-        if (previous_nominal_speed > 0.0F && junction_deviation > 0.0F) {
+        if (junction_deviation > 0.0F && previous_nominal_speed > 0.0F) {
             // Compute cosine of angle between previous and current path. (prev_unit_vec is negative)
             // NOTE: Max junction velocity is computed without sin() or acos() by trig half angle identity.
             float cos_theta = - this->previous_unit_vec[X_AXIS] * unit_vec[X_AXIS]
@@ -141,12 +130,12 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s,
 
             // Skip and use default max junction speed for 0 degree acute junction.
             if (cos_theta < 0.95F) {
-                vmax_junction = min(previous_nominal_speed, block->nominal_speed);
+                vmax_junction = std::min(previous_nominal_speed, block->nominal_speed);
                 // Skip and avoid divide by zero for straight junctions at 180 degrees. Limit to min() of nominal speeds.
                 if (cos_theta > -0.95F) {
                     // Compute maximum junction velocity based on maximum acceleration and junction deviation
                     float sin_theta_d2 = sqrtf(0.5F * (1.0F - cos_theta)); // Trig half angle identity. Always positive.
-                    vmax_junction = min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
+                    vmax_junction = std::min(vmax_junction, sqrtf(acceleration * junction_deviation * sin_theta_d2 / (1.0F - sin_theta_d2)));
                 }
             }
         }
@@ -155,7 +144,7 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s,
 
     // Initialize block entry speed. Compute based on deceleration to user-defined minimum_planner_speed.
     float v_allowable = max_allowable_speed(-acceleration, minimum_planner_speed, block->millimeters);
-    block->entry_speed = min(vmax_junction, v_allowable);
+    block->entry_speed = std::min(vmax_junction, v_allowable);
 
     // Initialize planner efficiency flags
     // Set flag if block will always reach maximum junction speed regardless of entry/exit speeds.
@@ -175,7 +164,7 @@ void Planner::append_block( ActuatorCoordinates &actuator_pos, float rate_mm_s,
     if(unit_vec != nullptr) {
         memcpy(this->previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
     }else{
-        clear_vector_float(this->previous_unit_vec);
+        memset(this->previous_unit_vec, 0, sizeof this->previous_unit_vec);
     }
 
     // Math-heavy re-computing of the whole queue to take the new

src/modules/robot/Planner.h

@@ -16,18 +16,14 @@ class Planner
 public:
     Planner();
     float max_allowable_speed( float acceleration, float target_velocity, float distance);
-    float get_acceleration() const { return acceleration; }
-    float get_z_acceleration() const { return z_acceleration > 0.0F ? z_acceleration : acceleration; }
 
     friend class Robot; // for acceleration, junction deviation, minimum_planner_speed
 
 private:
-    void append_block(ActuatorCoordinates &target, float rate_mm_s, float distance, float unit_vec[] );
+    void append_block(ActuatorCoordinates &target, float rate_mm_s, float distance, float unit_vec[], float accleration);
     void recalculate();
     void config_load();
     float previous_unit_vec[3];
-    float acceleration;          // Setting
-    float z_acceleration;        // Setting
     float junction_deviation;    // Setting
     float z_junction_deviation;  // Setting
     float minimum_planner_speed; // Setting