diff --git a/CHANGELOG.md b/CHANGELOG.md index 88668ace..a5c1a4f5 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased ### Added +* **Trapezoidal Trajectory Planner** * -Wdouble-promotion warning to compilation ### Changed diff --git a/Firmware/LICENSE b/Firmware/LICENSE index 9eeab46b..370d37c6 100644 --- a/Firmware/LICENSE +++ b/Firmware/LICENSE @@ -1,6 +1,6 @@ The MIT License (MIT) -Copyright (c) 2016 Oskar Weigl (madcowswe) +Copyright (c) 2016-2018 Oskar Weigl Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 7d79b3d6..4ff660fd 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -7,22 +7,25 @@ #include "odrive_main.h" Axis::Axis(const AxisHardwareConfig_t& hw_config, - AxisConfig_t& config, + Config_t& config, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, - Motor& motor) + Motor& motor, + TrapezoidalTrajectory& trap) : hw_config_(hw_config), config_(config), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), - motor_(motor) + motor_(motor), + trap_(trap) { encoder_.axis_ = this; sensorless_estimator_.axis_ = this; controller_.axis_ = this; motor_.axis_ = this; + trap_.axis_ = this; } static void step_cb_wrapper(void* ctx) { @@ -179,7 +182,7 @@ bool Axis::run_sensorless_spin_up() { bool Axis::run_sensorless_control_loop() { set_step_dir_enabled(config_.enable_step_dir); run_control_loop([this](){ - if (controller_.config_.control_mode >= CTRL_MODE_POSITION_CONTROL) + if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL) return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false; // Note that all estimators are updated in the loop prefix in run_control_loop diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index 111f9e26..6063ee3a 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -5,40 +5,6 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Warning: Do not reorder these enum values. -// The state machine uses ">" comparision on them. -enum AxisState_t { - AXIS_STATE_UNDEFINED = 0, //" comparision on them. + enum State_t { + AXIS_STATE_UNDEFINED = 0, //trap_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_, + axis_->trap_.config_.vel_limit, + axis_->trap_.config_.accel_limit, + axis_->trap_.config_.decel_limit); + traj_start_loop_count_ = axis_->loop_counter_; + config_.control_mode = CTRL_MODE_TRAJECTORY_CONTROL; +} + void Controller::start_anticogging_calibration() { // Ensure the cogging map was correctly allocated earlier and that the motor is capable of calibrating if (anticogging_.cogging_map != NULL && axis_->error_ == Axis::ERROR_NONE) { @@ -82,7 +91,28 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) bool Controller::update(float pos_estimate, float vel_estimate, float* current_setpoint_output) { // Only runs if anticogging_.calib_anticogging is true; non-blocking anticogging_calibration(pos_estimate, vel_estimate); - + float anticogging_pos = pos_estimate; + + // Trajectory control + if (config_.control_mode == CTRL_MODE_TRAJECTORY_CONTROL) { + // Note: uint32_t loop count delta is OK across overflow + // Beware of negative deltas, as they will not be well behaved due to uint! + float t = (axis_->loop_counter_ - traj_start_loop_count_) * current_meas_period; + if (t > axis_->trap_.Tf_) { + // Drop into position control mode when done to avoid problems on loop counter delta overflow + config_.control_mode = CTRL_MODE_POSITION_CONTROL; + // pos_setpoint already set by trajectory + vel_setpoint_ = 0.0f; + current_setpoint_ = 0.0f; + } else { + TrapezoidalTrajectory::Step_t traj_step = axis_->trap_.eval(t); + pos_setpoint_ = traj_step.Y; + vel_setpoint_ = traj_step.Yd; + current_setpoint_ = traj_step.Ydd * axis_->trap_.config_.A_per_css; + } + anticogging_pos = pos_setpoint_; // FF the position setpoint instead of the pos_estimate + } + // Position control // TODO Decide if we want to use encoder or pll position here float vel_des = vel_setpoint_; @@ -103,7 +133,7 @@ bool Controller::update(float pos_estimate, float vel_estimate, float* current_s // We get the current position and apply a current feed-forward // ensuring that we handle negative encoder positions properly (-1 == motor->encoder.encoder_cpr - 1) if (anticogging_.use_anticogging) { - Iq += anticogging_.cogging_map[mod(static_cast(pos_estimate), axis_->encoder_.config_.cpr)]; + Iq += anticogging_.cogging_map[mod(static_cast(anticogging_pos), axis_->encoder_.config_.cpr)]; } float v_err = vel_des - vel_estimate; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index 80f5d25a..3db712e4 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -5,32 +5,36 @@ #error "This file should not be included directly. Include odrive_main.h instead." #endif -// Note: these should be sorted from lowest level of control to -// highest level of control, to allow "<" style comparisons. -typedef enum { - CTRL_MODE_VOLTAGE_CONTROL = 0, - CTRL_MODE_CURRENT_CONTROL = 1, - CTRL_MODE_VELOCITY_CONTROL = 2, - CTRL_MODE_POSITION_CONTROL = 3 -} Motor_control_mode_t; - -struct ControllerConfig_t { - Motor_control_mode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t - float pos_gain = 20.0f; // [(counts/s) / counts] - float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] - // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] - float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] - float vel_limit = 20000.0f; // [counts/s] -}; - class Controller { public: - Controller(ControllerConfig_t& config); + // Note: these should be sorted from lowest level of control to + // highest level of control, to allow "<" style comparisons. + enum ControlMode_t{ + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, + CTRL_MODE_VELOCITY_CONTROL = 2, + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_TRAJECTORY_CONTROL = 4 + }; + + struct Config_t { + ControlMode_t control_mode = CTRL_MODE_POSITION_CONTROL; //see: Motor_control_mode_t + float pos_gain = 20.0f; // [(counts/s) / counts] + float vel_gain = 5.0f / 10000.0f; // [A/(counts/s)] + // float vel_gain = 5.0f / 200.0f, // [A/(rad/s)] + float vel_integrator_gain = 10.0f / 10000.0f; // [A/(counts/s * s)] + float vel_limit = 20000.0f; // [counts/s] + }; + + Controller(Config_t& config); void reset(); void set_pos_setpoint(float pos_setpoint, float vel_feed_forward, float current_feed_forward); void set_vel_setpoint(float vel_setpoint, float current_feed_forward); void set_current_setpoint(float current_setpoint); + + // Trajectory-Planned control + void move_to_pos(float goal_point); // TODO: make this more similar to other calibration loops void start_anticogging_calibration(); @@ -38,7 +42,7 @@ public: bool update(float pos_estimate, float vel_estimate, float* current_setpoint); - ControllerConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor // TODO: anticogging overhaul: @@ -71,6 +75,8 @@ public: float vel_integrator_current_ = 0.0f; // [A] float current_setpoint_ = 0.0f; // [A] + uint32_t traj_start_loop_count_ = 0; + // Communication protocol definitions auto make_protocol_definitions() { return make_protocol_member_list( @@ -91,6 +97,7 @@ public: "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"), make_protocol_function("start_anticogging_calibration", *this, &Controller::start_anticogging_calibration) ); } diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index 5b80ed8d..82c183b1 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -95,9 +95,9 @@ void Encoder::set_circular_count(int32_t count, bool update_offset) { // TODO: Do the scan with current, not voltage! bool Encoder::run_index_search() { float voltage_magnitude; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + 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_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; @@ -142,9 +142,9 @@ bool Encoder::run_offset_calibration() { shadow_count_ = count_in_cpr_; float voltage_magnitude; - if (axis_->motor_.config_.motor_type == MOTOR_TYPE_HIGH_CURRENT) + 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_TYPE_GIMBAL) + else if (axis_->motor_.config_.motor_type == Motor::MOTOR_TYPE_GIMBAL) voltage_magnitude = axis_->motor_.config_.calibration_current; else return false; diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 18b88433..4550605e 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -11,9 +11,10 @@ BoardConfig_t board_config; Encoder::Config_t encoder_configs[AXIS_COUNT]; SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; -ControllerConfig_t controller_configs[AXIS_COUNT]; -MotorConfig_t motor_configs[AXIS_COUNT]; -AxisConfig_t axis_configs[AXIS_COUNT]; +Controller::Config_t controller_configs[AXIS_COUNT]; +Motor::Config_t motor_configs[AXIS_COUNT]; +Axis::Config_t axis_configs[AXIS_COUNT]; +TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; bool user_config_loaded_; SystemStats_t system_stats_ = { 0 }; @@ -24,9 +25,10 @@ typedef Config< BoardConfig_t, Encoder::Config_t[AXIS_COUNT], SensorlessEstimator::Config_t[AXIS_COUNT], - ControllerConfig_t[AXIS_COUNT], - MotorConfig_t[AXIS_COUNT], - AxisConfig_t[AXIS_COUNT]> ConfigFormat; + Controller::Config_t[AXIS_COUNT], + Motor::Config_t[AXIS_COUNT], + TrapezoidalTrajectory::Config_t[AXIS_COUNT], + Axis::Config_t[AXIS_COUNT]> ConfigFormat; void save_configuration(void) { if (ConfigFormat::safe_store_config( @@ -35,6 +37,7 @@ void save_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //printf("saving configuration failed\r\n"); osDelay(5); } else { @@ -51,15 +54,17 @@ void load_configuration(void) { &sensorless_configs, &controller_configs, &motor_configs, + &trap_configs, &axis_configs)) { //If loading failed, restore defaults board_config = BoardConfig_t(); for (size_t i = 0; i < AXIS_COUNT; ++i) { encoder_configs[i] = Encoder::Config_t(); sensorless_configs[i] = SensorlessEstimator::Config_t(); - controller_configs[i] = ControllerConfig_t(); - motor_configs[i] = MotorConfig_t(); - axis_configs[i] = AxisConfig_t(); + controller_configs[i] = Controller::Config_t(); + motor_configs[i] = Motor::Config_t(); + trap_configs[i] = TrapezoidalTrajectory::Config_t(); + axis_configs[i] = Axis::Config_t(); } } else { user_config_loaded_ = true; @@ -162,8 +167,9 @@ int odrive_main(void) { Motor *motor = new Motor(hw_configs[i].motor_config, 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], - *encoder, *sensorless_estimator, *controller, *motor); + *encoder, *sensorless_estimator, *controller, *motor, *trap); } // Start ADC for temperature measurements and user measurements diff --git a/Firmware/MotorControl/motor.cpp b/Firmware/MotorControl/motor.cpp index 56817ee8..1782414c 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -6,8 +6,8 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, - const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config) : + const GateDriverHardwareConfig_t& gate_driver_config, + Config_t& config) : hw_config_(hw_config), gate_driver_config_(gate_driver_config), config_(config), @@ -298,10 +298,11 @@ bool Motor::FOC_voltage(float v_d, float v_q, float phase) { } bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { - Current_control_t* ictrl = ¤t_control_; + // Syntactic sugar + CurrentControl_t& ictrl = current_control_; // For Reporting - ictrl->Iq_setpoint = Iq_des; + ictrl.Iq_setpoint = Iq_des; // Clarke transform float Ialpha = -current_meas_.phB - current_meas_.phC; @@ -312,7 +313,7 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { float s = arm_sin_f32(phase); float Id = c * Ialpha + s * Ibeta; float Iq = c * Ibeta - s * Ialpha; - ictrl->Iq_measured = Iq; + ictrl.Iq_measured = Iq; // Current error float Ierr_d = Id_des - Id; @@ -320,8 +321,8 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { // TODO look into feed forward terms (esp omega, since PI pole maps to RL tau) // Apply PI control - float Vd = ictrl->v_current_control_integral_d + Ierr_d * ictrl->p_gain; - float Vq = ictrl->v_current_control_integral_q + Ierr_q * ictrl->p_gain; + float Vd = ictrl.v_current_control_integral_d + Ierr_d * ictrl.p_gain; + float Vq = ictrl.v_current_control_integral_q + Ierr_q * ictrl.p_gain; float mod_to_V = (2.0f / 3.0f) * vbus_voltage; float V_to_mod = 1.0f / mod_to_V; @@ -335,23 +336,23 @@ bool Motor::FOC_current(float Id_des, float Iq_des, float phase) { mod_d *= mod_scalefactor; mod_q *= mod_scalefactor; // TODO make decayfactor configurable - ictrl->v_current_control_integral_d *= 0.99f; - ictrl->v_current_control_integral_q *= 0.99f; + ictrl.v_current_control_integral_d *= 0.99f; + ictrl.v_current_control_integral_q *= 0.99f; } else { - ictrl->v_current_control_integral_d += Ierr_d * (ictrl->i_gain * current_meas_period); - ictrl->v_current_control_integral_q += Ierr_q * (ictrl->i_gain * current_meas_period); + ictrl.v_current_control_integral_d += Ierr_d * (ictrl.i_gain * current_meas_period); + ictrl.v_current_control_integral_q += Ierr_q * (ictrl.i_gain * current_meas_period); } // Compute estimated bus current - ictrl->Ibus = mod_d * Id + mod_q * Iq; + ictrl.Ibus = mod_d * Id + mod_q * Iq; // Inverse park transform float mod_alpha = c * mod_d - s * mod_q; float mod_beta = c * mod_q + s * mod_d; // Report final applied voltage in stationary frame (for sensorles estimator) - ictrl->final_v_alpha = mod_to_V * mod_alpha; - ictrl->final_v_beta = mod_to_V * mod_beta; + ictrl.final_v_alpha = mod_to_V * mod_alpha; + ictrl.final_v_beta = mod_to_V * mod_beta; // Apply SVM if (!enqueue_modulation_timings(mod_alpha, mod_beta)) diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 07820726..235ca6a8 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -7,51 +7,6 @@ #include "drv8301.h" -typedef enum { - MOTOR_TYPE_HIGH_CURRENT = 0, - // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented - MOTOR_TYPE_GIMBAL = 2 -} Motor_type_t; - -typedef struct { - float phB; - float phC; -} Iph_BC_t; - -typedef struct { - float p_gain; // [V/A] - float i_gain; // [V/As] - float v_current_control_integral_d; // [V] - float v_current_control_integral_q; // [V] - float Ibus; // DC bus current [A] - // Voltage applied at end of cycle: - float final_v_alpha; // [V] - float final_v_beta; // [V] - float Iq_setpoint; - float Iq_measured; - float max_allowed_current; -} Current_control_t; - -// NOTE: for gimbal motors, all units of A are instead V. -// example: vel_gain is [V/(count/s)] instead of [A/(count/s)] -// example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. -typedef struct { - bool pre_calibrated = false; // can be set to true to indicate that all values here are valid - int32_t pole_pairs = 7; - float calibration_current = 10.0f; // [A] - float resistance_calib_max_voltage = 1.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 - Motor_type_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] - float current_lim = 10.0f; //[A] - // Value used to compute shunt amplifier gains - float requested_current_range = 70.0f; // [A] - float current_control_bandwidth = 1000.0f; // [rad/s] -} MotorConfig_t; - class Motor { public: enum Error_t { @@ -68,6 +23,51 @@ public: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 }; + enum MotorType_t { + MOTOR_TYPE_HIGH_CURRENT = 0, + // MOTOR_TYPE_LOW_CURRENT = 1, //Not yet implemented + MOTOR_TYPE_GIMBAL = 2 + }; + + struct Iph_BC_t { + float phB; + float phC; + }; + + struct CurrentControl_t{ + float p_gain; // [V/A] + float i_gain; // [V/As] + float v_current_control_integral_d; // [V] + float v_current_control_integral_q; // [V] + float Ibus; // DC bus current [A] + // Voltage applied at end of cycle: + float final_v_alpha; // [V] + float final_v_beta; // [V] + float Iq_setpoint; + float Iq_measured; + float max_allowed_current; + }; + + // NOTE: for gimbal motors, all units of A are instead V. + // example: vel_gain is [V/(count/s)] instead of [A/(count/s)] + // example: current_lim and calibration_current will instead determine the maximum voltage applied to the motor. + struct Config_t { + bool pre_calibrated = false; // can be set to true to indicate that all values here are valid + int32_t pole_pairs = 7; + float calibration_current = 10.0f; // [A] + float resistance_calib_max_voltage = 1.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 + 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] + float current_lim = 10.0f; //[A] + // Value used to compute shunt amplifier gains + float requested_current_range = 70.0f; // [A] + float current_control_bandwidth = 1000.0f; // [rad/s] + }; + enum TimingLog_t { TIMING_LOG_GENERAL, TIMING_LOG_ADC_CB_I, @@ -90,7 +90,7 @@ public: Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config); + Config_t& config); bool arm(); void disarm(); @@ -119,7 +119,7 @@ public: const MotorHardwareConfig_t& hw_config_; const GateDriverHardwareConfig_t gate_driver_config_; - MotorConfig_t& config_; + Config_t& config_; Axis* axis_ = nullptr; // set by Axis constructor //private: @@ -144,7 +144,7 @@ public: Iph_BC_t current_meas_ = {0.0f, 0.0f}; Iph_BC_t DC_calib_ = {0.0f, 0.0f}; float phase_current_rev_gain_ = 0.0f; // Reverse gain for ADC to Amps (to be set by DRV8301_setup) - Current_control_t current_control_ = { + CurrentControl_t current_control_ = { .p_gain = 0.0f, // [V/A] should be auto set after resistance and inductance measurement .i_gain = 0.0f, // [V/As] should be auto set after resistance and inductance measurement .v_current_control_integral_d = 0.0f, diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 4e4db160..0165c673 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,6 +109,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast(~static_c #include #include #include +#include #include #include diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp new file mode 100644 index 00000000..f1e41aa5 --- /dev/null +++ b/Firmware/MotorControl/trapTraj.cpp @@ -0,0 +1,94 @@ +#include +#include "odrive_main.h" +#include "utils.h" + +// A sign function where input 0 has positive sign (not 0) +float sign_hard(float val) { + return (std::signbit(val)) ? -1.0f : 1.0f; +} + +// Symbol Description +// Ta, Tv and Td Duration of the stages of the AL profile +// Xi and Vi Adapted initial conditions for the AL profile +// Xf Position set-point +// s Direction (sign) of the trajectory +// Vmax, Amax, Dmax and jmax Kinematic bounds +// Ar, Dr and Vr Reached values of acceleration and velocity + +TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {} + +bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax) { + float dX = Xf - Xi; // Distance to travel + float stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distance + float dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacement + float s = sign_hard(dX - dXstop); // Sign of coast velocity (if any) + Ar_ = s * Amax; // Maximum Acceleration (signed) + Dr_ = -s * Dmax; // Maximum Deceleration (signed) + Vr_ = s * Vmax; // Maximum Velocity (signed) + + // If we start with a speed faster than cruising, then we need to decel instead of accel + // aka "double deceleration move" in the paper + if ((s * Vi) > (s * Vr_)) { + Ar_ = -s * Amax; + } + + // Time to accel/decel to/from Vr (cruise speed) + Ta_ = (Vr_ - Vi) / Ar_; + Td_ = -Vr_ / Dr_; + + // Integral of velocity ramps over the full accel and decel times to get + // minimum displacement required to reach cuising speed + float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_; + + // Are we displacing enough to reach cruising speed? + if (s*dX < s*dXmin) { + // Short move (triangle profile) + Vr_ = s * sqrtf((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_)); + Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); + Td_ = std::max(0.0f, -Vr_ / Dr_); + Tv_ = 0.0f; + } else { + // Long move (trapezoidal profile) + Tv_ = (dX - dXmin) / Vr_; + } + + // Fill in the rest of the values used at evaluation-time + Tf_ = Ta_ + Tv_ + Td_; + Xi_ = Xi; + Xf_ = Xf; + Vi_ = Vi; + yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phase + + return true; +} + +TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) { + Step_t trajStep; + if (t < 0.0f) { // Initial Condition + trajStep.Y = Xi_; + trajStep.Yd = Vi_; + trajStep.Ydd = 0.0f; + } else if (t < Ta_) { // Accelerating + trajStep.Y = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t); + trajStep.Yd = Vi_ + Ar_*t; + trajStep.Ydd = Ar_; + } else if (t < Ta_ + Tv_) { // Coasting + trajStep.Y = yAccel_ + Vr_*(t - Ta_); + trajStep.Yd = Vr_; + trajStep.Ydd = 0.0f; + } else if (t < Tf_) { // Deceleration + float td = t - Tf_; + trajStep.Y = Xf_ + 0.5f*Dr_*SQ(td); + trajStep.Yd = Dr_*td; + trajStep.Ydd = Dr_; + } else if (t >= Tf_) { // Final Condition + trajStep.Y = Xf_; + trajStep.Yd = 0.0f; + trajStep.Ydd = 0.0f; + } else { + // TODO: report error here + } + + return trajStep; +} \ No newline at end of file diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp new file mode 100644 index 00000000..42dac0ef --- /dev/null +++ b/Firmware/MotorControl/trapTraj.hpp @@ -0,0 +1,53 @@ +#ifndef _TRAP_TRAJ_H +#define _TRAP_TRAJ_H + +class TrapezoidalTrajectory { +public: + struct Config_t { + float vel_limit = 20000.0f; // [count/s] + float accel_limit = 5000.0f; // [count/s^2] + float decel_limit = 5000.0f; // [count/s^2] + float A_per_css = 0.0f; // [A/(count/s^2)] + }; + struct Step_t { + float Y; + float Yd; + float Ydd; + }; + + TrapezoidalTrajectory(Config_t& config); + bool planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax); + Step_t eval(float t); + + auto make_protocol_definitions() { + return make_protocol_member_list( + make_protocol_object("config", + make_protocol_property("vel_limit", &config_.vel_limit), + make_protocol_property("accel_limit", &config_.accel_limit), + make_protocol_property("decel_limit", &config_.decel_limit), + make_protocol_property("A_per_css", &config_.A_per_css) + ) + ); + } + + Axis* axis_ = nullptr; // set by Axis constructor + Config_t& config_; + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Vr_; + float Dr_; + + float Ta_; + float Tv_; + float Td_; + float Tf_; + + float yAccel_; +}; + +#endif \ No newline at end of file diff --git a/Firmware/MotorControl/utils.h b/Firmware/MotorControl/utils.h index 1cd63327..0ab35668 100644 --- a/Firmware/MotorControl/utils.h +++ b/Firmware/MotorControl/utils.h @@ -63,6 +63,8 @@ extern "C" { #define MACRO_MAX(x, y) (((x) > (y)) ? (x) : (y)) #define MACRO_MIN(x, y) (((x) < (y)) ? (x) : (y)) +#define SQ(x) ((x) * (x)) + static const float one_by_sqrt3 = 0.57735026919f; static const float two_by_sqrt3 = 1.15470053838f; static const float sqrt3_by_2 = 0.86602540378f; diff --git a/Firmware/Tupfile.lua b/Firmware/Tupfile.lua index fe59828d..0b3d43e3 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -155,6 +155,7 @@ build{ 'MotorControl/encoder.cpp', 'MotorControl/controller.cpp', 'MotorControl/sensorless_estimator.cpp', + 'MotorControl/trapTraj.cpp', 'MotorControl/main.cpp', 'communication/communication.cpp', 'communication/ascii_protocol.cpp', diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 626a7c92..0981cc1d 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -128,6 +128,18 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& axes[motor_number]->controller_.set_current_setpoint(current_setpoint); } + } else if (cmd[0] == 't') { // trapezoidal trajectory + unsigned motor_number; + float goal_point; + int numscan = sscanf(cmd, "t %u %f", &motor_number, &goal_point); + 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 { + axes[motor_number]->controller_.move_to_pos(goal_point); + } + } else if (cmd[0] == 'h') { // Help respond(response_channel, use_checksum, "Please see documentation for more details"); respond(response_channel, use_checksum, ""); diff --git a/docs/ascii-protocol.md b/docs/ascii-protocol.md index 49578af8..951ec214 100644 --- a/docs/ascii-protocol.md +++ b/docs/ascii-protocol.md @@ -24,6 +24,18 @@ command *42 ; comment [new line character] ## Command Reference +#### Motor trajectory command +``` +t motor destination +``` +* `t` for trajectory +* `motor` is the motor number, `0` or `1`. +* `destination` is the goal position, in encoder counts. + +Example: `t 0 -20000` + +For general moving around of the axis, this is the recommended command. + #### Motor Position command ``` p motor position velocity_ff current_ff diff --git a/docs/getting-started.md b/docs/getting-started.md index f4035948..e3682b4c 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -47,7 +47,6 @@ permalink: / ## Wiring up the ODrive -
Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disastrous oscillations or runaway.
@@ -65,11 +64,9 @@ Connect the encoder(s) to J4. The A,B phases are required, and the Z (index puls ![Image of ODrive all hooked up](https://docs.google.com/drawings/d/e/2PACX-1vTCD0P40Cd-wvD7Fl8UYEaxp3_UL81oI4qUVqrrCJPi6tkJeSs2rsffIXQRpdu6rNZs6-2mRKKYtILG/pub?w=1716&h=1281) ## Downloading and Installing Tools - Most instructions in this guide refer to a utility called `odrivetool`, so you should install that first. ### Windows - 1. Install Python 3. We recommend the Anaconda distribution because it packs a lot of useful scientific tools, however you can also install the standalone python. * __Anaconda__: Download the installer from [here](https://www.anaconda.com/download/#windows). Execute the downloaded file and follow the instructions. * __Standalone Python__: Download the installer from [here](https://www.python.org/downloads/). Execute the downloaded file and follow the instructions. @@ -121,7 +118,6 @@ Try step 5 again ### Linux - 1. [Install Python 3](https://www.python.org/downloads/). 2. Install the ODrive tools by opening a terminal and typing `pip install odrive` Enter 3. Set up USB permissions @@ -133,10 +129,10 @@ Try step 5 again ## Firmware #### ODrive v3.5 and later -Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue. +Your board should come preflashed with firmware. If you run into problems, follow the instructions [here](odrivetool.md#device-firmware-update) on the DFU procedure before you continue. #### ODrive v3.4 and earlier -Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue. +Your board does **not** come preflashed with any firmware. Follow the instructions [here](odrivetool.md#device-firmware-update) on the STP Link procedure before you continue. ## Start `odrivetool` To launch the main interactive ODrive tool, type `odrivetool` Enter. Connect your ODrive and wait for the tool to find it. Now you can, for instance type `odrv0.vbus_voltage` Enter to inpect the boards main supply voltage. @@ -258,9 +254,7 @@ You can now control the current with `odrv0.axis0.controller.current_setpoint = *Note: There is no velocity limiting in current control mode. Make sure that you don't overrev the motor, or exceed the max speed for your encoder.* ## What's next? - You can now: - * See what other [commands and parameters](commands.md) are available, including setting tuning parameters for better performance. * Control the ODrive from your own program or hook it up to an existing system through one of it's [interfaces](interfaces.md). * See how you can improve the behavior during the startup procedure, like [bypassing encoder calibration](encoders.md#encoder-with-index-signal). diff --git a/tools/motion_planning/PlanTrap.py b/tools/motion_planning/PlanTrap.py new file mode 100644 index 00000000..129da1e6 --- /dev/null +++ b/tools/motion_planning/PlanTrap.py @@ -0,0 +1,225 @@ +# Copyright (c) 2018 Paul Guénette +# Copyright (c) 2018 Oskar Weigl + +# Permission is hereby granted, free of charge, to any person obtaining a copy +# of this software and associated documentation files (the "Software"), to deal +# in the Software without restriction, including without limitation the rights +# to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +# copies of the Software, and to permit persons to whom the Software is +# furnished to do so, subject to the following conditions: + +# The above copyright notice and this permission notice shall be included in all +# copies or substantial portions of the Software. + +# THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +# IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +# FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +# AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +# LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +# OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +# SOFTWARE. + +# This algorithm is based on: +# FIR filter-based online jerk-constrained trajectory generation +# https://www.researchgate.net/profile/Richard_Bearee/publication/304358769_FIR_filter-based_online_jerk-controlled_trajectory_generation/links/5770ccdd08ae10de639c0ff7/FIR-filter-based-online-jerk-controlled-trajectory-generation.pdf + +import numpy as np +import math +import matplotlib.pyplot as plt +import random + +# Symbol Description +# Ta, Tv and Td Duration of the stages of the AL profile +# Xi and Vi Adapted initial conditions for the AL profile +# Xf Position set-point +# s Direction (sign) of the trajectory +# Vmax, Amax, Dmax and jmax Kinematic bounds +# Ar, Dr and Vr Reached values of acceleration and velocity + +# Test scales: +pos_range = 10000.0 +Vmax_range = 8000.0 +Amax_range = 10000.0 +plot_range = 10000.0 + + +def PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax): + dX = Xf - Xi # Distance to travel + stop_dist = Vi**2 / (2*Dmax) # Minimum stopping distance + dXstop = np.sign(Vi)*stop_dist # Minimum stopping displacement + s = np.sign(dX - dXstop) # Sign of coast velocity (if any) + Ar = s*Amax # Maximum Acceleration (signed) + Dr = -s*Dmax # Maximum Deceleration (signed) + Vr = s*Vmax # Maximum Velocity (signed) + + # If we start with a speed faster than cruising, then we need to decel instead of accel + # aka "double deceleration move" in the paper + if s*Vi > s*Vr: + print("Handbrake!") + Ar = -s*Amax + + # Time to accel/decel to/from Vr (cruise speed) + Ta = (Vr-Vi)/Ar + Td = -Vr/Dr + + # Integral of velocity ramps over the full accel and decel times to get + # minimum displacement required to reach cuising speed + dXmin = Ta*(Vr+Vi)/2.0 + Td*(Vr)/2.0 + + # Are we displacing enough to reach cruising speed? + if s*dX < s*dXmin: + print("Short Move:") + # From paper: + # Vr = s*math.sqrt((-(Vi**2/Ar)-2*dX)/(1/Dr-1/Ar)) + # Simplified for less divisions: + Vr = s*math.sqrt((Dr*Vi**2 + 2*Ar*Dr*dX) / (Dr-Ar)) + Ta = max(0, (Vr - Vi)/Ar) + Td = max(0, -Vr/Dr) + Tv = 0 + else: + print("Long move:") + Tv = (dX - dXmin)/Vr # Coasting time + + Tf = Ta+Tv+Td + + print("Xi: {:.2f}\tXf: {:.2f}\tVi: {:.2f}".format(Xi, Xf, Vi)) + print("Amax: {:.2f}\tVmax: {:.2f}\tDmax: {:.2f}".format(Amax, Vmax, Dmax)) + print("dX: {:.2f}\tdXst: {:.2f}\tdXmin: {:.2f}".format(dX, dXstop, dXmin)) + print("Ar: {:.2f}\tVr: {:.2f}\tDr: {:.2f}".format(Ar, Vr, Dr)) + print("Ta: {:.2f}\tTv: {:.2f}\tTd: {:.2f}".format(Ta, Tv, Td)) + + return (Ar, Vr, Dr, Ta, Tv, Td, Tf) + +def EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf): + # Create the time series and preallocate the position, velocity, and acceleration arrays + t_traj = np.arange(0, Tf+0.1, 1/10000) + y = [None]*len(t_traj) + yd = [None]*len(t_traj) + ydd = [None]*len(t_traj) + + # We only know acceleration (Ar and Dr), so we integrate to create + # the velocity and position curves + y_Accel = Xi + Vi*Ta + 0.5*Ar*Ta**2 + + for i in range(len(t_traj)): + t = t_traj[i] + if t < 0: # Initial conditions + y[i] = Xi + yd[i] = Vi + ydd[i] = 0 + elif t < Ta: # Acceleration + y[i] = Xi + Vi*t + 0.5*Ar*t**2 + yd[i] = Vi + Ar*t + ydd[i] = Ar + elif t < Ta+Tv: # Coasting + y[i] = y_Accel + Vr*(t-Ta) + yd[i] = Vr + ydd[i] = 0 + elif t < Tf: # Deceleration + td = t-Tf + y[i] = Xf + 0*td + 0.5*Dr*td**2 + yd[i] = 0 + Dr*td + ydd[i] = Dr + elif t >= Tf: # Final condition + y[i] = Xf + yd[i] = 0 + ydd[i] = 0 + else: + raise ValueError("t = {} is outside of considered range".format(t)) + + dy = np.diff(y) + dy_max = np.max(np.abs(dy)) + dyd = np.diff(yd) + dyd_max = np.max(np.abs(dyd)) + print("dy_max: {:.2f}\tdyd_max: {:.2f}".format(dy_max, dyd_max)) + + error = False + if dy_max/pos_range > 0.001: + print("---------- Bad Pos Continuity --------------------") + error = True + if dyd_max/Vmax_range > 0.001: + print("---------- Bad Vel Continuity --------------------") + error = True + if abs(Xi-y[0]) > 0.0001: + print("---------- Bad Initial Position --------------------") + error = True + if abs(Xf-y[-1]) > 0.0001: + print("---------- Bad Final Position --------------------") + error = True + if abs(Vi-yd[0]) > 0.0001: + print("---------- Bad Initial Velocity --------------------") + error = True + if abs(yd[-1]) > 0.0001: + print("---------- Bad Final Velocity --------------------") + error = True + + if error: + import ipdb; ipdb.set_trace() + + return (y, yd, ydd, t_traj) + +def graphical_test(): + numRows = 3 + numCols = 5 + fig, axes = plt.subplots(numRows, numCols) + random.seed(3) # Repeatable tests by using specific seed + for x in range(numRows*numCols): + rownow = int(x/numCols) + colnow = x % numCols + print("row: {}, col: {}".format(rownow, colnow)) + + Vmax = random.uniform(0.1*Vmax_range, Vmax_range) + Amax = random.uniform(0.1*Amax_range, Amax_range) + Dmax = Amax + Xf = random.uniform(-pos_range, pos_range) + Xi = random.uniform(-pos_range, pos_range) + if random.random() <= 0.5: + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + else: + Vi = 0 + + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) + + # Plotting + ax1 = axes[rownow, colnow] + # Vel limits (draw first for clearer z-order) + ax1.plot([t[0], t[-1]], [Vmax, Vmax], 'g--') + ax1.plot([t[0], t[-1]], [-Vmax, -Vmax], 'g--') + + ax1.plot(t, Y) # Pos + ax1.plot(t, Yd) # Vel + ax1.plot(0, Xi, 'bo') # Pos Initial + ax1.plot(0, Vi, 'ro') # Vel Initial + ## TODO: pull out Ta+Td+Td from planner for correct plot points + ax1.plot(t[-1]-0.1, Xf, 'b*') # Pos Final + ax1.plot(t[-1]-0.1, 0, 'r*') # Vel Final + + ax1.set_ylim(-plot_range, plot_range) + + print() + + plt.show() + +def large_test(): + random.seed(1) # Repeatable tests by using specific seed + for x in range(100): + print("Test {}".format(x)) + Vmax = random.uniform(0.1*Vmax_range, Vmax_range) + Amax = random.uniform(0.1*Amax_range, Amax_range) + Dmax = Amax + Xf = random.uniform(-pos_range, pos_range) + Xi = random.uniform(-pos_range, pos_range) + if random.random() <= 0.5: + Vi = random.uniform(-Vmax*1.5, Vmax*1.5) + else: + Vi = 0 + + (Ar, Vr, Dr, Ta, Tv, Td, Tf) = PlanTrap(Xf, Xi, Vi, Vmax, Amax, Dmax) + (Y, Yd, Ydd, t) = EvalTrap(Xf, Xi, Vi, Ar, Vr, Dr, Ta, Tv, Td, Tf) + + print() + +if __name__ == '__main__': + large_test() + graphical_test() \ No newline at end of file