diff --git a/CHANGELOG.md b/CHANGELOG.md index 4ff031fb..aecc53f7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,9 +4,16 @@ Please add a note of your changes below this heading if you make a Pull Request. # Unreleased ### Added +* **Trapezoidal Trajectory Planner** * Hook to execute protocol property written callback +* -Wdouble-promotion warning to compilation ### Changed +* Make python tools compatible with python 2.7 (so it can be used with ROS) + * Threading API constructor can't take the daemon parameter, so all thread creation had to be expanded out. + * `TimeoutError` isn't defined, but it makes for more readable code, so I defined it as an OSError subclass. + * `ModuleNotFoundError` is replaced by the older ImportError. + * Print function imported from future * Using new hooks to calculate: * `motor.config.current_control_bandwidth` * This depricates `motor.set_current_control_bandwidth()` @@ -27,8 +34,10 @@ Please add a note of your changes below this heading if you make a Pull Request. * Hardcoded maximum inductance now 2500 uH. ### Fixed +* Incorrect shifting of offset during index callback * Once you got an axis error `ERROR_INVALID_STATE` you could never clear it * Char to int conversion to read motornum on arduino example +* GPIO above #5 would not be used correctly in some cases ## [0.4.2] - 2018-08-04 ### Added diff --git a/Firmware/.vscode/c_cpp_properties.json b/Firmware/.vscode/c_cpp_properties.json index cb9d040e..570c0f29 100644 --- a/Firmware/.vscode/c_cpp_properties.json +++ b/Firmware/.vscode/c_cpp_properties.json @@ -4,6 +4,7 @@ "name": "Win32", "includePath": [ "${workspaceRoot}", + "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", @@ -16,12 +17,8 @@ "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/include", "${workspaceRoot}/Board/v3/Middlewares/Third_Party/FreeRTOS/Source/portable/GCC/ARM_CM4F", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include/c++/6.3.1/backward", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/lib/gcc/arm-none-eabi/6.3.1/include-fixed", - "c:/program files (x86)/gnu tools arm embedded/6 2017-q1-update/arm-none-eabi/include" + "${ARM_GCC_ROOT}/arm-none-eabi/include/**", + "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**" ], "defines": [ "STM32F405xx", @@ -38,10 +35,11 @@ "browse": { "path": [ "${workspaceRoot}", - "C:/Program Files (x86)/GNU Tools ARM Embedded" + "${ARM_GCC_ROOT}" ], "limitSymbolsToIncludedHeaders": true }, + "compilerPath": "${ARM_GCC_ROOT}/bin/arm-none-eabi-gcc.exe -mthumb -mcpu=cortex-m4 -mfpu=fpv4-sp-d16 -mfloat-abi=hard -specs=nosys.specs -specs=nano.specs -u _printf_float -u _scanf_float", "cStandard": "c11", "cppStandard": "c++14" }, @@ -49,6 +47,7 @@ "name": "Linux", "includePath": [ "${workspaceRoot}", + "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", @@ -61,8 +60,8 @@ "${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", "${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include", "${workspaceRoot}/Board/v3/Drivers/CMSIS/Include", - "/usr/lib/gcc/arm-none-eabi/4.9.3/include", - "/usr/lib/arm-none-eabi/include" + "${ARM_GCC_ROOT}/arm-none-eabi/include/**", + "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**" ], "defines": [ "STM32F405xx", @@ -78,8 +77,7 @@ "browse": { "path": [ "${workspaceRoot}", - "/usr/lib/gcc/arm-none-eabi/4.9.3/include", - "/usr/lib/arm-none-eabi/include" + "${ARM_GCC_ROOT}" ], "limitSymbolsToIncludedHeaders": true, "databaseFilename": "" @@ -92,6 +90,7 @@ "name": "Mac", "includePath": [ "${workspaceRoot}", + "${workspaceRoot}/fibre/cpp/include/**", "${workspaceRoot}/MotorControl", "${workspaceRoot}/Drivers/DRV8301", "${workspaceRoot}/Board/v3/Inc", @@ -104,10 +103,8 @@ "${workspaceRoot}/Board/v3/Drivers/STM32F4xx_HAL_Driver/Inc/Legacy", "${workspaceRoot}/Board/v3/Drivers/CMSIS/Device/ST/STM32F4xx/Include", "${workspaceRoot}/Board/v3/Drivers/CMSIS/Include", - "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include", - "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1", - "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/arm-none-eabi/include/c++/6.3.1/arm-none-eabi", - "/usr/local/Caskroom/gcc-arm-embedded/6-2017-q2-update/gcc-arm-none-eabi-6-2017-q2-update/lib/gcc/arm-none-eabi/6.3.1/include" + "${ARM_GCC_ROOT}/arm-none-eabi/include/**", + "${ARM_GCC_ROOT}/lib/gcc/arm-none-eabi/**" ], "defines": [ "STM32F405xx", @@ -123,7 +120,7 @@ "browse": { "path": [ "${workspaceRoot}", - "/usr/local/Caskroom/gcc-arm-embedded/" + "${ARM_GCC_ROOT}" ], "limitSymbolsToIncludedHeaders": true, "databaseFilename": "" diff --git a/Firmware/Board/v3/Src/gpio.c b/Firmware/Board/v3/Src/gpio.c index 031158d5..4276e697 100644 --- a/Firmware/Board/v3/Src/gpio.c +++ b/Firmware/Board/v3/Src/gpio.c @@ -285,6 +285,15 @@ GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){ case 4: return GPIO_4_GPIO_Port; break; #ifdef GPIO_5_GPIO_Port case 5: return GPIO_5_GPIO_Port; break; +#endif +#ifdef GPIO_6_GPIO_Port + case 6: return GPIO_6_GPIO_Port; break; +#endif +#ifdef GPIO_7_GPIO_Port + case 7: return GPIO_7_GPIO_Port; break; +#endif +#ifdef GPIO_8_GPIO_Port + case 8: return GPIO_8_GPIO_Port; break; #endif default: return GPIO_1_GPIO_Port; } @@ -298,6 +307,15 @@ uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){ case 4: return GPIO_4_Pin; break; #ifdef GPIO_5_Pin case 5: return GPIO_5_Pin; break; +#endif +#ifdef GPIO_6_Pin + case 6: return GPIO_6_Pin; break; +#endif +#ifdef GPIO_7_Pin + case 7: return GPIO_7_Pin; break; +#endif +#ifdef GPIO_8_Pin + case 8: return GPIO_8_Pin; break; #endif default: return GPIO_1_Pin; } 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 13fac4bb..e7e697b9 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -97,9 +97,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; @@ -144,9 +144,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 dc379f0c..dca5f74a 100644 --- a/Firmware/MotorControl/motor.cpp +++ b/Firmware/MotorControl/motor.cpp @@ -7,7 +7,7 @@ Motor::Motor(const MotorHardwareConfig_t& hw_config, const GateDriverHardwareConfig_t& gate_driver_config, - MotorConfig_t& config) : + Config_t& config) : hw_config_(hw_config), gate_driver_config_(gate_driver_config), config_(config), @@ -293,10 +293,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; @@ -307,7 +308,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; @@ -315,8 +316,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; @@ -330,23 +331,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 386b0e95..051c0228 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(); @@ -117,7 +117,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: @@ -142,7 +142,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 f242b259..0b3d43e3 100644 --- a/Firmware/Tupfile.lua +++ b/Firmware/Tupfile.lua @@ -89,7 +89,7 @@ FLAGS += '-mthumb' FLAGS += '-mcpu=cortex-m4' FLAGS += '-mfpu=fpv4-sp-d16' FLAGS += '-mfloat-abi=hard' -FLAGS += { '-Wall', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} +FLAGS += { '-Wall', '-Wdouble-promotion', '-Wfloat-conversion', '-fdata-sections', '-ffunction-sections'} -- debug build FLAGS += '-g -gdwarf-2' @@ -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/Firmware/fibre/cpp/include/fibre/protocol.hpp b/Firmware/fibre/cpp/include/fibre/protocol.hpp index ccf225c5..a2d15825 100644 --- a/Firmware/fibre/cpp/include/fibre/protocol.hpp +++ b/Firmware/fibre/cpp/include/fibre/protocol.hpp @@ -592,10 +592,10 @@ private: template struct format_traits_t; -template<> struct format_traits_t { using type = void; - static constexpr const char * fmt = "%f"; - static constexpr const char * fmtp = "%f"; -}; +// template<> struct format_traits_t { using type = void; +// static constexpr const char * fmt = "%f"; +// static constexpr const char * fmtp = "%f"; +// }; template<> struct format_traits_t { using type = void; static constexpr const char * fmt = "%ld"; static constexpr const char * fmtp = "%ld"; @@ -626,8 +626,15 @@ static bool to_string(const T& value, char * buffer, size_t length, int) { snprintf(buffer, length, format_traits_t::fmtp, value); return true; } +// Special case for float because printf promotes float to double, and we get warnings +template +static bool to_string(const float& value, char * buffer, size_t length, int) { + snprintf(buffer, length, "%f", (double)value); + return true; +} + + template -//__attribute__((__unused__)) static bool to_string(const bool& value, char * buffer, size_t length, int) { buffer[0] = value ? '1' : '0'; buffer[1] = 0; @@ -642,7 +649,11 @@ template::type> static bool from_string(const char * buffer, size_t length, T* property, int) { return sscanf(buffer, format_traits_t::fmt, property) == 1; } -//__attribute__((__unused__)) +// Special case for float because printf promotes float to double, and we get warnings +template +static bool from_string(const char * buffer, size_t length, float* property, int) { + return sscanf(buffer, "%f", property) == 1; +} template static bool from_string(const char * buffer, size_t length, bool* property, int) { int val; diff --git a/Firmware/fibre/python/fibre/__init__.py b/Firmware/fibre/python/fibre/__init__.py index 7309c8b2..0b8ed1b4 100644 --- a/Firmware/fibre/python/fibre/__init__.py +++ b/Firmware/fibre/python/fibre/__init__.py @@ -1,5 +1,5 @@ from .discovery import find_any, find_all -from .utils import Event, Logger +from .utils import Event, Logger, TimeoutError from .protocol import ChannelBrokenException, ChannelDamagedException from .shell import launch_shell diff --git a/Firmware/fibre/python/fibre/discovery.py b/Firmware/fibre/python/fibre/discovery.py index f1e62499..a3751633 100644 --- a/Firmware/fibre/python/fibre/discovery.py +++ b/Firmware/fibre/python/fibre/discovery.py @@ -11,7 +11,7 @@ import fibre.protocol import fibre.utils import fibre.remote_object from fibre.utils import Event, Logger -from fibre.protocol import ChannelBrokenException +from fibre.protocol import ChannelBrokenException, TimeoutError # Load all installed transport layers @@ -20,25 +20,25 @@ channel_types = {} try: import fibre.usbbulk_transport channel_types['usb'] = fibre.usbbulk_transport.discover_channels -except ModuleNotFoundError: +except ImportError: pass try: import fibre.serial_transport channel_types['serial'] = fibre.serial_transport.discover_channels -except ModuleNotFoundError: +except ImportError: pass try: import fibre.tcp_transport channel_types['tcp'] = fibre.tcp_transport.discover_channels -except ModuleNotFoundError: +except ImportError: pass try: import fibre.udp_transport channel_types['udp'] = fibre.udp_transport.discover_channels -except ModuleNotFoundError: +except ImportError: pass def noprint(text): @@ -102,9 +102,10 @@ def find_all(path, serial_number, prefix = search_spec.split(':')[0] the_rest = ':'.join(search_spec.split(':')[1:]) if prefix in channel_types: - threading.Thread(target=channel_types[prefix], - args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger), - daemon=True).start() + t = threading.Thread(target=channel_types[prefix], + args=(the_rest, serial_number, did_discover_channel, search_cancellation_token, channel_termination_token, logger)) + t.daemon = True + t.start() else: raise Exception("Invalid path spec \"{}\"".format(search_spec)) diff --git a/Firmware/fibre/python/fibre/protocol.py b/Firmware/fibre/python/fibre/protocol.py index c8814fc1..851ad457 100644 --- a/Firmware/fibre/python/fibre/protocol.py +++ b/Firmware/fibre/python/fibre/protocol.py @@ -6,7 +6,7 @@ import sys import threading import traceback #import fibre.utils -from fibre.utils import Event, wait_any +from fibre.utils import Event, wait_any, TimeoutError import abc if sys.version_info >= (3, 4): @@ -65,7 +65,6 @@ def calc_crc16(remainder, value): #print(hex(calc_crc8(0x12, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) #print(hex(calc_crc16(0xfeef, [1, 2, 3, 4, 5, 0x10, 0x13, 0x37]))) - class DeviceInitException(Exception): pass @@ -256,7 +255,9 @@ class Channel(PacketSink): self._logger.debug("receiver thread is exiting: " + traceback.format_exc()) finally: self._channel_broken.set() - threading.Thread(target=receiver_thread, daemon=True).start() + t = threading.Thread(target=receiver_thread) + t.daemon = True + t.start() def remote_endpoint_operation(self, endpoint_id, input, expect_ack, output_length): if input is None: diff --git a/Firmware/fibre/python/fibre/serial_transport.py b/Firmware/fibre/python/fibre/serial_transport.py index b8141633..931a9415 100644 --- a/Firmware/fibre/python/fibre/serial_transport.py +++ b/Firmware/fibre/python/fibre/serial_transport.py @@ -10,6 +10,7 @@ import traceback import serial import serial.tools.list_ports import fibre +from fibre.utils import TimeoutError # TODO: make this customizable DEFAULT_BAUDRATE = 115200 diff --git a/Firmware/fibre/python/fibre/tcp_transport.py b/Firmware/fibre/python/fibre/tcp_transport.py index 5d7ee3ef..5cfb1692 100644 --- a/Firmware/fibre/python/fibre/tcp_transport.py +++ b/Firmware/fibre/python/fibre/tcp_transport.py @@ -4,7 +4,7 @@ import socket import time import traceback import fibre.protocol -from fibre.utils import wait_any +from fibre.utils import wait_any, TimeoutError def noprint(x): pass diff --git a/Firmware/fibre/python/fibre/usbbulk_transport.py b/Firmware/fibre/python/fibre/usbbulk_transport.py index c724e55a..dd32b106 100644 --- a/Firmware/fibre/python/fibre/usbbulk_transport.py +++ b/Firmware/fibre/python/fibre/usbbulk_transport.py @@ -8,6 +8,7 @@ import time import fibre.protocol import traceback import platform +from fibre.utils import TimeoutError # Currently we identify fibre-enabled devices by VID,PID # TODO: identify by USB descriptors diff --git a/Firmware/fibre/python/fibre/utils.py b/Firmware/fibre/python/fibre/utils.py index 511bd436..d0bbe946 100644 --- a/Firmware/fibre/python/fibre/utils.py +++ b/Firmware/fibre/python/fibre/utils.py @@ -12,10 +12,16 @@ try: # TODO: we should win32console anyway so we could just omit colorama import colorama colorama.init() -except ModuleNotFoundError: +except ImportError: print("Could not init terminal features.") sys.stdout.flush() pass + +if sys.version_info < (3, 3): + class TimeoutError(Exception): + pass +else: + TimeoutError = TimeoutError def get_serial_number_str(device): if hasattr(device, 'serial_number'): @@ -92,7 +98,10 @@ class Event(): def delayed_trigger(): if not self.wait(timeout=timeout): self.set() - threading.Thread(target=delayed_trigger, daemon=True).start() + threading.Thread(target=delayed_trigger) + t.daemon = True + t.start() + def wait_any(timeout=None, *events): """ 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/configuring-vscode.md b/docs/configuring-vscode.md index 17f7e702..bfb3261b 100644 --- a/docs/configuring-vscode.md +++ b/docs/configuring-vscode.md @@ -16,6 +16,7 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis * Include Autocomplete * Path Autocomplete * Auto Comment Blocks +1. Create an environment variable named `ARM_GCC_ROOT` whose value is the location of the `GNU Arm Embedded Toolchain` (.e.g `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`) that you installed in the prerequisites section of the developer's guide. 1. Restart VSCode 1. Open the VSCode Workspace file, which is located in the root of the ODrive repository. It is called `ODrive_Workspace.code-workspace`. The first time you open it, VSCode will install some dependencies. If it fails, you may need to [change your proxy settings](https://code.visualstudio.com/docs/getstarted/settings). diff --git a/docs/developer-guide.md b/docs/developer-guide.md index 2a3cfde6..30a20543 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -32,7 +32,7 @@ The recommended tools for ODrive development are: * **make**: Used to invoke tup * **Tup**: The build system used to invoke the compile commands - * **ARM GNU Compiler**: For cross-comiling code + * **ARM GNU Compiler**: For cross-compiling code * **ARM GDB**: For debugging the code and stepping through on the device * **OpenOCD**: For flashing the ODrive with the STLink/v2 programmer * **Python**: For running the Python tools @@ -80,7 +80,7 @@ __Note__: make sure these programs are not only installed but also added to your Some instructions in this document may assume that you're using a bash command prompt, such as the Windows 10 built-in bash or [Git](https://git-scm.com/download/win) bash. -* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads) +* [ARM compiler](https://developer.arm.com/open-source/gnu-toolchain/gnu-rm/downloads) __Note__: After installing, create an environment variable named `ARM_GCC_ROOT` whose value is the path you installed to. e.g. `C:\Program Files (x86)\GNU Tools Arm Embedded\7 2018-q2-update`. This variable is used to locate include files for the c/c++ Visual Studio Code extension. * [Tup](http://gittup.org/tup/index.html) * [Make for Windows](http://gnuwin32.sourceforge.net/packages/make.htm) * [OpenOCD](http://gnuarmeclipse.github.io/openocd/install/). Also follow the instructions on the ST-LINK/V2 drivers. diff --git a/docs/getting-started.md b/docs/getting-started.md index 994009f1..e3682b4c 100644 --- a/docs/getting-started.md +++ b/docs/getting-started.md @@ -12,6 +12,7 @@ permalink: / - [Hardware Requirements](#hardware-requirements) - [Wiring up the ODrive](#wiring-up-the-odrive) - [Downloading and Installing Tools](#downloading-and-installing-tools) +- [Firmware](#firmware) - [Start `odrivetool`](#start-odrivetool) - [Configure M0](#configure-m0) - [Position control of M0](#position-control-of-m0) @@ -21,8 +22,7 @@ permalink: / ## Hardware Requirements -You will need: - +### You will need: * One or two [brushless motors](https://docs.google.com/spreadsheets/d/12vzz7XVEK6YNIOqH0jAz51F5VUpc-lJEs3mmkWP1H4Y). It is fine, even recommended, to start testing with just a single motor and encoder. * One or two [quadrature incremental encoder(s)](encoders) * A power resistor. A good starting point would be the 50W resistor included with your ODrive. @@ -47,25 +47,26 @@ You will need: ## Wiring up the ODrive -
-Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disasterous oscillations or runaway. +Make sure you have a good mechanical connection between the encoder and the motor, slip can cause disastrous oscillations or runaway.
All non-power I/O is 3.3V output and 5V tolerant on input, on ODrive v3.3 and newer. -1. Wire up the motor phases into the 3-phase screw terminals, and the power resistor to the AUX terminal. Wire up the power source to the DC terminal, make sure to pay attention to the polarity. Do not apply power just yet. +### Wiring up the motors +* Connect the motor phases into the 3-phase screw terminals. It is not recommended to use a clip-on connector such as an alligator clip, as this can cause issues with the phase resistance/inductance measurements. +* Connect the power source to the DC terminals. Make sure to pay attention to the polarity. +* Do not apply power just yet. -2. Wire up the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. +### Wiring up the encoders +Connect the encoder(s) to J4. The A,B phases are required, and the Z (index pulse) is optional. The A,B and Z lines have 3.3k pull up resistors, for use with open-drain encoder outputs. For single ended push-pull signals with weak drive current (\<4mA), you may want to desolder the pull-ups. ![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. @@ -117,23 +118,24 @@ 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. __Linux__: set up USB permissions +3. Set up USB permissions ```bash echo 'SUBSYSTEM=="usb", ATTR{idVendor}=="1209", ATTR{idProduct}=="0d[0-9][0-9]", MODE="0666"' | sudo tee /etc/udev/rules.d/91-odrive.rules sudo udevadm control --reload-rules sudo udevadm trigger ``` +## 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. + +#### 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. + ## Start `odrivetool` - -
__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.
- -
__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.
- -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. +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. It should look something like this: ```text @@ -148,63 +150,77 @@ Out[1]: 11.97055721282959 The tool you're looking at is a fully capable Python command prompt, so you can type any valid python code. -You can read more about the odrivetool [here](odrivetool.md). +You can read more about `odrivetool` [here](odrivetool.md). ## Configure M0 -
Read this section carefully, else you risk breaking something.
There is a [separate guide](hoverboard.md) specifically for hoverboard motors.
### 1. Set the limits:
Wait, how do I set these?
+ In the previous step we started `odrivetool`. In there, you can assign variables directly by name. For instance, to set the current limit of M0 to 10A you would type: `odrv0.axis0.motor.config.current_lim = 10` Enter
-* The current limit: `odrv0.axis0.motor.config.current_lim` [A]. The default current limit, for safety reasons, is set to 10A. This is quite weak, and good for making sure the drive is stable. Once you have tuned the drive, you can increase this to 75A to get some performance. Note that above 75A, you must change the current amplifier gains. You do this by requesting a different current range. i.e. for 90A on M0: 'odrv0.axis0.motor.config.requested_current_range = 90' [A], then save the configeration and reboot as the gains are written out to the DRV (MOSFET driver) only during startup. - * Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current. -
Ok so tell me how it actually works then...
- The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time. +#### Current limit +`odrv0.axis0.motor.config.current_lim` [A]. +The default current limit, for safety reasons, is set to 10A. This is quite weak, but good for making sure the drive is stable. Once you have tuned the oDrive, you can increase this to 75A to increase performance. Note that above 75A, you must change the current amplifier gains. You do this by requesting a different current range. i.e. for 90A on M0: `odrv0.axis0.motor.config.requested_current_range = 90` [A], then save the configuration and reboot as the gains are written out to the DRV (MOSFET driver) only during startup. - The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other. +*Note: The motor current and the current drawn from the power supply is not the same in general. You should not look at the power supply current to see what is going on with the motor current.* + +
Ok, so tell me how it actually works then...
+The current in the motor is only connected to the current in the power supply _sometimes_ and other times it just cycles out of one phase and back in the other. This is what the modulation magnitude is (sometimes people call this duty cycle, but that's a bit confusing because we use SVM not straight PWM). When the modulation magnitude is 0, the average voltage seen across the motor phases is 0, and the motor current is never connected to the power supply. When the magnitude is 100%, it is always connected, and at 50% it's connected half the time, and cycled in just the motor half the time. + +The largest effect on modulation magnitude is speed. There are other smaller factors, but in general: if the motor is still it's not unreasonable to have 50A in the motor from 5A on the power supply. When the motor is spinning close to top speed, the power supply current and the motor current will be somewhat close to each other.
-* The velocity limit: `odrv0.axis0.controller.config.vel_limit` [counts/s]. The motor will be limited to this speed; again the default value is quite slow. -* You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continously when the motor is stationary. If you are using a small motor (i.e. 15A current rated) you may need to reduce `calibration_current` to a value smaller than the default. +#### Velocity limit +`odrv0.axis0.controller.config.vel_limit` [counts/s]. +The motor will be limited to this speed. Again the default value is quite slow. +#### Calibration current +You can change `odrv0.axis0.motor.config.calibration_current` [A] to the largest value you feel comfortable leaving running through the motor continuously when the motor is stationary. If you are using a small motor (i.e. 15A current rated) you may need to reduce `calibration_current` to a value smaller than the default. -### 2. Set other hardware parameters: +### 2. Set other hardware parameters +`odrv0.config.brake_resistance` [Ohm] +This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. Note that there may be some extra resistance in your wiring and in the screw terminals, so if you are getting issues while braking you may want to increase this parameter by around 0.05 ohm. + +`odrv0.axis0.motor.config.pole_pairs` +This is the number of **magnet poles** in the rotor, **divided by two**. To find this, you can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ +If you can't see them, try sliding a magnet around the rotor, and counting how many times it stops. This will be the number of **pole pairs**. If you use a magnetic piece of metal instead of a magnet, you will get the number of **magnet poles**. +`odrv0.axis0.motor.config.motor_type` +This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and gimbal motors (`MOTOR_TYPE_GIMBAL`). - * `odrv0.config.brake_resistance` [Ohm]: This is the resistance of the brake resistor. If you are not using it, you may set it to `0`. Note that there may be some extra resistance in your wiring and in the screw terminals, so if you are getting issues while braking you may want to increase this parameter by around 0.05 ohm. - * `odrv0.axis0.motor.config.pole_pairs`: This is the number of **magnet poles** in the rotor, **divided by two**. You can simply count the number of permanent magnets in the rotor, if you can see them. _Note: this is not the same as the number of coils in the stator._ - * `odrv0.axis0.motor.config.motor_type`: This is the type of motor being used. Currently two types of motors are supported: High-current motors (`MOTOR_TYPE_HIGH_CURRENT`) and Gimbal motors (`MOTOR_TYPE_GIMBAL`). +
Which motor_type to choose?
-
Which `motor_type` to choose?
- If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive. +If you're using a regular hobby brushless motor like [this](https://hobbyking.com/en_us/turnigy-aerodrive-sk3-5065-236kv-brushless-outrunner-motor.html) one, you should set `motor_mode` to `MOTOR_TYPE_HIGH_CURRENT`. For low-current gimbal motors like [this](https://hobbyking.com/en_us/turnigy-hd-5208-brushless-gimbal-motor-bldc.html) one, you should choose `MOTOR_TYPE_GIMBAL`. Do not use `MOTOR_TYPE_GIMBAL` on a motor that is not a gimbal motor, as it may overheat the motor or the ODrive. - **Further detail:** - If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. - If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (omega * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. - If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. -
+**Further detail:** +If 100's of mA of current noise is "small" for you, you can choose `MOTOR_TYPE_HIGH_CURRENT`. +If 100's of mA of current noise is "large" for you, and you do not intend to spin the motor very fast (Ω * L << R), and the motor is fairly large resistance (1 ohm or larger), you can chose `MOTOR_TYPE_GIMBAL`. +If 100's of mA current noise is "large" for you, _and_ you intend to spin the motor fast, then you need to replace the shunt resistors on the ODrive. - Note: When using gimbal motors, `current_lim` and `calibration_current` actually mean "voltage limit" and "calibration voltage", since we don't use current feedback. This means that if you set it to 10, it means 10V, despite the name of the parameter. +

- * _if using encoder_: `odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution (CPR). This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. - * _if not using encoder_: - * If you wish to run in sensorless mode, please see [Setting up Sensorless](commands.md#setting-up-sensorless). - * If you are using hall sensor feedback, please see the [hoverboard motor example](hoverboard.md). +*Note: When using gimbal motors,* `current_lim` *and* `calibration_current` *actually mean "voltage limit" and "calibration voltage", since we don't use current feedback. This means that if you set it to 10, it means 10V, despite the name of the parameter.* + +#### If using encoder +`odrv0.axis0.encoder.config.cpr`: Encoder Count Per Revolution [CPR] +This is 4x the Pulse Per Revolution (PPR) value. Usually this is indicated in the datasheet of your encoder. +#### If not using encoder +* If you wish to run in sensorless mode, please see [Setting up sensorless](commands.md#setting-up-sensorless). +* If you are using hall sensor feedback, please see the [hoverboard motor example](hoverboard.md). -### 3. Save configuration. -You can save all `.config` parameters to persistent memory such that the ODrive remembers them between power cycles. +### 3. Save configuration +You can save all `.config` parameters to persistent memory so the ODrive remembers them between power cycles. * `odrv0.save_configuration()` Enter. Due to a [known issue](https://github.com/madcowswe/ODrive/issues/183) it is strongly recommended that you reboot following every save of your configuration using `odrv0.reboot()`. ## Position control of M0 - -Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, so feel free to replace read "axis1" wherever it says "axis0". +Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, so feel free to substitute `axis0` wherever it says `axis0`. 1. Type `odrv0.axis0.requested_state = AXIS_STATE_FULL_CALIBRATION_SEQUENCE` Enter. After about 2 seconds should hear a beep. Then the motor will turn slowly in one direction for a few seconds, then back in the other direction. @@ -214,12 +230,13 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, The startup procedure is demonstrated [here](https://www.youtube.com/watch?v=VCX1bA2xnuY). - **Note**: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initalisation, but in the current software the default behaviour is not like that. + *Note: the rotor must be allowed to rotate without any biased load during startup. That means mass and weak friction loads are fine, but gravity or spring loads are not okay. Also note that in the video, the motors spin after initialization, but in the current software the default behaviour is not like that.*
Help, something isn't working!
+ Check the encoder wiring and that the encoder is firmly connected to the motor. Check the value of `hex(odrv0.axis0.error)` and then refer to the [error code documentation](troubleshooting.md#error-codes) for details. - Once you have understood the error and fixed its cause, you may clear the error state (`odrv0.axis0.error = 0` Enter) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error`). + Once you understand the error and have fixed its cause, you may clear the error state with (`odrv0.axis0.error = 0` Enter) and retry. You may also need to clear the error state of other subcomponents (e.g. `odrv0.axis0.motor.error = 0`).
2. Type `odrv0.axis0.requested_state = AXIS_STATE_CLOSED_LOOP_CONTROL` Enter. From now on the ODrive will try to hold the motor's position. If you try to turn it by hand, it will fight you gently. That is unless you bump up `odrv0.axis0.motor.config.current_lim`, in which case it will fight you more fiercely. @@ -227,13 +244,17 @@ Let's get motor 0 up and running. The procedure for motor 1 is exactly the same, ### Other control modes The ODrive also supports velocity control and current (torque) control. -* **Velocity control**: Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL`. You can now control the velocity with `odrv0.axis0.controller.vel_setpoint = 5000`. Units are counts/s. -* **Current control**: Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`. You can now control the current with `odrv0.axis0.controller.current_setpoint = 3`. Units are A. **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. +#### Velocity control +Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL`. +You can now control the velocity with `odrv0.axis0.controller.vel_setpoint = 5000` [count/s]. +#### Current control +Set `odrv0.axis0.controller.config.control_mode = CTRL_MODE_CURRENT_CONTROL`. +You can now control the current with `odrv0.axis0.controller.current_setpoint = 3` [A]. + +*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/docs/odrivetool.md b/docs/odrivetool.md index 6041928d..01670257 100644 --- a/docs/odrivetool.md +++ b/docs/odrivetool.md @@ -143,36 +143,14 @@ This procedure is only necessary for ODrive v3.4 or earlier. You will need an ST Power up the ODrive. 4. Open up a terminal and navigate to the directory where the firmware is. 5. Run the following command (replace `ODriveFirmware_v3.4-24V.elf` with the name of your firmware file): - ``` -~/Downloads $ openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c reset\ halt -c flash\ write_image\ erase\ ODriveFirmware_v3.4-24V.elf -c reset\ run -c exit -Open On-Chip Debugger 0.10.0 -Licensed under GNU GPL v2 -For bug reports, read - http://openocd.org/doc/doxygen/bugs.html -Info : auto-selecting first available session transport "hla_swd". To override use 'transport select '. -Info : The selected transport took over low-level target control. The results might differ compared to plain JTAG/SWD -adapter speed: 2000 kHz -adapter_nsrst_delay: 100 -none separate -Info : Unable to match requested speed 2000 kHz, using 1800 kHz -Info : Unable to match requested speed 2000 kHz, using 1800 kHz -Info : clock speed 1800 kHz -Info : STLINK v2 JTAG v17 API v2 SWIM v4 VID 0x0483 PID 0x3748 -Info : using stlink api v2 -Info : Target voltage: 3.236027 -Info : stm32f4x.cpu: hardware has 6 breakpoints, 4 watchpoints -adapter speed: 2000 kHz -target halted due to debug-request, current mode: Thread -xPSR: 0x01000000 pc: 0x08009224 msp: 0x20020000 -auto erase enabled -Info : device id = 0x10076413 -Info : flash size = 1024kbytes -target halted due to breakpoint, current mode: Thread -xPSR: 0x61000000 pc: 0x20000046 msp: 0x20020000 -Warn : no flash bank found for address 10000000 +``` +openocd -f interface/stlink-v2.cfg -f target/stm32f4x.cfg -c init -c "reset halt" -c "flash write_image erase ODriveFirmware_v3.4-24V.elf" -c "reset run" -c exit +``` + +If everything worked correctly, you should see something similar to this towards the end of the printout: +``` wrote 262144 bytes from file ODriveFirmware_v3.4-24V.elf in 10.194110s (25.113 KiB/s) -adapter speed: 2000 kHz - ``` +``` If something doesn't work, make sure `openocd` is in your `PATH` variable, check that the wires are connected properly and try with elevated privileges. 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 diff --git a/tools/odrive/dfu.py b/tools/odrive/dfu.py index cf81fba6..6283b81a 100755 --- a/tools/odrive/dfu.py +++ b/tools/odrive/dfu.py @@ -3,6 +3,7 @@ Tool for flashing .hex files to the ODrive via the STM built-in USB DFU mode. """ +from __future__ import print_function import argparse import sys import time @@ -206,7 +207,7 @@ def show_deferred_message(message, cancellation_token): time.sleep(1) if not cancellation_token.is_set(): print(message) - t = threading.Thread(target=show_message_thread, args=(message, cancellation_token), daemon=True) + t = threading.Thread(target=show_message_thread, args=(message, cancellation_token)) t.daemon = True t.start() @@ -436,7 +437,10 @@ def launch_dfu(args, logger, cancellation_token): def find_device_in_dfu_mode_thread(): devices[0] = find_device_in_dfu_mode(serial_number, find_odrive_cancellation_token) find_odrive_cancellation_token.set() - threading.Thread(target=find_device_in_dfu_mode_thread, daemon=True).start() + t = threading.Thread(target=find_device_in_dfu_mode_thread) + t.daemon = True + t.start() + # Scan for ODrives not in DFU mode # We only scan on USB because DFU is only implemented over USB diff --git a/tools/odrive/tests.py b/tools/odrive/tests.py index eeb5dcec..264fa62b 100644 --- a/tools/odrive/tests.py +++ b/tools/odrive/tests.py @@ -1,4 +1,4 @@ - +from __future__ import print_function import subprocess import shlex import math diff --git a/tools/odrive/utils.py b/tools/odrive/utils.py index 531c72d4..0dde8387 100755 --- a/tools/odrive/utils.py +++ b/tools/odrive/utils.py @@ -1,3 +1,4 @@ +from __future__ import print_function import sys import time @@ -12,7 +13,7 @@ try: import win32console import colorama colorama.init() -except ModuleNotFoundError: +except ImportError: print("Could not init terminal features.") print("Refer to install instructions at http://docs.odriverobotics.com/#downloading-and-installing-tools") sys.stdout.flush() @@ -72,8 +73,14 @@ def start_liveplotter(get_var_callback): fig.canvas.draw() fig.canvas.start_event_loop(1/plot_rate) - threading.Thread(target=fetch_data, daemon=True).start() - threading.Thread(target=plot_data, daemon=True).start() + fetch_t = threading.Thread(target=fetch_data) + fetch_t.daemon = True + fetch_t.start() + + plot_t = threading.Thread(target=plot_data) + plot_t.daemon = True + plot_t.start() + return cancellation_token; #plot_data() diff --git a/tools/run_tests.py b/tools/run_tests.py index 2e1359d6..357ff0bc 100755 --- a/tools/run_tests.py +++ b/tools/run_tests.py @@ -35,7 +35,8 @@ def for_all_parallel(objects, get_name, callback): # Start a thread for each element in the list all_threads = [] for element in objects: - thread = threading.Thread(target=run_callback, args=(element,), daemon=True) + thread = threading.Thread(target=run_callback, args=(element,)) + thread.daemon = True thread.start() all_threads.append(thread) diff --git a/tools/setup.py b/tools/setup.py index fcdea0bd..b2cfeb74 100644 --- a/tools/setup.py +++ b/tools/setup.py @@ -48,6 +48,10 @@ from setuptools import setup import os import sys +if sys.version_info < (3, 3): + import exceptions + PermissionError = exceptions.OSError + creating_package = "sdist" in sys.argv # Load version from Git tag @@ -78,7 +82,10 @@ if creating_package: fibre_link = os.path.join(os.path.dirname( os.path.realpath(__file__)), "fibre") if not os.path.exists(fibre_link): - os.symlink(fibre_src, fibre_link, True) + if sys.version_info > (3, 3): + os.symlink(fibre_src, fibre_link, target_is_directory=True) + else: + os.symlink(fibre_src, fibre_link) # TODO: find a better place for this if not creating_package: