fix misunderstanding for return value of snprintf, where it matters.

[clinton/Smoothieware.git] / src / modules / robot / Robot.cpp

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index d39d73c..8165323 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -324,14 +324,14 @@ void Robot::get_current_machine_position(float *pos) const
     arm_solution->actuator_to_cartesian(current_position, pos);
 }
 
-void Robot::print_position(uint8_t subcode, std::string& res) const
+void Robot::print_position(uint8_t subcode, std::string& res, bool ignore_extruders) const
 {
     // M114.1 is a new way to do this (similar to how GRBL does it).
     // it returns the realtime position based on the current step position of the actuators.
     // this does require a FK to get a machine position from the actuator position
     // and then invert all the transforms to get a workspace position from machine position
     // M114 just does it the old way uses machine_position and does inverse transforms to get the requested position
-    int n = 0;
+    uint32_t n = 0;
     char buf[64];
     if(subcode == 0) { // M114 print WCS
         wcs_t pos= mcs2wcs(machine_position);
@@ -372,13 +372,14 @@ void Robot::print_position(uint8_t subcode, std::string& res) const
         }
     }
 
+    if(n > sizeof(buf)) n= sizeof(buf);
     res.append(buf, n);
 
     #if MAX_ROBOT_ACTUATORS > 3
     // deal with the ABC axis
     for (int i = A_AXIS; i < n_motors; ++i) {
         n= 0;
-        if(actuators[i]->is_extruder()) continue; // don't show an extruder as that will be E
+        if(ignore_extruders && actuators[i]->is_extruder()) continue; // don't show an extruder as that will be E
         if(subcode == 4) { // M114.4 print last milestone
             n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, machine_position[i]);
 
@@ -386,6 +387,7 @@ void Robot::print_position(uint8_t subcode, std::string& res) const
             // current actuator position
             n= snprintf(buf, sizeof(buf), " %c:%1.4f", 'A'+i-A_AXIS, actuators[i]->get_current_position());
         }
+        if(n > sizeof(buf)) n= sizeof(buf);
         if(n > 0) res.append(buf, n);
     }
     #endif
@@ -630,7 +632,7 @@ void Robot::on_gcode_received(void *argument)
 
             case 114:{
                 std::string buf;
-                print_position(gcode->subcode, buf);
+                print_position(gcode->subcode, buf, true); // ignore extruders as they will print E themselves
                 gcode->txt_after_ok.append(buf);
                 return;
             }
@@ -926,7 +928,7 @@ void Robot::process_move(Gcode *gcode, enum MOTION_MODE_T motion_mode)
         }
 
     }else{
-        // already in machine coordinates, we do not add tool offset for that
+        // already in machine coordinates, we do not add wcs or tool offset for that
         for(int i= X_AXIS; i <= Z_AXIS; ++i) {
             if(!isnan(param[i])) target[i] = param[i];
         }
@@ -1152,7 +1154,7 @@ bool Robot::append_milestone(const float target[], float rate_mm_s)
             unit_vec[i] = deltas[i] / distance;
 
             // Do not move faster than the configured cartesian limits for XYZ
-            if ( max_speeds[i] > 0 ) {
+            if ( i <= Z_AXIS && max_speeds[i] > 0 ) {
                 float axis_speed = fabsf(unit_vec[i] * rate_mm_s);
 
                 if (axis_speed > max_speeds[i])
@@ -1225,8 +1227,16 @@ bool Robot::append_milestone(const float target[], float rate_mm_s)
         }
     }
 
+    // if we are in feed hold wait here until it is released, this means that even segemnted lines will pause
+    while(THEKERNEL->get_feed_hold()) {
+        THEKERNEL->call_event(ON_IDLE, this);
+        // if we also got a HALT then break out of this
+        if(THEKERNEL->is_halted()) return false;
+    }
+
     // 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
+    // NOTE this call will bock until there is room in the block queue, on_idle will continue to be called
     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 new compensated machine position
         memcpy(this->compensated_machine_position, transformed_target, n_motors*sizeof(float));
@@ -1336,10 +1346,11 @@ bool Robot::append_line(Gcode *gcode, const float target[], float rate_mm_s, flo
         // We always add another point after this loop so we stop at segments-1, ie i < segments
         for (int i = 1; i < segments; i++) {
             if(THEKERNEL->is_halted()) return false; // don't queue any more segments
-            for (int i = 0; i < n_motors; i++)
-                segment_end[i] += segment_delta[i];
+            for (int j = 0; j < n_motors; j++)
+                segment_end[j] += segment_delta[j];
 
             // Append the end of this segment to the queue
+            // this can block waiting for free block queue or if in feed hold
             bool b= this->append_milestone(segment_end, rate_mm_s);
             moved= moved || b;
         }
@@ -1378,6 +1389,7 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[]
     // Patch from GRBL Firmware - Christoph Baumann 04072015
     // CCW angle between position and target from circle center. Only one atan2() trig computation required.
     float angular_travel = atan2f(r_axis0 * rt_axis1 - r_axis1 * rt_axis0, r_axis0 * rt_axis0 + r_axis1 * rt_axis1);
+    if (plane_axis_2 == Y_AXIS) { is_clockwise = !is_clockwise; }  //Math for XZ plane is revere of other 2 planes
     if (is_clockwise) { // Correct atan2 output per direction
         if (angular_travel >= -ARC_ANGULAR_TRAVEL_EPSILON) { angular_travel -= (2 * PI); }
     } else {
@@ -1436,13 +1448,16 @@ bool Robot::append_arc(Gcode * gcode, const float target[], const float offset[]
     float sin_T = theta_per_segment;
 
     // TODO we need to handle the ABC axis here by segmenting them
-    float arc_target[3];
+    float arc_target[n_motors];
     float sin_Ti;
     float cos_Ti;
     float r_axisi;
     uint16_t i;
     int8_t count = 0;
 
+    // init array for all axis
+    memcpy(arc_target, machine_position, n_motors*sizeof(float));
+
     // Initialize the linear axis
     arc_target[this->plane_axis_2] = this->machine_position[this->plane_axis_2];