set is_g123 and S value in block

diff --git a/src/modules/robot/Planner.cpp b/src/modules/robot/Planner.cpp
index c4711fa..35f745d 100644 (file)
--- a/src/modules/robot/Planner.cpp
+++ b/src/modules/robot/Planner.cpp
@@ -50,19 +50,19 @@ void Planner::config_load()
 
 
 // Append a block to the queue, compute it's speed factors
-bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration)
+bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors, float rate_mm_s, float distance, float *unit_vec, float acceleration, float s_value, bool g123)
 {
     // Create ( recycle ) a new block
     Block* block = THECONVEYOR->queue.head_ref();
 
     // Direction bits
-    bool has_steps= false;
+    bool has_steps = false;
     for (size_t i = 0; i < n_motors; i++) {
         int32_t steps = THEROBOT->actuators[i]->steps_to_target(actuator_pos[i]);
         // Update current position
         if(steps != 0) {
             THEROBOT->actuators[i]->update_last_milestones(actuator_pos[i], steps);
-            has_steps= true;
+            has_steps = true;
         }
 
         // find direction
@@ -71,28 +71,35 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
         block->steps[i] = labs(steps);
     }
 
-    // sometimres even though there is a detectable movement it turns out there are no steps to be had from such a small move
-    if(!has_steps) return false;
+    // sometimes even though there is a detectable movement it turns out there are no steps to be had from such a small move
+    if(!has_steps) {
+        block->clear();
+        return false;
+    }
+
+    // info needed by laser
+    block->s_value= s_value;
+    block->is_g123= g123;
 
     // use default JD
     float junction_deviation = this->junction_deviation;
 
     // use either regular junction deviation or z specific and see if a primary axis move
-    block->primary_axis= true;
-    if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0){
+    block->primary_axis = true;
+    if(block->steps[ALPHA_STEPPER] == 0 && block->steps[BETA_STEPPER] == 0) {
         if(block->steps[GAMMA_STEPPER] != 0) {
             // z only move
             if(!isnan(this->z_junction_deviation)) junction_deviation = this->z_junction_deviation;
-        }else{
+        } else {
             // is not a primary axis move
-            block->primary_axis= false;
+            block->primary_axis = false;
         }
     }
 
     block->acceleration = acceleration; // save in block
 
     // Max number of steps, for all axes
-    auto mi= std::max_element(block->steps.begin(), block->steps.end());
+    auto mi = std::max_element(block->steps.begin(), block->steps.end());
     block->steps_event_count = *mi;
 
     block->millimeters = distance;
@@ -127,7 +134,7 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
 
     // if unit_vec was null then it was not a primary axis move so we skip the junction deviation stuff
     if (unit_vec != nullptr && !THECONVEYOR->is_queue_empty()) {
-        Block *prev_block= THECONVEYOR->queue.item_ref(THECONVEYOR->queue.prev(THECONVEYOR->queue.head_i));
+        Block *prev_block = THECONVEYOR->queue.item_ref(THECONVEYOR->queue.prev(THECONVEYOR->queue.head_i));
         float previous_nominal_speed = prev_block->primary_axis ? prev_block->nominal_speed : 0;
 
         if (junction_deviation > 0.0F && previous_nominal_speed > 0.0F) {
@@ -172,7 +179,7 @@ bool Planner::append_block( ActuatorCoordinates &actuator_pos, uint8_t n_motors,
     // Update previous path unit_vector and nominal speed
     if(unit_vec != nullptr) {
         memcpy(previous_unit_vec, unit_vec, sizeof(previous_unit_vec)); // previous_unit_vec[] = unit_vec[]
-    }else{
+    } else {
         memset(previous_unit_vec, 0, sizeof(previous_unit_vec));
     }
 

diff --git a/src/modules/robot/Planner.h b/src/modules/robot/Planner.h
index ee43057..2d10784 100644 (file)
--- a/src/modules/robot/Planner.h
+++ b/src/modules/robot/Planner.h
@@ -20,7 +20,7 @@ public:
     friend class Robot; // for acceleration, junction deviation, minimum_planner_speed
 
 private:
-    bool append_block(ActuatorCoordinates &target, uint8_t n_motors, float rate_mm_s, float distance, float unit_vec[], float accleration);
+    bool append_block(ActuatorCoordinates &target, uint8_t n_motors, float rate_mm_s, float distance, float unit_vec[], float accleration, float s_value, bool g123);
     void recalculate();
     void config_load();
     float previous_unit_vec[3];

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 4098435..763008d 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -109,6 +109,7 @@ Robot::Robot()
     this->next_command_is_MCS = false;
     this->disable_segmentation= false;
     this->n_motors= 0;
+    this->s_value= 0;
 }
 
 //Called when the module has just been loaded
@@ -719,7 +720,11 @@ void Robot::on_gcode_received(void *argument)
     }
 
     if( motion_mode != NONE) {
+        is_g123= motion_mode != SEEK;
         process_move(gcode, motion_mode);
+
+    }else{
+        is_g123= false;
     }
 
     next_command_is_MCS = false; // must be on same line as G0 or G1
@@ -812,6 +817,9 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode)
             this->feed_rate = this->to_millimeters( gcode->get_value('F') );
     }
 
+    // S is modal
+    if(gcode->has_letter('S')) s_value= gcode->get_value('S');
+
     bool moved= false;
 
     // Perform any physical actions
@@ -1023,7 +1031,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s)
 
     // Append the block to the planner
     // NOTE that distance here should be either the distance travelled by the XYZ axis, or the E mm travel if a solo E move
-    if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration )) {
+    if(THEKERNEL->planner->append_block( actuator_pos, n_motors, rate_mm_s, distance, auxilliary_move ? nullptr : unit_vec, acceleration, s_value, is_g123)) {
         // this is the machine position
         memcpy(this->last_machine_position, transformed_target, n_motors*sizeof(float));
         return true;

diff --git a/src/modules/robot/Robot.h b/src/modules/robot/Robot.h
index c8df702..f22d9d3 100644 (file)
--- a/src/modules/robot/Robot.h
+++ b/src/modules/robot/Robot.h
@@ -82,6 +82,7 @@ class Robot : public Module {
             bool disable_segmentation:1;                      // set to disable segmentation
             bool segment_z_moves:1;
             bool save_g92:1;                                  // save g92 on M500 if set
+            bool is_g123:1;
             uint8_t plane_axis_0:2;                           // Current plane ( XY, XZ, YZ )
             uint8_t plane_axis_1:2;
             uint8_t plane_axis_2:2;
@@ -128,6 +129,7 @@ class Robot : public Module {
         float delta_segments_per_second;                     // Setting : Used to split lines into segments for delta based on speed
         float seconds_per_minute;                            // for realtime speed change
         float default_acceleration;                          // the defualt accleration if not set for each axis
+        float s_value;                                       // modal S value
 
         // Number of arc generation iterations by small angle approximation before exact arc trajectory
         // correction. This parameter may be decreased if there are issues with the accuracy of the arc