add ability to see exactly where actuator currently is

add ABC to M114

diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp
index 34834f8..b49a493 100644 (file)
--- a/src/libs/StepperMotor.cpp
+++ b/src/libs/StepperMotor.cpp
@@ -43,6 +43,7 @@ void StepperMotor::init()
     steps_per_mm         = 1.0F;
     max_rate             = 50.0F;
 
+    current_position_steps= 0;
     last_milestone_steps = 0;
     last_milestone_mm    = 0.0F;
 }
@@ -67,6 +68,9 @@ void StepperMotor::step()
         this->step_signal_hook->call();
     }
 
+    // keep track of actuators actual position in steps
+    this->current_position_steps += (this->direction ? -1 : 1);
+
     // Is this move finished ?
     if( this->stepped == this->steps_to_move ) {
         // Mark it as finished, then StepTicker will call signal_mode_finished()
@@ -80,7 +84,6 @@ void StepperMotor::step()
 // If the move is finished, the StepTicker will call this ( because we asked it to in tick() )
 void StepperMotor::signal_move_finished()
 {
-
     // work is done ! 8t
     this->moving = false;
     this->steps_to_move = 0;
@@ -177,12 +180,14 @@ void StepperMotor::change_steps_per_mm(float new_steps)
 {
     steps_per_mm = new_steps;
     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+    current_position_steps = last_milestone_steps;
 }
 
 void StepperMotor::change_last_milestone(float new_milestone)
 {
     last_milestone_mm = new_milestone;
     last_milestone_steps = lround(last_milestone_mm * steps_per_mm);
+    current_position_steps = last_milestone_steps;
 }
 
 int  StepperMotor::steps_to_target(float target)

diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h
index dfc92a8..a06b744 100644 (file)
--- a/src/libs/StepperMotor.h
+++ b/src/libs/StepperMotor.h
@@ -40,6 +40,7 @@ class StepperMotor {
         void change_steps_per_mm(float);
         void change_last_milestone(float);
         float get_last_milestone(void) const { return last_milestone_mm; }
+        float get_current_position(void) const { return (float)current_position_steps/steps_per_mm; }
 
         int  steps_to_target(float);
         uint32_t get_steps_to_move() const { return steps_to_move; }
@@ -78,6 +79,7 @@ class StepperMotor {
         float steps_per_mm;
         float max_rate;
 
+        volatile int32_t current_position_steps;
         int32_t last_milestone_steps;
         float   last_milestone_mm;
 

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 090c8c2..de63243 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -379,11 +379,14 @@ void Robot::on_gcode_received(void *argument)
                 check_max_actuator_speeds();
                 return;
             case 114: {
-                char buf[32];
-                int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f",
+                char buf[64];
+                int n = snprintf(buf, sizeof(buf), "C: X:%1.3f Y:%1.3f Z:%1.3f A:%1.3f B:%1.3f C:%1.3f ",
                                  from_millimeters(this->last_milestone[0]),
                                  from_millimeters(this->last_milestone[1]),
-                                 from_millimeters(this->last_milestone[2]));
+                                 from_millimeters(this->last_milestone[2]),
+                                 actuators[X_AXIS]->get_current_position(),
+                                 actuators[Y_AXIS]->get_current_position(),
+                                 actuators[Z_AXIS]->get_current_position() );
                 gcode->txt_after_ok.append(buf, n);
                 gcode->mark_as_taken();
             }
@@ -602,12 +605,10 @@ void Robot::reset_axis_position(float position, int axis)
 }
 
 // Use FK to find out where actuator is and reset lastmilestone to match
-// FIXME we need to know where the actual current actuator position is, this does not currently do that and so is useless
 void Robot::reset_position_from_current_actuator_position()
 {
-    // FIXME do not want last_milestone we need actual actuator position
-    // float actuator_pos[]= {actuators[X_AXIS]->get_current_position_mm(), actuators[Y_AXIS]->get_current_position_mm(), actuators[Z_AXIS]->get_current_position_mm()};
-    // arm_solution->actuator_to_cartesian(actuator_pos, this->last_milestone);
+    float actuator_pos[]= {actuators[X_AXIS]->get_current_position(), actuators[Y_AXIS]->get_current_position(), actuators[Z_AXIS]->get_current_position()};
+    arm_solution->actuator_to_cartesian(actuator_pos, this->last_milestone);
 }
 
 // Convert target from millimeters to steps, and append this to the planner