refactor lockin mode into separate variables for sensorless spinup, encoder calibration and general (user initiated) mode

Firmware/MotorControl/axis.cpp

@@ -33,6 +33,48 @@ Axis::Axis(int axis_num,
     update_watchdog_settings();
 }
 
+Axis::LockinConfig_t Axis::default_calibration() {
+    Axis::LockinConfig_t config;
+    config.current = 10.0f;           // [A]
+    config.ramp_time = 0.4f;          // [s]
+    config.ramp_distance = 1 * M_PI;  // [rad]
+    config.accel = 20.0f;     // [rad/s^2]
+    config.vel = 40.0f; // [rad/s]
+    config.finish_distance = 100.0f * 2.0f * M_PI;  // [rad]
+    config.finish_on_vel = false;
+    config.finish_on_distance = true;
+    config.finish_on_enc_idx = true;
+    return config;
+}
+
+Axis::LockinConfig_t Axis::default_sensorless() {
+    Axis::LockinConfig_t config;
+    config.current = 10.0f;           // [A]
+    config.ramp_time = 0.4f;          // [s]
+    config.ramp_distance = 1 * M_PI;  // [rad]
+    config.accel = 200.0f;     // [rad/s^2]
+    config.vel = 400.0f; // [rad/s]
+    config.finish_distance = 100.0f;  // [rad]
+    config.finish_on_vel = true;
+    config.finish_on_distance = false;
+    config.finish_on_enc_idx = false;
+    return config;
+}
+
+Axis::LockinConfig_t Axis::default_lockin() {
+    Axis::LockinConfig_t config;
+    config.current = 10.0f;           // [A]
+    config.ramp_time = 0.4f;          // [s]
+    config.ramp_distance = 1 * M_PI;  // [rad]
+    config.accel = 20.0f;     // [rad/s^2]
+    config.vel = 400.0f; // [rad/s]
+    config.finish_distance = 100.0f;  // [rad]
+    config.finish_on_vel = false;
+    config.finish_on_distance = false;
+    config.finish_on_enc_idx = false;
+    return config;
+}
+
 static void step_cb_wrapper(void* ctx) {
     reinterpret_cast<Axis*>(ctx)->step_cb();
 }
@@ -179,32 +221,32 @@ bool Axis::watchdog_check() {
     }
 }
 
-bool Axis::run_lockin_spin() {
+bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
     // Spiral up current for softer rotor lock-in
     lockin_state_ = LOCKIN_STATE_RAMP;
     float x = 0.0f;
     run_control_loop([&]() {
-        float phase = wrap_pm_pi(config_.lockin.ramp_distance * x);
-        float I_mag = config_.lockin.current * x;
-        x += current_meas_period / config_.lockin.ramp_time;
+        float phase = wrap_pm_pi(lockin_config.ramp_distance * x);
+        float I_mag = lockin_config.current * x;
+        x += current_meas_period / lockin_config.ramp_time;
         if (!motor_.update(I_mag, phase, 0.0f))
             return false;
         return x < 1.0f;
     });
     
     // Spin states
-    float distance = config_.lockin.ramp_distance;
+    float distance = lockin_config.ramp_distance;
     float phase = wrap_pm_pi(distance);
-    float vel = distance / config_.lockin.ramp_time;
+    float vel = distance / lockin_config.ramp_time;
 
     // Function of states to check if we are done
     auto spin_done = [&](bool vel_override = false) -> bool {
         bool done = false;
-        if (config_.lockin.finish_on_vel || vel_override)
-            done = done || fabsf(vel) >= fabsf(config_.lockin.vel);
-        if (config_.lockin.finish_on_distance)
-            done = done || fabsf(distance) >= fabsf(config_.lockin.finish_distance);
-        if (config_.lockin.finish_on_enc_idx)
+        if (lockin_config.finish_on_vel || vel_override)
+            done = done || fabsf(vel) >= fabsf(lockin_config.vel);
+        if (lockin_config.finish_on_distance)
+            done = done || fabsf(distance) >= fabsf(lockin_config.finish_distance);
+        if (lockin_config.finish_on_enc_idx)
             done = done || encoder_.index_found_;
         return done;
     };
@@ -212,11 +254,11 @@ bool Axis::run_lockin_spin() {
     // Accelerate
     lockin_state_ = LOCKIN_STATE_ACCELERATE;
     run_control_loop([&]() {
-        vel += config_.lockin.accel * current_meas_period;
+        vel += lockin_config.accel * current_meas_period;
         distance += vel * current_meas_period;
         phase = wrap_pm_pi(phase + vel * current_meas_period);
 
-        if (!motor_.update(config_.lockin.current, phase, vel))
+        if (!motor_.update(lockin_config.current, phase, vel))
             return false;
         return !spin_done(true); //vel_override to go to next phase
     });
@@ -227,12 +269,12 @@ bool Axis::run_lockin_spin() {
     // Constant speed
     if (!spin_done()) {
         lockin_state_ = LOCKIN_STATE_CONST_VEL;
-        vel = config_.lockin.vel; // reset to actual specified vel to avoid small integration error
+        vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
         run_control_loop([&]() {
             distance += vel * current_meas_period;
             phase = wrap_pm_pi(phase + vel * current_meas_period);
 
-            if (!motor_.update(config_.lockin.current, phase, vel))
+            if (!motor_.update(lockin_config.current, phase, vel))
                 return false;
             return !spin_done();
         });
@@ -371,17 +413,17 @@ void Axis::run_state_machine_loop() {
             case AXIS_STATE_LOCKIN_SPIN: {
                 if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                     goto invalid_state_label;
-                status = run_lockin_spin();
+                status = run_lockin_spin(config_.lockin);
             } break;
 
             case AXIS_STATE_SENSORLESS_CONTROL: {
                 if (!motor_.is_calibrated_ || motor_.config_.direction==0)
                         goto invalid_state_label;
-                status = run_lockin_spin(); // TODO: restart if desired
+                status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
                 if (status) {
                     // call to controller.reset() that happend when arming means that vel_setpoint
                     // is zeroed. So we make the setpoint the spinup target for smooth transition.
-                    controller_.vel_setpoint_ = config_.lockin.vel;
+                    controller_.vel_setpoint_ = config_.sensorless_ramp.vel;
                     status = run_sensorless_control_loop();
                 }
             } break;

Firmware/MotorControl/axis.hpp

@@ -49,6 +49,10 @@ public:
         bool finish_on_enc_idx = false;
     };
 
+    static LockinConfig_t default_calibration();
+    static LockinConfig_t default_sensorless();
+    static LockinConfig_t default_lockin();
+
     struct Config_t {
         bool startup_motor_calibration = false;   //<! run motor calibration at startup, skip otherwise
         bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
@@ -66,7 +70,9 @@ public:
         uint16_t step_gpio_pin = 0;
         uint16_t dir_gpio_pin = 0;
 
-        LockinConfig_t lockin;
+        LockinConfig_t calibration_lockin = default_calibration();
+        LockinConfig_t lockin = default_lockin();
+        LockinConfig_t sensorless_ramp = default_sensorless();
     };
 
     enum thread_signals {
@@ -177,7 +183,7 @@ public:
         }
     }
 
-    bool run_lockin_spin();
+    bool run_lockin_spin(const LockinConfig_t &lockin_config);
     bool run_sensorless_control_loop();
     bool run_closed_loop_control_loop();
     bool run_idle_loop();
@@ -240,7 +246,25 @@ public:
                     [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
                 make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin,
                     [](void* ctx) { static_cast<Axis*>(ctx)->decode_step_dir_pins(); }, this),
-                make_protocol_object("lockin",
+                make_protocol_object("calibration_lockin",
+                    make_protocol_property("current", &config_.calibration_lockin.current),
+                    make_protocol_property("ramp_time", &config_.calibration_lockin.ramp_time),
+                    make_protocol_property("ramp_distance", &config_.calibration_lockin.ramp_distance),
+                    make_protocol_property("accel", &config_.calibration_lockin.accel),
+                    make_protocol_property("vel", &config_.calibration_lockin.vel)
+                ),
+                make_protocol_object("sensorless_ramp",
+                    make_protocol_property("current", &config_.sensorless_ramp.current),
+                    make_protocol_property("ramp_time", &config_.sensorless_ramp.ramp_time),
+                    make_protocol_property("ramp_distance", &config_.sensorless_ramp.ramp_distance),
+                    make_protocol_property("accel", &config_.sensorless_ramp.accel),
+                    make_protocol_property("vel", &config_.sensorless_ramp.vel),
+                    make_protocol_property("finish_distance", &config_.sensorless_ramp.finish_distance),
+                    make_protocol_property("finish_on_vel", &config_.sensorless_ramp.finish_on_vel),
+                    make_protocol_property("finish_on_distance", &config_.sensorless_ramp.finish_on_distance),
+                    make_protocol_property("finish_on_enc_idx", &config_.sensorless_ramp.finish_on_enc_idx)
+                ),
+                make_protocol_object("general_lockin",
                     make_protocol_property("current", &config_.lockin.current),
                     make_protocol_property("ramp_time", &config_.lockin.ramp_time),
                     make_protocol_property("ramp_distance", &config_.lockin.ramp_distance),

Firmware/MotorControl/encoder.cpp

@@ -93,6 +93,8 @@ void Encoder::set_linear_count(int32_t count) {
     // Update states
     shadow_count_ = count;
     pos_estimate_ = (float)count;
+    tim_cnt_sample_ = count;
+
     //Write hardware last
     hw_config_.timer->Instance->CNT = count;
 
@@ -125,20 +127,17 @@ bool Encoder::run_index_search() {
     }
     set_idx_subscribe();
 
-    bool orig_finish_on_enc_idx = axis_->config_.lockin.finish_on_enc_idx;
-    axis_->config_.lockin.finish_on_enc_idx = true;
-    bool status = axis_->run_lockin_spin();
-    axis_->config_.lockin.finish_on_enc_idx = orig_finish_on_enc_idx;
+    bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
     return status;
 }
 
 bool Encoder::run_direction_find() {
     int32_t init_enc_val = shadow_count_;
-    bool orig_finish_on_distance = axis_->config_.lockin.finish_on_distance;
-    axis_->config_.lockin.finish_on_distance = true;
+    bool orig_finish_on_distance = axis_->config_.calibration_lockin.finish_on_distance;
+    axis_->config_.calibration_lockin.finish_on_distance = true;
     axis_->motor_.config_.direction = 1; // Must test spin forwards for direction detect logic
-    bool status = axis_->run_lockin_spin();
-    axis_->config_.lockin.finish_on_distance = orig_finish_on_distance;
+    bool status = axis_->run_lockin_spin(axis_->config_.calibration_lockin);
+    axis_->config_.calibration_lockin.finish_on_distance = orig_finish_on_distance;
 
     if (status) {
         // Check response and direction

docs/commands.md

@@ -121,6 +121,7 @@ odrv0.axis0.controller.config.vel_gain = 0.01
 odrv0.axis0.controller.config.vel_integrator_gain = 0.05
 odrv0.axis0.controller.config.control_mode = 2
 odrv0.axis0.controller.vel_setpoint = 400
+odrv0.axis0.motor.config.direction = 1
 odrv0.axis0.sensorless_estimator.config.pm_flux_linkage = 5.51328895422 / (<pole pairs> * <motor kv>)
 ```
 

docs/encoders.md

@@ -86,6 +86,12 @@ and now see if after a reboot you can run:
 
 without getting errors. 
 
+### Reversing index search
+Sometimes you would like the index search to only happen in a particular direction (the reverse of the default), instead of swapping the motor leads, you can run the following commands
+* `<axis0>.config.calibration_lockin.vel = -<axis>.config.calibration_lockin.vel`
+* `<axis0>.config.calibration_lockin.accel = -<axis>.config.calibration_lockin.accel`
+* `<axis0>.config.calibration_lockin.ramp_distance = -<axis>.config.calibration_lockin.ramp_distance`
+
 ## What happens if calibration fails
 There are subtle ways that encoder problems will impact your ODrive. For example, ODrive may not complete the calibrate sequence when you go to:
 * `<axis>.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE`