diff --git a/Arduino/ODriveArduino/ODriveArduino.cpp b/Arduino/ODriveArduino/ODriveArduino.cpp index 2b2d6feb..00fce19a 100644 --- a/Arduino/ODriveArduino/ODriveArduino.cpp +++ b/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(); } diff --git a/Arduino/ODriveArduino/ODriveArduino.h b/Arduino/ODriveArduino/ODriveArduino.h index 1ebe1e33..86b3aaf1 100644 --- a/Arduino/ODriveArduino/ODriveArduino.h +++ b/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(); diff --git a/CHANGELOG.md b/CHANGELOG.md index 46feb061..85831d9a 100644 --- a/CHANGELOG.md +++ b/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. diff --git a/Firmware/Board/v3/Src/stm32f4xx_it.c b/Firmware/Board/v3/Src/stm32f4xx_it.c index f0fe1d3e..7746ae87 100644 --- a/Firmware/Board/v3/Src/stm32f4xx_it.c +++ b/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" + ); } /** diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 8be43d15..0ba614c3 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -141,36 +141,66 @@ bool Axis::do_updates() { return check_for_errors(); } -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; + 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; 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; - // Late Spin-up: accelerate - float vel = config_.ramp_up_distance / config_.ramp_up_time; - float phase = wrap_pm_pi(config_.ramp_up_distance); - run_control_loop([&](){ - vel += config_.spin_up_acceleration * current_meas_period; + // Spin states + float distance = config_.lockin.ramp_distance; + float phase = wrap_pm_pi(distance); + float vel = distance / config_.lockin.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) + done = done || encoder_.index_found_; + return done; + }; + + // Accelerate + lockin_state_ = LOCKIN_STATE_ACCELERATE; + run_control_loop([&]() { + 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(); } @@ -270,44 +300,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; + } break; default: + invalid_state_label: error_ |= ERROR_INVALID_STATE; status = false; // this will set the state to idle break; diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index a6b6bdae..dd8e88ef 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -22,8 +22,6 @@ public: ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, }; - // Warning: Do not reorder these enum values. - // The state machine uses ">" comparision on them. enum State_t { AXIS_STATE_UNDEFINED = 0, //(ctx)->decode_step_dir_pins(); }, this), make_protocol_property("dir_gpio_pin", &config_.dir_gpio_pin, [](void* ctx) { static_cast(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_object("motor", motor_.make_protocol_definitions()), make_protocol_object("controller", controller_.make_protocol_definitions()), diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 4e46c88b..d295246c 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/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() { diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b39c2890..7ffa25b6 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/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) ); } diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index d79e33de..4ea99133 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/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) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 1cda9800..dff98f28 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/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(ctx)->set_idx_subscribe(); }, this), + make_protocol_property("find_idx_on_lockin_only", &config_.find_idx_on_lockin_only, + [](void* ctx) { static_cast(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(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) ) ); diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index b25da149..7ffeb5e3 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -97,7 +97,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) { diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 9742835e..279e6848 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/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] diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 380530d1..8a0d3287 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/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, ¤t_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]; diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index 7c08654a..79982e64 100644 --- a/Firmware/communication/communication.cpp +++ b/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) diff --git a/Firmware/communication/interface_usb.cpp b/Firmware/communication/interface_usb.cpp index cd44c907..036a8203 100644 --- a/Firmware/communication/interface_usb.cpp +++ b/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); } diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 51ff18ea..0c510fe6 100644 --- a/docs/ascii-protocol.md +++ b/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 diff --git a/docs/getting-started.md b/docs/getting-started.md index 1a346bd8..982ec861 100644 --- a/docs/getting-started.md +++ b/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`.
+While in position control mode, use the `move_to_pos` or `move_incremental` functions. See the **Usage** section for details
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)
@@ -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: ``` -..controller.move_to_pos() +..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` +``` +..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` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 817e39e1..fc5439bd 100644 --- a/tools/odrive/enums.py +++ b/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: