Merge branch 'devel' into feature/CAN

Arduino/ODriveArduino/ODriveArduino.cpp

@@ -42,6 +42,10 @@ void ODriveArduino::SetCurrent(int motor_number, float current) {
     serial_ << "c " << motor_number << " " << current << "\n";
 }
 
+void ODriveArduino::TrapezoidalMove(int motor_number, float position){
+    serial_ << "t " << motor_number << " " << position << "\n";
+}
+
 float ODriveArduino::readFloat() {
     return readString().toFloat();
 }

Arduino/ODriveArduino/ODriveArduino.h

@@ -27,7 +27,7 @@ public:
     void SetVelocity(int motor_number, float velocity);
     void SetVelocity(int motor_number, float velocity, float current_feedforward);
     void SetCurrent(int motor_number, float current);
-
+    void TrapezoidalMove(int motor_number, float position);
     // General params
     float readFloat();
     int32_t readInt();

CHANGELOG.md

@@ -3,16 +3,28 @@ Please add a note of your changes below this heading if you make a Pull Request.
 
 ### Added
 * `dump_errors()` utility function in odrivetool to dump, decode and optionally clear errors.
+* `f` command to ascii protocol to get encoder position and velocity feedback.
 * `q` command to ascii protocol. It is like the old `p` command, but velocity and current mean limits, not feed-forward.
+* `ss`, `se`, `sr` commands to ascii protocol, for save config, erase config and reboot.
+* `move_incremental` function for relative trajectory moves.
+* `encoder.config.ignore_illegal_hall_state` option.
+* `encoder.config.enable_phase_interpolation` option. Setting to false may reduce jerky pulsations at low speed when using hall sensor feedback.
+* Analog input. Used the same way as the PWM input mappings.
 * Voltage limit soft clamping instead of ERROR_MODULATION_MAGNITUDE in gimbal motor closed loop.
 * Thermal current limit with linear derating.
 
+### Changed
+* Unified lockin drive modes. Current for index searching and encoder offset calibration now moved to axis.lockin.current.
+
+### Fixed
+* Added required 1.5 cycle phase shift between ADC and PWM, lack thereof caused unstable current controller at high eRPM.
+
 # Releases
 ## [0.4.7] - 2018-11-28
 ### Added
 * Overspeed fault
 * Current sense saturation fault.
-* Supress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
+* Suppress startup transients by sampling encoder estimate into position setpoint when entering closed loop control.
 * Make step dir gpio pins configurable.
 * Configuration variable `encoder.config.zero_count_on_find_idx`, true by default. Set to false to leave the initial encoder count to be where the axis was at boot.
 * Circular position setpoint mode: position setpoints wrapped [0, cpr). Useful for infinite incremental position control.

Firmware/Board/v3/Src/stm32f4xx_it.c

@@ -91,22 +91,34 @@ void NMI_Handler(void)
   /* USER CODE END NonMaskableInt_IRQn 1 */
 }
 
+void get_regs(void** stack_ptr) {
+  void* volatile r0 __attribute__((unused)) = stack_ptr[0];
+  void* volatile r1 __attribute__((unused)) = stack_ptr[1];
+  void* volatile r2 __attribute__((unused)) = stack_ptr[2];
+  void* volatile r3 __attribute__((unused)) = stack_ptr[3];
+
+  void* volatile r12 __attribute__((unused)) = stack_ptr[4];
+  void* volatile lr __attribute__((unused)) = stack_ptr[5];  // Link register
+  void* volatile pc __attribute__((unused)) = stack_ptr[6];  // Program counter
+  void* volatile psr __attribute__((unused)) = stack_ptr[7];  // Program status register
+
+  volatile bool stay_looping = true;
+  while(stay_looping);
+}
+
 /**
 * @brief This function handles Hard fault interrupt.
 */
+__attribute__((naked))
 void HardFault_Handler(void)
 {
-  /* USER CODE BEGIN HardFault_IRQn 0 */
-
-  /* USER CODE END HardFault_IRQn 0 */
-  while (1)
-  {
-    /* USER CODE BEGIN W1_HardFault_IRQn 0 */
-    /* USER CODE END W1_HardFault_IRQn 0 */
-  }
-  /* USER CODE BEGIN HardFault_IRQn 1 */
-
-  /* USER CODE END HardFault_IRQn 1 */
+  __asm(
+    " tst lr, #4     \n\t"
+    " ite eq         \n\t"
+    " mrseq r0, msp  \n\t"
+    " mrsne r0, psp  \n\t"
+    " b get_regs     \n\t"
+  );
 }
 
 /**

Firmware/MotorControl/axis.cpp

@@ -143,36 +143,66 @@ bool Axis::do_updates() {
     return ret;
 }
 
-bool Axis::run_sensorless_spin_up() {
-    // Early Spin-up: spiral up current
+bool Axis::run_lockin_spin() {
+    // 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_.ramp_up_distance * x);
-        float I_mag = config_.spin_up_current * x;
-        x += current_meas_period / config_.ramp_up_time;
+        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;
         if (!motor_.update(I_mag, phase, 0.0f))
-            return error_ |= ERROR_MOTOR_FAILED, false;
+            return false;
         return x < 1.0f;
     });
-    if (error_ != ERROR_NONE)
-        return false;
+    
+    // Spin states
+    float distance = config_.lockin.ramp_distance;
+    float phase = wrap_pm_pi(distance);
+    float vel = distance / config_.lockin.ramp_time;
 
-    // Late Spin-up: accelerate
-    float vel = config_.ramp_up_distance / config_.ramp_up_time;
-    float phase = wrap_pm_pi(config_.ramp_up_distance);
+    // 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)
+            done = done || encoder_.index_found_;
+        return done;
+    };
+
+    // Accelerate
+    lockin_state_ = LOCKIN_STATE_ACCELERATE;
     run_control_loop([&]() {
-        vel += config_.spin_up_acceleration * current_meas_period;
+        vel += config_.lockin.accel * current_meas_period;
+        distance += vel * current_meas_period;
         phase = wrap_pm_pi(phase + vel * current_meas_period);
-        float I_mag = config_.spin_up_current;
-        if (!motor_.update(I_mag, phase, vel))
-            return error_ |= ERROR_MOTOR_FAILED, false;
-        return vel < config_.spin_up_target_vel;
+
+        if (!motor_.update(config_.lockin.current, phase, vel))
+            return false;
+        return !spin_done(true); //vel_override to go to next phase
     });
 
-    // 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_.spin_up_target_vel;
+    if (!encoder_.index_found_)
+        encoder_.set_idx_subscribe(true);
 
+    // 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
+        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))
+                return false;
+            return !spin_done();
+        });
+    }
+
+    lockin_state_ = LOCKIN_STATE_INACTIVE;
     return check_for_errors();
 }
 
@@ -271,44 +301,69 @@ void Axis::run_state_machine_loop() {
 
         // Note that current_state is a reference to task_chain_[0]
 
-        // Validate the state before running it
-        if (current_state_ > AXIS_STATE_MOTOR_CALIBRATION && !motor_.is_calibrated_)
-            current_state_ = AXIS_STATE_UNDEFINED;
-        if (current_state_ > AXIS_STATE_ENCODER_OFFSET_CALIBRATION && !encoder_.is_ready_)
-            current_state_ = AXIS_STATE_UNDEFINED;
-
         // Run the specified state
         // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
         bool status;
         switch (current_state_) {
-            case AXIS_STATE_MOTOR_CALIBRATION:
+            case AXIS_STATE_MOTOR_CALIBRATION: {
                 status = motor_.run_calibration();
-                break;
+            } break;
+
+            case AXIS_STATE_ENCODER_INDEX_SEARCH: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+                if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
+                    goto invalid_state_label;
 
-            case AXIS_STATE_ENCODER_INDEX_SEARCH:
                 status = encoder_.run_index_search();
-                break;
+            } break;
 
-            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION:
+            case AXIS_STATE_ENCODER_DIR_FIND: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
+
+                status = encoder_.run_direction_find();
+            } break;
+
+            case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
+                if (!motor_.is_calibrated_)
+                    goto invalid_state_label;
                 status = encoder_.run_offset_calibration();
-                break;
+            } break;
 
-            case AXIS_STATE_SENSORLESS_CONTROL:
-                status = run_sensorless_spin_up();  // TODO: restart if desired
-                if (status)
+            case AXIS_STATE_LOCKIN_SPIN: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                    goto invalid_state_label;
+                status = run_lockin_spin();
+            } 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
+                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;
                     status = run_sensorless_control_loop();
-                break;
+                }
+            } break;
 
-            case AXIS_STATE_CLOSED_LOOP_CONTROL:
+            case AXIS_STATE_CLOSED_LOOP_CONTROL: {
+                if (!motor_.is_calibrated_ || motor_.config_.direction==0)
+                    goto invalid_state_label;
+                if (!encoder_.is_ready_)
+                    goto invalid_state_label;
                 status = run_closed_loop_control_loop();
-                break;
+            } break;
 
-            case AXIS_STATE_IDLE:
+            case AXIS_STATE_IDLE: {
                 run_idle_loop();
-                status = motor_.arm();  // done with idling - try to arm the motor
-                break;
+                status = motor_.arm(); // done with idling - try to arm the motor
+            } break;
 
             default:
+            invalid_state_label:
                 error_ |= ERROR_INVALID_STATE;
                 status = false;  // this will set the state to idle
                 break;

Firmware/MotorControl/axis.hpp

@@ -23,8 +23,6 @@ public:
         ERROR_ESTOP_REQUESTED = 0x800
     };
 
-    // Warning: Do not reorder these enum values.
-    // The state machine uses ">" comparision on them.
     enum State_t {
         AXIS_STATE_UNDEFINED = 0,           //<! will fall through to idle
         AXIS_STATE_IDLE = 1,                //<! disable PWM and do nothing
@@ -34,7 +32,21 @@ public:
         AXIS_STATE_SENSORLESS_CONTROL = 5,  //<! run sensorless control
         AXIS_STATE_ENCODER_INDEX_SEARCH = 6, //<! run encoder index search
         AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, //<! run encoder offset calibration
-        AXIS_STATE_CLOSED_LOOP_CONTROL = 8  //<! run closed loop control
+        AXIS_STATE_CLOSED_LOOP_CONTROL = 8,  //<! run closed loop control
+        AXIS_STATE_LOCKIN_SPIN = 9,       //<! run lockin spin
+        AXIS_STATE_ENCODER_DIR_FIND = 10,
+    };
+
+    struct LockinConfig_t {
+        float current = 10.0f;           // [A]
+        float ramp_time = 0.4f;          // [s]
+        float ramp_distance = 1 * M_PI;  // [rad]
+        float accel = 20.0f;     // [rad/s^2]
+        float vel = 40.0f; // [rad/s]
+        float finish_distance = 100.0f;  // [rad]
+        bool finish_on_vel = false;
+        bool finish_on_distance = false;
+        bool finish_on_enc_idx = false;
     };
 
     struct Config_t {
@@ -52,13 +64,7 @@ public:
         uint16_t step_gpio_pin = 0;
         uint16_t dir_gpio_pin = 0;
 
-        // Spinup settings
-        float ramp_up_time = 0.4f;            // [s]
-        float ramp_up_distance = 4 * M_PI;    // [rad]
-        float spin_up_current = 10.0f;        // [A]
-        float spin_up_acceleration = 400.0f;  // [rad/s^2]
-        float spin_up_target_vel = 400.0f;    // [rad/s]
-
+        LockinConfig_t lockin;
         uint8_t can_node_id = 0; // Both axes will have the same id to start
     };
 
@@ -66,6 +72,13 @@ public:
         M_SIGNAL_PH_CURRENT_MEAS = 1u << 0
     };
 
+    enum LockinState_t {
+        LOCKIN_STATE_INACTIVE,
+        LOCKIN_STATE_RAMP,
+        LOCKIN_STATE_ACCELERATE,
+        LOCKIN_STATE_CONST_VEL,
+    };
+
     Axis(const AxisHardwareConfig_t& hw_config,
             Config_t& config,
             Encoder& encoder,
@@ -90,7 +103,6 @@ public:
     bool do_checks();
     bool do_updates();
 
-
     // True if there are no errors
     bool inline check_for_errors() {
         return error_ == ERROR_NONE;
@@ -154,7 +166,7 @@ public:
         }
     }
 
-    bool run_sensorless_spin_up();
+    bool run_lockin_spin();
     bool run_sensorless_control_loop();
     bool run_closed_loop_control_loop();
     bool run_idle_loop();
@@ -187,6 +199,7 @@ public:
     State_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
     State_t& current_state_ = task_chain_[0];
     uint32_t loop_counter_ = 0;
+    LockinState_t lockin_state_ = LOCKIN_STATE_INACTIVE;
     uint32_t last_heartbeat_ = 0;
 
     // Communication protocol definitions
@@ -197,6 +210,7 @@ public:
             make_protocol_ro_property("current_state", &current_state_),
             make_protocol_property("requested_state", &requested_state_),
             make_protocol_ro_property("loop_counter", &loop_counter_),
+            make_protocol_ro_property("lockin_state", &lockin_state_),
             make_protocol_object("config",
                 make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
                 make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
@@ -209,11 +223,17 @@ 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_property("ramp_up_time", &config_.ramp_up_time),
-                make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
-                make_protocol_property("spin_up_current", &config_.spin_up_current),
-                make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
-                make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel),
+                make_protocol_object("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),
+                    make_protocol_property("accel", &config_.lockin.accel),
+                    make_protocol_property("vel", &config_.lockin.vel),
+                    make_protocol_property("finish_distance", &config_.lockin.finish_distance),
+                    make_protocol_property("finish_on_vel", &config_.lockin.finish_on_vel),
+                    make_protocol_property("finish_on_distance", &config_.lockin.finish_on_distance),
+                    make_protocol_property("finish_on_enc_idx", &config_.lockin.finish_on_enc_idx)
+                ),
                 make_protocol_property("can_node_id", &config_.can_node_id)
             ),
             make_protocol_object("motor", motor_.make_protocol_definitions()),

Firmware/MotorControl/controller.cpp

@@ -56,6 +56,15 @@ void Controller::move_to_pos(float goal_point) {
                                  axis_->trap_.config_.decel_limit);
     traj_start_loop_count_ = axis_->loop_counter_;
     config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL;
+    goal_point_ = goal_point;
+}
+
+void Controller::move_incremental(float displacement, bool from_goal_point = true){
+    if(from_goal_point){
+        move_to_pos(goal_point_ + displacement);
+    } else{
+        move_to_pos(pos_setpoint_ + displacement);
+    }
 }
 
 void Controller::start_anticogging_calibration() {

Firmware/MotorControl/controller.hpp

@@ -44,6 +44,7 @@ public:
 
     // Trajectory-Planned control
     void move_to_pos(float goal_point);
+    void move_incremental(float displacement, bool from_goal_point);
     
     // TODO: make this more similar to other calibration loops
     void start_anticogging_calibration();
@@ -89,6 +90,8 @@ public:
 
     uint32_t traj_start_loop_count_ = 0;
 
+    float goal_point_ = 0.0f;
+
     // Communication protocol definitions
     auto make_protocol_definitions() {
         return make_protocol_member_list(
@@ -114,8 +117,9 @@ public:
             make_protocol_function("set_vel_setpoint", *this, &Controller::set_vel_setpoint,
                 "vel_setpoint", "current_feed_forward"),
             make_protocol_function("set_current_setpoint", *this, &Controller::set_current_setpoint,
-                "current_setpoint"),
-            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "goal_point"),
+                                   "current_setpoint"),
+            make_protocol_function("move_to_pos", *this, &Controller::move_to_pos, "pos_setpoint"),
+            make_protocol_function("move_incremental", *this, &Controller::move_incremental, "displacement", "from_goal_point"),
             make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration)
         );
     }

Firmware/MotorControl/encoder.cpp

@@ -20,8 +20,7 @@ static void enc_index_cb_wrapper(void* ctx) {
 
 void Encoder::setup() {
     HAL_TIM_Encoder_Start(hw_config_.timer, TIM_CHANNEL_ALL);
-    GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_NOPULL,
-            enc_index_cb_wrapper, this);
+    set_idx_subscribe();
 }
 
 void Encoder::set_error(Error_t error) {
@@ -56,6 +55,20 @@ void Encoder::enc_index_cb() {
         }
         index_found_ = true;
     }
+
+    // Disable interrupt
+    GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
+}
+
+void Encoder::set_idx_subscribe(bool override_enable) {
+    if (override_enable || (config_.use_index && !config_.find_idx_on_lockin_only)) {
+        GPIO_subscribe(hw_config_.index_port, hw_config_.index_pin, GPIO_PULLDOWN,
+                enc_index_cb_wrapper, this);
+    }
+
+    if (!config_.use_index || config_.find_idx_on_lockin_only) {
+        GPIO_unsubscribe(hw_config_.index_port, hw_config_.index_pin);
+    }
 }
 
 // Function that sets the current encoder count to a desired 32-bit value.
@@ -90,36 +103,42 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) {
     cpu_exit_critical(prim);
 }
 
-
-// @brief Slowly turns the motor in one direction until the
-// encoder index is found.
-// TODO: Do the scan with current, not voltage!
 bool Encoder::run_index_search() {
-    float voltage_magnitude;
-    if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_HIGH_CURRENT)
-        voltage_magnitude = axis_->motor_.config_.calibration_current * axis_->motor_.config_.phase_resistance;
-    else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL)
-        voltage_magnitude = axis_->motor_.config_.calibration_current;
-    else
-        return false;
-    
-    float omega = (float)(axis_->motor_.config_.direction) * config_.idx_search_speed;
-
+    config_.use_index = true;
     index_found_ = false;
-    float phase = 0.0f;
-    axis_->run_control_loop([&](){
-        phase = wrap_pm_pi(phase + omega * current_meas_period);
+    if (!config_.idx_search_unidirectional && axis_->motor_.config_.direction == 0) {
+        axis_->motor_.config_.direction = 1;
+    }
 
-        float v_alpha = voltage_magnitude * our_arm_cos_f32(phase);
-        float v_beta = voltage_magnitude * our_arm_sin_f32(phase);
-        if (!axis_->motor_.enqueue_voltage_timings(v_alpha, v_beta))
-            return false; // error set inside enqueue_voltage_timings
-        axis_->motor_.log_timing(Motor::TIMING_LOG_IDX_SEARCH);
+    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;
+    return status;
+}
 
-        // continue until the index is found
-        return !index_found_;
-    });
-    return true;
+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;
+    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;
+
+    if (status) {
+        // Check response and direction
+        if (shadow_count_ > init_enc_val + 8) {
+            // motor same dir as encoder
+            axis_->motor_.config_.direction = 1;
+        } else if (shadow_count_ < init_enc_val - 8) {
+            // motor opposite dir as encoder
+            axis_->motor_.config_.direction = -1;
+        } else {
+            axis_->motor_.config_.direction = 0;
+        }
+    }
+
+    return status;
 }
 
 // @brief Turns the motor in one direction for a bit and then in the other
@@ -342,7 +361,7 @@ bool Encoder::update() {
     //// run encoder count interpolation
     int32_t corrected_enc = count_in_cpr_ - config_.offset;
     // if we are stopped, make sure we don't randomly drift
-    if (snap_to_zero_vel) {
+    if (snap_to_zero_vel || !config_.enable_phase_interpolation) {
         interpolation_ = 0.5f;
     // reset interpolation if encoder edge comes
     } else if (delta_enc > 0) {

Firmware/MotorControl/encoder.hpp

@@ -31,14 +31,16 @@ public:
                                     // be determined by run_offset_calibration.
                                     // In this case the encoder will enter ready
                                     // state as soon as the index is found.
-        float idx_search_speed = 10.0f; // [rad/s electrical]
         bool zero_count_on_find_idx = true;
         int32_t cpr = (2048 * 4);   // Default resolution of CUI-AMT102 encoder,
         int32_t offset = 0;        // Offset between encoder count and rotor electrical phase
         float offset_float = 0.0f; // Sub-count phase alignment offset
-        float calib_range = 0.02f;
+        bool enable_phase_interpolation = true; // Use velocity to interpolate inside the count state
+        float calib_range = 0.02f; // Accuracy required to pass encoder cpr check
         float bandwidth = 1000.0f;
-        bool ignore_illegal_hall_state = false;
+        bool find_idx_on_lockin_only = false; // Only be sensitive during lockin scan constant vel state
+        bool idx_search_unidirectional = false; // Only allow index search in known direction
+        bool ignore_illegal_hall_state = false; // dont error on bad states like 000 or 111
     };
 
     Encoder(const EncoderHardwareConfig_t& hw_config,
@@ -49,13 +51,14 @@ public:
     bool do_checks();
 
     void enc_index_cb();
+    void set_idx_subscribe(bool override_enable = false);
 
     void set_linear_count(int32_t count);
     void set_circular_count(int32_t count, bool update_offset);
     bool calib_enc_offset(float voltage_magnitude);
-    bool scan_for_enc_idx(float omega, float voltage_magnitude);
 
     bool run_index_search();
+    bool run_direction_find();
     bool run_offset_calibration();
     void sample_now();
     bool update();
@@ -103,16 +106,20 @@ public:
             // make_protocol_property("pll_ki", &pll_ki_),
             make_protocol_object("config",
                 make_protocol_property("mode", &config_.mode),
-                make_protocol_property("use_index", &config_.use_index),
+                make_protocol_property("use_index", &config_.use_index,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
+                make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only,
+                    [](void* ctx) { static_cast<Encoder*>(ctx)->set_idx_subscribe(); }, this),
                 make_protocol_property("pre_calibrated", &config_.pre_calibrated),
-                make_protocol_property("idx_search_speed", &config_.idx_search_speed),
                 make_protocol_property("zero_count_on_find_idx", &config_.zero_count_on_find_idx),
                 make_protocol_property("cpr", &config_.cpr),
                 make_protocol_property("offset", &config_.offset),
                 make_protocol_property("offset_float", &config_.offset_float),
+                make_protocol_property("enable_phase_interpolation", &config_.enable_phase_interpolation),
                 make_protocol_property("bandwidth", &config_.bandwidth,
                     [](void* ctx) { static_cast<Encoder*>(ctx)->update_pll_gains(); }, this),
                 make_protocol_property("calib_range", &config_.calib_range),
+                make_protocol_property("idx_search_unidirectional", &config_.idx_search_unidirectional),
                 make_protocol_property("ignore_illegal_hall_state", &config_.ignore_illegal_hall_state)
             )
         );

Firmware/MotorControl/main.cpp

@@ -104,7 +104,7 @@ void enter_dfu_mode() {
 
 extern "C" {
 int odrive_main(void);
-void vApplicationStackOverflowHook(void) {
+void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) {
     for (;;); // TODO: safe action
 }
 void vApplicationIdleHook(void) {

Firmware/MotorControl/motor.hpp

@@ -63,7 +63,7 @@ public:
         float resistance_calib_max_voltage = 2.0f; // [V] - You may need to increase this if this voltage isn't sufficient to drive calibration_current through the motor.
         float phase_inductance = 0.0f;        // to be set by measure_phase_inductance
         float phase_resistance = 0.0f;        // to be set by measure_phase_resistance
-        int32_t direction = 1;                // 1 or -1
+        int32_t direction = 0;                // 1 or -1 (0 = unspecified)
         MotorType_t motor_type = MOTOR_TYPE_HIGH_CURRENT;
         // Read out max_allowed_current to see max supported value for current_lim.
         // float current_lim = 70.0f; //[A]

Firmware/communication/ascii_protocol.cpp

@@ -106,15 +106,17 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         unsigned motor_number;
         float pos_setpoint, vel_limit, current_lim;
         int numscan = sscanf(cmd, "q %u %f %f %f", &motor_number, &pos_setpoint, &vel_limit, &current_lim);
-        if (numscan < 4) {
+        if (numscan < 2) {
             respond(response_channel, use_checksum, "invalid command format");
         } else if (motor_number >= AXIS_COUNT) {
             respond(response_channel, use_checksum, "invalid motor %u", motor_number);
         } else {
             Axis* axis = axes[motor_number];
             axis->controller_.pos_setpoint_ = pos_setpoint;
-            axis->controller_.config_.vel_limit = vel_limit;
-            axis->motor_.config_.current_lim = current_lim;
+            if (numscan >= 3)
+                axis->controller_.config_.vel_limit = vel_limit;
+            if (numscan >= 4)
+                axis->motor_.config_.current_lim = current_lim;
         }
 
     } else if (cmd[0] == 'v') { // velocity control
@@ -155,10 +157,24 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
             axes[motor_number]->controller_.move_to_pos(goal_point);
         }
 
+    } else if (cmd[0] == 'f') { // feedback
+        unsigned motor_number;
+        int numscan = sscanf(cmd, "f %u", &motor_number);
+        if (numscan < 1) {
+            respond(response_channel, use_checksum, "invalid command format");
+        } else if (motor_number >= AXIS_COUNT) {
+            respond(response_channel, use_checksum, "invalid motor %u", motor_number);
+        } else {
+            respond(response_channel, use_checksum, "%f %f",
+                    (double)axes[motor_number]->encoder_.pos_estimate_,
+                    (double)axes[motor_number]->encoder_.vel_estimate_);
+        }
+
     } else if (cmd[0] == 'h') {  // Help
         respond(response_channel, use_checksum, "Please see documentation for more details");
         respond(response_channel, use_checksum, "");
         respond(response_channel, use_checksum, "Available commands syntax reference:");
+        respond(response_channel, use_checksum, "Position: q axis pos vel-lim I-lim");
         respond(response_channel, use_checksum, "Position: p axis pos vel-ff I-ff");
         respond(response_channel, use_checksum, "Velocity: v axis vel I-ff");
         respond(response_channel, use_checksum, "Current: c axis I");
@@ -166,6 +182,10 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         respond(response_channel, use_checksum, "Properties start at odrive root, such as axis0.requested_state");
         respond(response_channel, use_checksum, "Read: r property");
         respond(response_channel, use_checksum, "Write: w property value");
+        respond(response_channel, use_checksum, "");
+        respond(response_channel, use_checksum, "Save config: ss");
+        respond(response_channel, use_checksum, "Erase config: se");
+        respond(response_channel, use_checksum, "Reboot: sr");
 
     } else if (cmd[0] == 'i'){ // Dump device info
         // respond(response_channel, use_checksum, "Signature: %#x", STM_ID_GetSignature());
@@ -175,12 +195,14 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink&
         respond(response_channel, use_checksum, "Firmware version: %d.%d.%d", FW_VERSION_MAJOR, FW_VERSION_MINOR, FW_VERSION_REVISION);
         respond(response_channel, use_checksum, "Serial number: %s", serial_number_str);
 
-    } else if (cmd[0] == 's'){ // Save config
-        save_configuration();
-    } else if (cmd[0] == 'e'){ // Erase config
-        erase_configuration();
-    } else if (cmd[0] == 'b'){ // Reboot
-        NVIC_SystemReset();
+    } else if (cmd[0] == 's'){ // System
+        if(cmd[1] == 's') { // Save config
+            save_configuration();
+        } else if (cmd[1] == 'e'){ // Erase config
+            erase_configuration();
+        } else if (cmd[1] == 'b'){ // Reboot
+            NVIC_SystemReset();
+        }
 
     } else if (cmd[0] == 'r') { // read property
         char name[MAX_LINE_LENGTH];

Firmware/communication/communication.cpp

@@ -84,7 +84,7 @@ void init_communication(void) {
     printf("hi!\r\n");
 
     // Start command handling thread
-    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 6000 /* in 32-bit words */); // TODO: fix stack issues
+    osThreadDef(task_cmd_parse, communication_task, osPriorityNormal, 0, 8000 /* in 32-bit words */); // TODO: fix stack issues
     comm_thread = osThreadCreate(osThread(task_cmd_parse), NULL);
 
     while (!endpoint_list_valid)

Firmware/communication/interface_usb.cpp

@@ -177,6 +177,6 @@ void usb_rx_process_packet(uint8_t *buf, uint32_t len, uint8_t endpoint_pair) {
 
 void start_usb_server() {
     // Start USB communication thread
-    osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 512);
+    osThreadDef(usb_server_thread_def, usb_server_thread, osPriorityNormal, 0, 1024);
     usb_thread = osThreadCreate(osThread(usb_server_thread_def), NULL);
 }

docs/ascii-protocol.md

@@ -37,6 +37,20 @@ Example: `t 0 -20000`
 For general moving around of the axis, this is the recommended command.
 
 #### Motor Position command
+For basic use where you send one setpoint at at a time, use the `q` command.
+If you have a realtime controller that is streaming setpoints and tracking a trajectory, use the `p` command.
+
+```
+q motor position velocity_lim current_lim
+```
+* `q` for position
+* `motor` is the motor number, `0` or `1`.
+* `position` is the desired position, in encoder counts.
+* `velocity_lim` is the velocity limit, in counts/s (optional).
+* `current_lim` is the current limit, in A (optional).
+
+Example: `q 0 -20000 10000 10`
+
 ```
 p motor position velocity_ff current_ff
 ```
@@ -90,3 +104,8 @@ Not all parameters can be accessed via the ASCII protocol but at least all param
    * `property` name of the property, as seen in ODrive Tool
    * `value` text representation of the value to be written
    * Example: `w axis0.controller.pos_setpoint -123.456`
+
+#### System commands:
+* `ss` - Save config
+* `se` - Erase config
+* `sr` - Reboot

docs/getting-started.md

@@ -261,7 +261,7 @@ You can also directly control the current of the motor, which is proportional to
 
 
 ### Trajectory control
-Set `axis.controller.config.control_mode = CTRL_MODE_TRAJECTORY_CONTROL`.<br>
+While in position control mode, use the `move_to_pos` or `move_incremental` functions. See the **Usage** section for details<br>
 This mode lets you smoothly accelerate, coast, and decelerate the axis from one position to another. With raw position control, the controller simply tries to go to the setpoint as quickly as possible. Using a trajectory lets you tune the feedback gains more aggressively to reject disturbance, while keeping smooth motion.
 
 ![Taptraj](TrapTrajPosVel.PNG)<br>
@@ -291,9 +291,18 @@ Keep in mind that you must still set your safety limits as before.  I recommend
 #### Usage
 Use the `move_to_pos` function to move to an absolute position:
 ```
-<odrv>.<axis>.controller.move_to_pos(<Float>)
+<odrv>.<axis>.controller.move_to_pos(your_absolute_pos)
 ```
 
+Use the `move_incremental` function to move to a relative position.
+To set the goal relative to the current actual position, use `from_goal_point = False`
+To set the goal relative to the previous destination, use `from_goal_point = True`
+```
+<odrv>.<axis>.controller.move_incremental(pos_increment, from_goal_point)
+```
+
+You can also execute a move with the [appropriate ascii command](ascii-protocol.md#motor-trajectory-command).
+
 ### Circular position control
 
 To enable Circular position control, set `axis.controller.config.setpoints_in_cpr = True`

tools/odrive/enums.py

@@ -10,6 +10,8 @@ AXIS_STATE_SENSORLESS_CONTROL = 5
 AXIS_STATE_ENCODER_INDEX_SEARCH = 6
 AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7
 AXIS_STATE_CLOSED_LOOP_CONTROL = 8
+AXIS_STATE_LOCKIN_SPIN = 9
+AXIS_STATE_ENCODER_DIR_FIND = 10
 
 class errors:
     class axis: