Merge branch 'devel'

Arduino/ODriveArduino/ODriveArduino.cpp

@@ -50,6 +50,11 @@ float ODriveArduino::readFloat() {
     return readString().toFloat();
 }
 
+float ODriveArduino::GetVelocity(int motor_number){
+	serial_<< "r axis" << motor_number << ".encoder.vel_estimate\n";
+	return ODriveArduino::readFloat();
+}
+
 int32_t ODriveArduino::readInt() {
     return readString().toInt();
 }

Arduino/ODriveArduino/ODriveArduino.h

@@ -28,6 +28,8 @@ public:
     void SetVelocity(int motor_number, float velocity, float current_feedforward);
     void SetCurrent(int motor_number, float current);
     void TrapezoidalMove(int motor_number, float position);
+    // Getters
+    float GetVelocity(int motor_number);
     // General params
     float readFloat();
     int32_t readInt();

CHANGELOG.md

@@ -1,6 +1,15 @@
 # Unreleased Features
 Please add a note of your changes below this heading if you make a Pull Request.
 
+# Releases
+## [0.4.11] - 2019-07-25
+### Added
+* Separate lockin configs for sensorless, index search, and general.
+* Check current limit violation: added `ERROR_CURRENT_UNSTABLE`, `motor.config.current_lim_tolerance`.
+
+### Changed
+* Ascii command for reboot changed from `sb` to `sr`.
+
 # Releases
 ## [0.4.10] - 2019-04-24
 ### Fixed

Firmware/.clang-format

@@ -0,0 +1,7 @@
+---
+BasedOnStyle: Google
+AlignConsecutiveAssignments: 'true'
+AllowShortCaseLabelsOnASingleLine: 'true'
+IndentWidth: '4'
+
+...

Firmware/.vscode/launch.json

@@ -15,6 +15,7 @@
                 "interface/stlink-v2.cfg",
                 "target/stm32f4x_stlink.cfg",
             ],
+            "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd",
             "cwd": "${workspaceRoot}"
         },
         {

Firmware/.vscode/settings.json

@@ -1,5 +1,4 @@
 {
-    "C_Cpp.clang_format_style": "{ BasedOnStyle: Google, IndentWidth: 4, ColumnLimit: 0 }",
     "C_Cpp.intelliSenseEngine": "Default",
     "C_Cpp.intelliSenseEngineFallback": "Disabled",
     "files.exclude": {

Firmware/Board/v3/Drivers/CMSIS/Include/cmsis_gcc.h

@@ -161,7 +161,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
  */
 __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
 {
-  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
+  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : );
 }
 
 
@@ -187,7 +187,7 @@ __attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
  */
 __attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
 {
-  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
+  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : );
 }
 
 

Firmware/MotorControl/axis.cpp

@@ -6,14 +6,16 @@
 #include "utils.h"
 #include "odrive_main.h"
 
-Axis::Axis(const AxisHardwareConfig_t& hw_config,
+Axis::Axis(int axis_num,
+           const AxisHardwareConfig_t& hw_config,
            Config_t& config,
            Encoder& encoder,
            SensorlessEstimator& sensorless_estimator,
            Controller& controller,
            Motor& motor,
            TrapezoidalTrajectory& trap)
-    : hw_config_(hw_config),
+    : axis_num_(axis_num),
+      hw_config_(hw_config),
       config_(config),
       encoder_(encoder),
       sensorless_estimator_(sensorless_estimator),
@@ -31,6 +33,34 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
     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;
+}
+
 static void step_cb_wrapper(void* ctx) {
     reinterpret_cast<Axis*>(ctx)->step_cb();
 }
@@ -177,32 +207,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;
     };
@@ -210,11 +240,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
     });
@@ -225,12 +255,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();
         });
@@ -369,17 +399,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,6 +70,8 @@ public:
         uint16_t step_gpio_pin = 0;
         uint16_t dir_gpio_pin = 0;
 
+        LockinConfig_t calibration_lockin = default_calibration();
+        LockinConfig_t sensorless_ramp = default_sensorless();
         LockinConfig_t lockin;
     };
 
@@ -80,7 +86,8 @@ public:
         LOCKIN_STATE_CONST_VEL,
     };
 
-    Axis(const AxisHardwareConfig_t& hw_config,
+    Axis(int axis_num,
+            const AxisHardwareConfig_t& hw_config,
             Config_t& config,
             Encoder& encoder,
             SensorlessEstimator& sensorless_estimator,
@@ -176,13 +183,14 @@ 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();
 
     void run_state_machine_loop();
 
+    int axis_num_;
     const AxisHardwareConfig_t& hw_config_;
     Config_t& config_;
 
@@ -238,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

Firmware/MotorControl/main.cpp

@@ -169,7 +169,7 @@ int odrive_main(void) {
                                  hw_configs[i].gate_driver_config,
                                  motor_configs[i]);
         TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]);
-        axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
+        axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i],
                 *encoder, *sensorless_estimator, *controller, *motor, *trap);
     }
     

Firmware/MotorControl/motor.cpp

@@ -342,6 +342,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
     if (fabsf(current_meas_.phB) > ictrl.overcurrent_trip_level
      || fabsf(current_meas_.phC) > ictrl.overcurrent_trip_level) {
         set_error(ERROR_CURRENT_SENSE_SATURATION);
+        return false;
     }
 
     // Clarke transform
@@ -356,6 +357,13 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float I_phase, float pwm_pha
     ictrl.Iq_measured += ictrl.I_measured_report_filter_k * (Iq - ictrl.Iq_measured);
     ictrl.Id_measured += ictrl.I_measured_report_filter_k * (Id - ictrl.Id_measured);
 
+    // Check for violation of current limit
+    float I_trip = config_.current_lim_tolerance * effective_current_lim();
+    if (SQ(Id) + SQ(Iq) > SQ(I_trip)) {
+        set_error(ERROR_CURRENT_UNSTABLE);
+        return false;
+    }
+
     // Current error
     float Ierr_d = Id_des - Id;
     float Ierr_q = Iq_des - Iq;

Firmware/MotorControl/motor.hpp

@@ -22,7 +22,8 @@ public:
         ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100,
         ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200,
         ERROR_CURRENT_SENSE_SATURATION = 0x0400,
-        ERROR_INVERTER_OVER_TEMP = 0x0800
+        ERROR_INVERTER_OVER_TEMP = 0x0800,
+        ERROR_CURRENT_UNSTABLE = 0x1000
     };
 
     enum MotorType_t {
@@ -68,6 +69,7 @@ public:
         // Read out max_allowed_current to see max supported value for current_lim.
         // float current_lim = 70.0f; //[A]
         float current_lim = 10.0f;  //[A]
+        float current_lim_tolerance = 1.25f;  // multiple of current_lim
         // Value used to compute shunt amplifier gains
         float requested_current_range = 60.0f; // [A]
         float current_control_bandwidth = 1000.0f;  // [rad/s]
@@ -227,6 +229,7 @@ public:
                 make_protocol_property("direction", &config_.direction),
                 make_protocol_property("motor_type", &config_.motor_type),
                 make_protocol_property("current_lim", &config_.current_lim),
+                make_protocol_property("current_lim_tolerance", &config_.current_lim_tolerance),
                 make_protocol_property("inverter_temp_limit_lower", &config_.inverter_temp_limit_lower),
                 make_protocol_property("inverter_temp_limit_upper", &config_.inverter_temp_limit_upper),
                 make_protocol_property("requested_current_range", &config_.requested_current_range),

Firmware/communication/ascii_protocol.cpp

@@ -210,7 +210,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             save_configuration();
         } else if (cmd[1] == 'e'){ // Erase config
             erase_configuration();
-        } else if (cmd[1] == 'b'){ // Reboot
+        } else if (cmd[1] == 'r'){ // Reboot
             NVIC_SystemReset();
         }
 

Firmware/fibre/cpp/include/fibre/protocol.hpp

@@ -976,17 +976,17 @@ public:
         output_properties_.register_endpoints(list, id + 1 + decltype(input_properties_)::endpoint_count, length);
     }
 
-    template<typename> std::enable_if_t<sizeof...(TOutputs) == 0>
+    template<size_t i = sizeof...(TOutputs)> std::enable_if_t<i == 0>
     handle_ex() {
         invoke_function_with_tuple(*obj_, func_ptr_, in_args_);
     }
 
-    template<typename> std::enable_if_t<sizeof...(TOutputs) == 1>
+    template<size_t i = sizeof...(TOutputs)> std::enable_if_t<i == 1>
     handle_ex() {
         std::get<0>(out_args_) = invoke_function_with_tuple(*obj_, func_ptr_, in_args_);
     }
     
-    template<typename> std::enable_if_t<sizeof...(TOutputs) >= 2>
+    template<size_t i = sizeof...(TOutputs)> std::enable_if_t<i >= 2>
     handle_ex() {
         out_args_ = invoke_function_with_tuple(*obj_, func_ptr_, in_args_);
     }
@@ -997,7 +997,7 @@ public:
         (void) output;
         LOG_FIBRE("tuple still at %x and of size %u\r\n", (uintptr_t)&in_args_, sizeof(in_args_));
         LOG_FIBRE("invoke function using %d and %.3f\r\n", std::get<0>(in_args_), std::get<1>(in_args_));
-        handle_ex<void>();
+        handle_ex();
     }
 
     const char * name_;

docs/ascii-protocol.md

@@ -136,4 +136,4 @@ Not all parameters can be accessed via the ASCII protocol but at least all param
 #### System commands:
 * `ss` - Save config
 * `se` - Erase config
-* `sb` - Reboot
+* `sr` - Reboot

docs/encoders.md

@@ -40,9 +40,15 @@ Below are the steps to do the one-time calibration and configuration. Note that
 
 That's it, now on every reboot the motor will turn in one direction until it finds the encoder index.
 
-* If you wish to scan for the index pulse in the other direction, that feature is currently undocumented.
 * If your motor has problems reaching the index location due to the mechanical load, you can increase `<axis>.motor.config.calibration_current`.
 
+### 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 ensure the following three values are negative:
+* `<axis0>.config.calibration_lockin.vel`
+* `<axis0>.config.calibration_lockin.accel`
+* `<axis0>.config.calibration_lockin.ramp_distance`
+
+
 *IMPORTANT:* Your motor should find the same rotational position when the ODrive performs an index search if the index signal is working properly. This means that the motor should spin, and stop at the same position if you have set <axis>.config.startup_encoder_index_search so the search starts on reboot, or you if call the command:<axis>.requested_state = AXIS_STATE_ENCODER_INDEX_SEARCH after reboot. You can test this. Send the reboot() command, and while it's rebooting turn your motor, then make sure the motor returns back to the correct position each time when it comes out of reboot. Try this procedure a couple of times to be sure. 
 
 ### Startup sequence notes

tools/odrive/enums.py

@@ -42,6 +42,7 @@ class errors:
         ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100
         ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200
         ERROR_CURRENT_SENSE_SATURATION = 0x0400
+        ERROR_CURRENT_UNSTABLE = 0x1000
 
     class encoder:
         ERROR_NONE = 0