Add motor_id to stepper motors so they know what axis they are

Change MotorDriver Control so that the designator is actually the axis it is attached to.

diff --git a/src/libs/StepperMotor.cpp b/src/libs/StepperMotor.cpp
index 287d348..65b0248 100644 (file)
--- a/src/libs/StepperMotor.cpp
+++ b/src/libs/StepperMotor.cpp
@@ -57,7 +57,7 @@ void StepperMotor::on_enable(void *argument)
     if(bm == 0x01) {
         enable(true);
 
-    }else if(bm == 0 || ((bm&0x01) == 0 && ((bm&(0x02<<index)) != 0)) ) {
+    }else if(bm == 0 || ((bm&0x01) == 0 && ((bm&(0x02<<motor_id)) != 0)) ) {
         enable(false);
     }
 }

diff --git a/src/libs/StepperMotor.h b/src/libs/StepperMotor.h
index 17ec0eb..1673a36 100644 (file)
--- a/src/libs/StepperMotor.h
+++ b/src/libs/StepperMotor.h
@@ -15,6 +15,9 @@ class StepperMotor  : public Module {
         StepperMotor(Pin& step, Pin& dir, Pin& en);
         ~StepperMotor();
 
+        void set_motor_id(uint8_t id) { motor_id= id; }
+        uint8_t get_motor_id() const { return motor_id; }
+
         // called from step ticker ISR
         inline bool step() { step_pin.set(1); current_position_steps += (direction?-1:1); return moving; }
         // called from unstep ISR
@@ -56,8 +59,6 @@ class StepperMotor  : public Module {
         void on_halt(void *argument);
         void on_enable(void *argument);
 
-        int index;
-
         Pin step_pin;
         Pin dir_pin;
         Pin en_pin;
@@ -72,6 +73,7 @@ class StepperMotor  : public Module {
         float   last_milestone_mm;
 
         volatile struct {
+            uint8_t motor_id:8;
             volatile bool direction:1;
             volatile bool moving:1;
             bool selected:1;

diff --git a/src/modules/robot/Robot.cpp b/src/modules/robot/Robot.cpp
index 9d16186..b91c2c8 100644 (file)
--- a/src/modules/robot/Robot.cpp
+++ b/src/modules/robot/Robot.cpp
@@ -254,6 +254,7 @@ uint8_t Robot::register_motor(StepperMotor *motor)
         __debugbreak();
     }
     actuators.push_back(motor);
+    motor->set_motor_id(n_motors);
     return n_motors++;
 }
 

diff --git a/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp b/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp
index 62b1b47..f5611e8 100644 (file)
--- a/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp
+++ b/src/modules/utils/motordrivercontrol/MotorDriverControl.cpp
@@ -262,8 +262,8 @@ void MotorDriverControl::on_gcode_received(void *argument)
                 microsteps= set_microstep(microsteps); // driver may change the steps it sets to
                 if(gcode->subcode == 1 && current_microsteps != microsteps) {
                     // also reset the steps/mm
-                    int a= designator-'A';
-                    if(a >= 0 && a <=2) {
+                    uint32_t a= (designator >= 'X' || designator <= 'Z') ? designator-'X' : designator-'A'+3;
+                    if(a < THEROBOT->get_number_registered_motors()) {
                         float s= THEROBOT->actuators[a]->get_steps_per_mm()*((float)microsteps/current_microsteps);
                         THEROBOT->actuators[a]->change_steps_per_mm(s);
                         gcode->stream->printf("steps/mm for %c changed to: %f\n", designator, s);
@@ -282,9 +282,9 @@ void MotorDriverControl::on_gcode_received(void *argument)
         } else if(gcode->m == 911) {
             // set or get raw registers
             // M911 will dump all the registers and status of all the motors
-            // M911.1 Pn (or A0) will dump the registers and status of the selected motor. X0 will request format in processing machine readable format
-            // M911.2 Pn (or B0) Rxxx Vyyy sets Register xxx to value yyy for motor nnn, xxx == 255 writes the registers, xxx == 0 shows what registers are mapped to what
-            // M911.3 Pn (or C0) will set the options based on the parameters passed as below...
+            // M911.1 Pn (or X0) will dump the registers and status of the selected motor. R0 will request format in processing machine readable format
+            // M911.2 Pn (or Y0) Rxxx Vyyy sets Register xxx to value yyy for motor nnn, xxx == 255 writes the registers, xxx == 0 shows what registers are mapped to what
+            // M911.3 Pn (or Z0) will set the options based on the parameters passed as below...
             // TMC2660:-
             // M911.3 Onnn Qnnn setStallGuardThreshold O=stall_guard_threshold, Q=stall_guard_filter_enabled
             // M911.3 Hnnn Innn Jnnn Knnn Lnnn setCoolStepConfiguration H=lower_SG_threshold, I=SG_hysteresis, J=current_decrement_step_size, K=current_increment_step_size, L=lower_current_limit
@@ -302,7 +302,7 @@ void MotorDriverControl::on_gcode_received(void *argument)
 
             }else if(gcode->get_value('P') == id || gcode->has_letter(designator)) {
                 if(gcode->subcode == 1) {
-                    dump_status(gcode->stream, !gcode->has_letter('X'));
+                    dump_status(gcode->stream, !gcode->has_letter('R'));
 
                 }else if(gcode->subcode == 2 && gcode->has_letter('R') && gcode->has_letter('V')) {
                     set_raw_register(gcode->stream, gcode->get_value('R'), gcode->get_value('V'));