diff --git a/CHANGELOG.md b/CHANGELOG.md index 47e41b83..f671d0f7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,12 +19,12 @@ Please add a note of your changes below this heading if you make a Pull Request. * Gain scheduling for anti-hunt when close to 0 position error * Velocity Limiting in Current Control mode according to `vel_limit` and `vel_gain` * Regen current limiting according to `max_regen_limit`, in Amps -* DC Bus hard current limiting according to `power_supply_min_current` and `power_supply_max_current` +* DC Bus hard current limiting according to `dc_max_negative_current` and `dc_max_positive_current` * Unit Testing with Doctest has been started for select algorithms, see [Firmware/Tests/test_runner.cpp](Firmware/Tests/test_runner.cpp) * Added support for Flylint VSCode Extension for static code analysis * Using an STM32F405 .svd file allows CortexDebug to view registers during debugging * Added scripts for building via docker. -* Brake resistor logic now attempts to clamp voltage according to `odrv.config.nominal_voltage` +* Brake resistor logic now attempts to clamp voltage according to `odrv.config.dc_bus_overvoltage_ramp_start` and `odrv.config.dc_bus_overvoltage_ramp_end` ### Changed * Changed ratiometric `motor.config.current_lim_tolerance` to absolute `motor.config.current_lim_margin` @@ -38,6 +38,7 @@ Please add a note of your changes below this heading if you make a Pull Request. * Fix IPython `RuntimeWarning` that would occur every time `odrivetool` was started. * Reboot on `erase_configuration()`. This avoids unexpected behavior of a subsequent `save_configuration()` call, since the configuration is only erased from NVM, not from RAM. * Change `motor.get_inverter_temp()` to use a property which was already being sampled at `motor.inverter_temp` +* Fixed a numerical issue in the trajectory planner that could cause sudden jumps of the position setpoint # Releases ## [0.4.11] - 2019-07-25 diff --git a/Firmware/.vscode/launch.json b/Firmware/.vscode/launch.json index 48fecec2..5107086e 100644 --- a/Firmware/.vscode/launch.json +++ b/Firmware/.vscode/launch.json @@ -30,6 +30,26 @@ "interface/stlink-v2.cfg", "target/stm32f4x_stlink.cfg", ], + "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd", + "cwd": "${workspaceRoot}" + }, + { + // For the Cortex-Debug extension + // ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\"" + "type": "cortex-debug", + "servertype": "external", + "gdbTarget": "localhost:3333", + "preLaunchCommands": [ + "load" + ], + "request": "launch", + "name": "Debug ODrive via external server", + "executable": "${workspaceRoot}/build/ODriveFirmware.elf", + "configFiles": [ + "interface/stlink-v2.cfg", + "target/stm32f4x_stlink.cfg", + ], + "svdFile": "${workspaceRoot}/Board/v3/STM32F40x.svd", "cwd": "${workspaceRoot}" }, ] diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index 53b9925d..b68ef01b 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -166,21 +166,6 @@ bool Axis::do_checks() { if (!(vbus_voltage <= board_config.dc_bus_overvoltage_trip_level)) error_ |= ERROR_DC_BUS_OVER_VOLTAGE; - // This is the same math that's used in update_brake_current(). Should we calculate IBus globally? - float Ibus_sum = 0.0f; - for (size_t i = 0; i < AXIS_COUNT; ++i) { - if (axes[i]->motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { - Ibus_sum += axes[i]->motor_.current_control_.Ibus; - } - } - - if (Ibus_sum > board_config.power_supply_max_current) { - error_ |= ERROR_DC_BUS_OVER_CURRENT; - } - if (Ibus_sum < board_config.power_supply_min_current) { - error_ |= ERROR_DC_BUS_UNDER_CURRENT; - } - // Sub-components should use set_error which will propegate to this error_ motor_.do_checks(); // encoder_.do_checks(); diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index c5a87a67..5b489a93 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -24,8 +24,6 @@ public: ERROR_MIN_ENDSTOP_PRESSED = 0x1000, ERROR_MAX_ENDSTOP_PRESSED = 0x2000, ERROR_ESTOP_REQUESTED = 0x4000, - ERROR_DC_BUS_UNDER_CURRENT = 0x8000, // too much current pushed into the power supply - ERROR_DC_BUS_OVER_CURRENT = 0x10000, // too much current pulled out of the power supply ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000, // the min endstop was not enabled during homing }; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index 3b450277..f4b7f84b 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -110,7 +110,8 @@ bool Controller::anticogging_calibration(float pos_estimate, float vel_estimate) } void Controller::update_filter_gains() { - input_filter_ki_ = 2.0f * config_.input_filter_bandwidth; // basic conversion to discrete time + float bandwidth = std::min(config_.input_filter_bandwidth, 0.25f * current_meas_hz); + input_filter_ki_ = 2.0f * bandwidth; // basic conversion to discrete time input_filter_kp_ = 0.25f * (input_filter_ki_ * input_filter_ki_); // Critically damped } @@ -175,7 +176,7 @@ bool Controller::update(float* current_setpoint_output) { float delta_vel = input_vel_ - vel_setpoint_; // Vel error float accel = input_filter_kp_*delta_pos + input_filter_ki_*delta_vel; // Feedback current_setpoint_ = accel * config_.inertia; // Accel - vel_setpoint_ += std::clamp(current_meas_period * accel, 2.0f * std::abs(delta_vel), -2.0f * std::abs(delta_vel)); // delta vel + vel_setpoint_ += current_meas_period * accel; // delta vel pos_setpoint_ += current_meas_period * vel_setpoint_; // Delta pos } break; case INPUT_MODE_MIRROR: { diff --git a/Firmware/MotorControl/encoder.cpp b/Firmware/MotorControl/encoder.cpp index dcb1bbf9..72986105 100644 --- a/Firmware/MotorControl/encoder.cpp +++ b/Firmware/MotorControl/encoder.cpp @@ -3,7 +3,7 @@ Encoder::Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config, Motor::Config_t motor_config) : + Config_t& config, const Motor::Config_t& motor_config) : hw_config_(hw_config), config_(config) { diff --git a/Firmware/MotorControl/encoder.hpp b/Firmware/MotorControl/encoder.hpp index 872bf810..28830c38 100644 --- a/Firmware/MotorControl/encoder.hpp +++ b/Firmware/MotorControl/encoder.hpp @@ -56,7 +56,7 @@ public: }; Encoder(const EncoderHardwareConfig_t& hw_config, - Config_t& config, Motor::Config_t motor_config); + Config_t& config, const Motor::Config_t& motor_config); void setup(); void set_error(Error_t error); diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index 14a59b4d..8236d39e 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -35,7 +35,9 @@ const float adc_ref_voltage = 3.3f; // This value is updated by the DC-bus reading ADC. // Arbitrary non-zero inital value to avoid division by zero if ADC reading is late float vbus_voltage = 12.0f; +float ibus_ = 0.0f; // exposed for monitoring only bool brake_resistor_armed = false; +bool brake_resistor_saturated = false; /* Private constant data -----------------------------------------------------*/ static const GPIO_TypeDef* GPIOs_to_samp[] = { GPIOA, GPIOB, GPIOC }; static const int num_GPIO = sizeof(GPIOs_to_samp) / sizeof(GPIOs_to_samp[0]); @@ -593,19 +595,43 @@ void update_brake_current() { // Don't start braking until -Ibus > regen_current_allowed float brake_current = -Ibus_sum - board_config.max_regen_current; - float brake_duty = brake_current * std::abs(board_config.brake_resistance) / vbus_voltage; - brake_duty += std::max((vbus_voltage - board_config.nominal_voltage) / (board_config.dc_bus_overvoltage_trip_level / 0.9f - board_config.nominal_voltage), 0.0f); - - // Clamp the duty cycle - brake_duty = std::clamp(brake_duty, 0.0f, 0.9f); + float brake_duty = brake_current * board_config.brake_resistance / vbus_voltage; + + if (board_config.enable_dc_bus_overvoltage_ramp && (board_config.dc_bus_overvoltage_ramp_start < board_config.dc_bus_overvoltage_ramp_end)) { + brake_duty += std::fmax((vbus_voltage - board_config.dc_bus_overvoltage_ramp_start) / (board_config.dc_bus_overvoltage_ramp_end - board_config.dc_bus_overvoltage_ramp_start), 0.0f); + } + + if (std::isnan(brake_duty)) { + // Shuts off all motors AND brake resistor, sets error code on all motors. + low_level_fault(Motor::ERROR_BRAKE_DUTY_CYCLE_NAN); + return; + } + + if (brake_duty >= 0.95f) { + brake_resistor_saturated = true; + } + + // Duty limit at 95% to allow bootstrap caps to charge + brake_duty = std::clamp(brake_duty, 0.0f, 0.95f); + + // Special handling to avoid the case 0.0/0.0 == NaN. + Ibus_sum += brake_duty ? (brake_duty * vbus_voltage / board_config.brake_resistance) : 0.0f; + + ibus_ = Ibus_sum; + + if (Ibus_sum > board_config.dc_max_positive_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_CURRENT); + return; + } + if (Ibus_sum < board_config.dc_max_negative_current) { + low_level_fault(Motor::ERROR_DC_BUS_OVER_REGEN_CURRENT); + return; + } - // Duty limit at 90% to allow bootstrap caps to charge - // If brake_duty is NaN, this expression will also evaluate to false int high_on = static_cast(TIM_APB1_PERIOD_CLOCKS * (1.0f - brake_duty)); int low_off = high_on - TIM_APB1_DEADTIME_CLOCKS; if (low_off < 0) low_off = 0; safety_critical_apply_brake_resistor_timings(low_off, high_on); - } diff --git a/Firmware/MotorControl/low_level.h b/Firmware/MotorControl/low_level.h index 503b98e1..f0b72ecb 100644 --- a/Firmware/MotorControl/low_level.h +++ b/Firmware/MotorControl/low_level.h @@ -22,7 +22,9 @@ extern const float adc_full_scale; extern const float adc_ref_voltage; /* Exported variables --------------------------------------------------------*/ extern float vbus_voltage; +extern float ibus_; extern bool brake_resistor_armed; +extern bool brake_resistor_saturated; extern uint16_t adc_measurements_[ADC_CHANNEL_COUNT]; /* Exported macro ------------------------------------------------------------*/ /* Exported functions --------------------------------------------------------*/ diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index 71b6425e..f49d02d6 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -23,7 +23,7 @@ bool user_config_loaded_; SystemStats_t system_stats_; -Axis *axes[AXIS_COUNT]; +std::array axes; ODriveCAN *odCAN = nullptr; typedef Config< @@ -242,7 +242,7 @@ int odrive_main(void) { axes[i]->setup(); } - for(auto axis : axes){ + for(auto& axis : axes){ axis->encoder_.setup(); } diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index 5d493437..aba285cc 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -23,7 +23,10 @@ public: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, ERROR_CURRENT_SENSE_SATURATION = 0x0400, ERROR_INVERTER_OVER_TEMP = 0x0800, - ERROR_CURRENT_LIMIT_VIOLATION = 0x1000 + ERROR_CURRENT_LIMIT_VIOLATION = 0x1000, + ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000, + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000, // too much current pushed into the power supply + ERROR_DC_BUS_OVER_CURRENT = 0x8000, // too much current pulled out of the power supply }; enum MotorType_t { diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index fe14446f..2dbb82a6 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -95,9 +95,31 @@ struct BoardConfig_t { // brake_duty_cycle += 0% + * vbus_voltage == dc_bus_overvoltage_ramp_end => brake_duty_cycle += 100% + * + * Remarks: + * - This setting is active even when all motors are disarmed. + * - brake_resistance must be non-zero, otherwise this will result in an + * overcurrent fault as soon as vbus_voltage exceeds dc_bus_overvoltage_ramp_start. + */ + bool enable_dc_bus_overvoltage_ramp = false; + float dc_bus_overvoltage_ramp_start = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. + //!< Do not set this lower than your usual vbus_voltage, + //!< unless you like fried brake resistors. + float dc_bus_overvoltage_ramp_end = 1.07f * HW_VERSION_VOLTAGE; //!< See `enable_dc_bus_overvoltage_ramp`. + //!< Must be larger than `dc_bus_overvoltage_ramp_start`, + //!< otherwise the ramp feature is disabled. + + float dc_max_positive_current = INFINITY; // Max current [A] the power supply can source + float dc_max_negative_current = -0.000001f; // Max current [A] the power supply can sink. You most likely want a non-positive value here. Set to -INFINITY to disable. PWMMapping_t pwm_mappings[GPIO_COUNT]; PWMMapping_t analog_mappings[GPIO_COUNT]; }; @@ -110,7 +132,7 @@ class Motor; class ODriveCAN; constexpr size_t AXIS_COUNT = 2; -extern Axis *axes[AXIS_COUNT]; +extern std::array axes; extern ODriveCAN *odCAN; // if you use the oscilloscope feature you can bump up this value diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index 2067c35e..dd7f64a2 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -44,7 +44,7 @@ bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, // 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_)); + Vr_ = s * sqrtf(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f)); Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_); Td_ = std::max(0.0f, -Vr_ / Dr_); Tv_ = 0.0f; diff --git a/Firmware/Tests/test_trap_traj.cpp b/Firmware/Tests/test_trap_traj.cpp new file mode 100644 index 00000000..b5d30014 --- /dev/null +++ b/Firmware/Tests/test_trap_traj.cpp @@ -0,0 +1,235 @@ + +#include +#include +#include +#include +#include + +#include "MotorControl/utils.hpp" + +// TODO: This is currently a copy-paste of the real code due to non-trivial +// include dependencies. Should include real code. + +class TrapezoidalTrajectory { +public: + struct Step_t { + float Y; + float Yd; + float Ydd; + }; + + explicit TrapezoidalTrajectory(); + bool planTrapezoidal(float Xf, float Xi, float Vi, + float Vmax, float Amax, float Dmax); + Step_t eval(float t); + + float Xi_; + float Xf_; + float Vi_; + + float Ar_; + float Vr_; + float Dr_; + + float Ta_; + float Tv_; + float Td_; + float Tf_; + + float yAccel_; + + float t_; +}; + + + +// 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() {} + +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(std::fmax((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f)); + //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; +} + +static_assert(sizeof(float) * CHAR_BIT == 32); + + +void run_trajectory_test(float goal, float position, float velocity, float Vmax, float Amax, float Dmax) { + float dt = 0.000125f; + int replan_interval = 10; // must be > 2 (see note below) + float t = 0.0f; + float Vmax_test = std::max(Vmax, std::abs(velocity)); + + TrapezoidalTrajectory traj{}; + + int replan_counter = 0; + + do { + if (replan_counter <= 0) { + CHECK(traj.planTrapezoidal(goal, position, velocity, Vmax, Amax, Dmax)); + t = 0.0f; + replan_counter = replan_interval; + } else { + replan_counter--; + } + + TrapezoidalTrajectory::Step_t step = traj.eval(t); + t += dt; + + //std::cerr << "vel: " << step.Yd << ", pos: " << step.Y << "\n"; + + // Check if acceleration within bounds + if (velocity >= 0.0f) { + CHECK(step.Ydd <= Amax); + CHECK(step.Ydd >= -Dmax); + CHECK((step.Yd - velocity) / dt <= Amax * 1.002f); + CHECK((step.Yd - velocity) / dt >= -Dmax * 1.002f); + } else { + CHECK(step.Ydd <= Dmax); + CHECK(step.Ydd >= -Amax); + CHECK((step.Yd - velocity) / dt <= Dmax * 1.002f); + CHECK((step.Yd - velocity) / dt >= -Amax * 1.002f); + } + + // Check if velocity within bounds + CHECK(step.Yd >= -Vmax_test); + CHECK(step.Yd <= Vmax_test); + CHECK((step.Y - position) / dt >= -Vmax_test * 1.002f); + CHECK((step.Y - position) / dt <= Vmax_test * 1.002f); + velocity = step.Yd; + + // Check if position is making progress + // TODO: the trajectory planner currently needs three "warm-up" iterations + // until its position makes progress. This should probably be revisited. + // TODO: this is disabled currently because there are legitimate trajectories + // where the position first moves in the wrong direction. + //if ((replan_counter < replan_interval - 2) && (t <= traj.Tf_)) { + // CHECK(std::abs(step.Y - goal) < std::abs(position - goal)); + //} + position = step.Y; + + } while (t <= traj.Tf_); + + CHECK(position >= goal - 1.0f); + CHECK(position <= goal + 1.0f); + CHECK(velocity >= -Dmax * dt); + CHECK(velocity <= Dmax * dt); +} + + +TEST_SUITE("Trajectory Planner") { + // these form a triangle trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 > 16384 + TEST_CASE("neg-dir-triangle") { + run_trajectory_test(-8192.0f, 8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-triangle") { + run_trajectory_test(8192.0f, -8192.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + + // these form a trapezoid trajectory because 2*v^2/(2*a) = 2 * 27712^2 / (2*22288) = 34456 < 16384 + TEST_CASE("neg-dir-trapezoid") { + run_trajectory_test(-25000.0f, 25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-trapezoid") { + run_trajectory_test(25000.0f, -25000.0f, 0.0f, 27712.0f, 22288.0f, 22288.0f); + } + + // for the following tests note that v^2/(2*a) = 27712^2 / (2*22288) = 17227 > 16384 + TEST_CASE("neg-dir-not-enough-braking-distance") { + run_trajectory_test(-8192.0f, 8192.0f, -27712.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-not-enough-braking-distance") { + run_trajectory_test(8192.0f, -8192.0f, 27712.0f, 27712.0f, 22288.0f, 22288.0f); + } + + TEST_CASE("neg-dir-over-speed") { + run_trajectory_test(-8192.0f, 8192.0f, -40000.0f, 27712.0f, 22288.0f, 22288.0f); + } + TEST_CASE("pos-dir-over-speed") { + run_trajectory_test(8192.0f, -8192.0f, 40000.0f, 27712.0f, 22288.0f, 22288.0f); + } +} diff --git a/Firmware/communication/communication.cpp b/Firmware/communication/communication.cpp index ea52ca87..fb033bdd 100644 --- a/Firmware/communication/communication.cpp +++ b/Firmware/communication/communication.cpp @@ -116,6 +116,7 @@ public: static inline auto make_obj_tree() { return make_protocol_member_list( make_protocol_ro_property("vbus_voltage", &vbus_voltage), + make_protocol_ro_property("ibus", &ibus_), make_protocol_ro_property("serial_number", &serial_number), make_protocol_ro_property("hw_version_major", &hw_version_major), make_protocol_ro_property("hw_version_minor", &hw_version_minor), @@ -126,6 +127,7 @@ static inline auto make_obj_tree() { make_protocol_ro_property("fw_version_unreleased", &fw_version_unreleased), make_protocol_ro_property("user_config_loaded", const_cast(&user_config_loaded_)), make_protocol_ro_property("brake_resistor_armed", &brake_resistor_armed), + make_protocol_property("brake_resistor_saturated", &brake_resistor_saturated), make_protocol_object("system_stats", make_protocol_ro_property("uptime", &system_stats_.uptime), make_protocol_ro_property("min_heap_space", &system_stats_.min_heap_space), @@ -159,7 +161,6 @@ static inline auto make_obj_tree() { ), make_protocol_object("config", make_protocol_property("brake_resistance", &board_config.brake_resistance), - make_protocol_property("nominal_voltage", &board_config.nominal_voltage), make_protocol_property("max_regen_current", &board_config.max_regen_current), // TODO: changing this currently requires a reboot - fix this make_protocol_property("enable_uart", &board_config.enable_uart), @@ -167,8 +168,11 @@ static inline auto make_obj_tree() { make_protocol_property("enable_ascii_protocol_on_usb", &board_config.enable_ascii_protocol_on_usb), make_protocol_property("dc_bus_undervoltage_trip_level", &board_config.dc_bus_undervoltage_trip_level), make_protocol_property("dc_bus_overvoltage_trip_level", &board_config.dc_bus_overvoltage_trip_level), - make_protocol_property("power_supply_min_current", &board_config.power_supply_min_current), - make_protocol_property("power_supply_max_current", &board_config.power_supply_max_current), + make_protocol_property("enable_dc_bus_overvoltage_ramp", &board_config.enable_dc_bus_overvoltage_ramp), + make_protocol_property("dc_bus_overvoltage_ramp_start", &board_config.dc_bus_overvoltage_ramp_start), + make_protocol_property("dc_bus_overvoltage_ramp_end", &board_config.dc_bus_overvoltage_ramp_end), + make_protocol_property("dc_max_negative_current", &board_config.dc_max_negative_current), + make_protocol_property("dc_max_positive_current", &board_config.dc_max_positive_current), #if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 3 make_protocol_object("gpio1_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[0])), make_protocol_object("gpio2_pwm_mapping", make_protocol_definitions(board_config.pwm_mappings[1])), diff --git a/analysis/filterpoles.py b/analysis/filterpoles.py index abdaee2c..ca57bc8f 100644 --- a/analysis/filterpoles.py +++ b/analysis/filterpoles.py @@ -5,15 +5,20 @@ from scipy.integrate import solve_ivp import matplotlib.pyplot as plt do_mass_spring = True -do_PLL = True +do_PLL = False -bandwidth = 1 +bandwidth = 10 pos_ref = 0 vel_ref = 0 init_pos = 1000 init_vel = 0 +plotend = 1 +plotfrequency = 1000.0 + +fig, ax1 = plt.subplots() + if do_mass_spring: # 2nd order system response with manipulation of velocity only # This is similar to a mass/spring/damper system @@ -36,10 +41,20 @@ if do_mass_spring: Xdot = [pos_dot, vel_dot] return Xdot - sol = solve_ivp(get_Xdot, (0.0, 10.0), [init_pos, init_vel], t_eval=np.linspace(0, 10, 100)) + sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency)) - plt.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='physical mass pos') - plt.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='physical mass vel') + color = 'tab:red' + ax1.set_xlabel('time (s)') + ax1.set_ylabel('pos', color=color) + ax1.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='physical mass', color=color) + ax1.tick_params(axis='y', labelcolor=color) + + ax2 = ax1.twinx() # instantiate a second axes that shares the same x-axis + + color = 'tab:blue' + ax2.set_ylabel('vel', color=color) # we already handled the x-label with ax1 + ax2.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='physical mass', color=color) + ax2.tick_params(axis='y', labelcolor=color) if do_PLL: @@ -64,7 +79,7 @@ if do_PLL: Xdot = [pos_dot, vel_dot] return Xdot - sol = solve_ivp(get_Xdot, (0.0, 10.0), [init_pos, init_vel], t_eval=np.linspace(0, 10, 100)) + sol = solve_ivp(get_Xdot, (0.0, plotend), [init_pos, init_vel], t_eval=np.linspace(0, plotend, plotend*plotfrequency)) plt.plot(np.transpose(sol.t), np.transpose(sol.y[0,:]), label='PLL pos') plt.plot(np.transpose(sol.t), np.transpose(sol.y[1,:]), label='PLL vel') @@ -72,4 +87,4 @@ if do_PLL: plt.legend() -plt.show(block=False) \ No newline at end of file +plt.show(block=True) \ No newline at end of file diff --git a/docs/configuring-vscode.md b/docs/configuring-vscode.md index 69edf772..7eb70178 100644 --- a/docs/configuring-vscode.md +++ b/docs/configuring-vscode.md @@ -23,12 +23,12 @@ Before doing the VSCode setup, make sure you've installed all of your [prerequis You should now be ready to compile and test the ODrive project. ## Building the Firmware -* Tasks -> Run Build Task +* Terminal -> Run Build Task (Ctrl+Shift+B) A terminal window will open with your native shell. VSCode is configured to run the command `make -j4` in this terminal. ## Flashing the Firmware -* Tasks -> Run Task -> flash +* Terminal -> Run Task -> flash A terminal window will open with your native shell. VSCode is configured to run the command `make flash` in this terminal. @@ -42,12 +42,13 @@ Note: If developing on Windows, you should have `arm-none-eabi-gdb` and `openOCD * Make sure you have the Firmware folder as your active folder * Set `CONFIG_DEBUG=true` in the tup.config file * Flash the board with the newest code (starting debug session doesn't do this) - * Debug -> Start Debugging (or press F5) + * In the _Run_ tab (Ctrl+Shift+D), select "Debug ODrive (Firmware)" + * Press _Start Debugging_ (or press F5) * The processor will reset and halt. * Set your breakpoints. Note: you can only set breakpoints when the processor is halted, if you set them during run mode, they won't get applied. - * Run (F5) + * _Continue_ (F5) * Stepping over/in/out, restarting, and changing breakpoints can be done by first pressing the "pause" (F6) button at the top the screen. - * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. + * When done debugging, simply stop (Shift+F5) the debugger. It will kill your openOCD process too. ## Cleaning the Build This sometimes needs to be done if you change branches. diff --git a/docs/developer-guide.md b/docs/developer-guide.md index 6f351070..dc20b208 100644 --- a/docs/developer-guide.md +++ b/docs/developer-guide.md @@ -59,7 +59,7 @@ sudo apt-get install openocd sudo add-apt-repository ppa:jonathonf/tup && sudo apt-get update && sudo apt-get install tup ``` -#### Arch Linux +#### Linux (Arch Linux) ```bash sudo pacman -S arm-none-eabi-gcc arm-none-eabi-binutils sudo pacman -S arm-none-eabi-gdb @@ -128,7 +128,7 @@ If the flashing worked, you can connect to the board using the [odrivetool](gett

## Testing -The script `tools/run_tests.py` runs a sequence of automated tests for several firmware features as well as high power burn-in tests. Some tests only need one ODrive and one motor/encoder pair while other tests need a back-to-back test rig such as [this one](https://cad.onshape.com/documents/026bda35ad5dff4d73c1d37f/w/ae302174f402737e1fdb3783/e/5ca143a6e5e24daf1fe8e434). In any case, to run the tests you need to provide a YAML file that lists the parameters of your test setup. An example can be found at [`tools/test-rig-parallel.yaml`](tools/test-rig-parallel.yaml`). The programmer serial number can be found by running `Firmware/find_programmer.sh` (make sure it has the latest formware from STM). +The script `tools/run_tests.py` runs a sequence of automated tests for several firmware features as well as high power burn-in tests. Some tests only need one ODrive and one motor/encoder pair while other tests need a back-to-back test rig such as [this one](https://cad.onshape.com/documents/026bda35ad5dff4d73c1d37f/w/ae302174f402737e1fdb3783/e/5ca143a6e5e24daf1fe8e434). In any case, to run the tests you need to provide a YAML file that lists the parameters of your test setup. An example can be found at [`tools/test-rig-parallel.yaml`](tools/test-rig-parallel.yaml`). The programmer serial number can be found by running `Firmware/find_programmer.sh` (make sure it has the latest firmware from STM).
The test script commands the ODrive to high currents and high motor speeds so if your ODrive is connected to anything other than a stirdy test-rig (or free spinning motors), it will probably break your machine.
diff --git a/docs/testing.md b/docs/testing.md index 347db2b1..771d4e44 100644 --- a/docs/testing.md +++ b/docs/testing.md @@ -3,21 +3,119 @@ This section describes how to use the automated testing facilities. You don't have to do this as an end user. -They test the following aspects: - - System functions (communication interfaces, configuration storage) - - Functionality of the motor controller and state machine - - High speed and high load conditions - The testing facility consists of the following components: - * **Test rig:** In the simplest case this can be a single ODrive with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. + * **Test rig:** In the simplest case this can be a single ODrive optionally with a single motor and encoder pair. Can also be multiple ODrives with multiple axes, some of which may be mechanically coupled. * **Test host:** The PC on which the test script runs. All ODrives must be connected to the test host via USB. * **test-rig.yaml:** Describes your test rig. Make sure all values are correct. Incorrect values may physically break or fry your test setup. - * **run_tests.py:** This is the main script that runs all the tests. + * **test_runner.py:** This is the main script that runs all the tests. + * **..._test.py** The actual tests -## How to run +## The Tests -Example: + - `analog_input_test.py`: Analog Input + - `calibration_test.py`: Motor calibration, encoder offset calibration, encoder direction find, encoder index search + - `can_test.py`: Partial coverage of the commands described in [CAN Protocol](can-protocol) + - `closed_loop_test.py`: Velocity control, position control (TODO: sensorless control), brake regen current protection + - `encoder_test.py`: Incremental encoder, hall effect encoder, sin/cos encoder, SPI encoders (AMS, CUI) + - `nvm_test.py`: Configuration storage + - `pwm_input_test.py`: PWM input + - `step_dir_test.py`: Step/dir input + - `uart_ascii_test.py`: Partial coverage of the commands described in [ASCII Protocol](ascii-protocol) + +All tests in a file can be run with e.g.: + + python3 uart_ascii_test.py --test-rig-yaml ../../test-rig-rpi.yaml + +See the following sections for a more detailed test flow description. + +## Our test rig + +Our test rig essentially consists of the following components: + + - an ODrive as the test subject + - a Teensy 4.0 to emulate external hardware such as encoders + - a Motor + Encoder pair for closed loop control tests + - a Raspberry Pi 4.0 as test host + - a CAN hat for the Raspberry Pi for CAN tests + +This document is therefore centered around this test rig layout. +If your test rig differs, you may be able to run some but not all of the tests. + +## How to set up a Raspberry Pi as testing host + + 1. Install Raspbian Lite on a Raspberry Pi 4.0. I used the NOOBS installer for this. + 2. Prepare the installation: + + sudo systemctl enable ssh + sudo systemctl start ssh + # Transfer your public key for passwordless SSH. All subsequent steps can be done via SSH. + sudo apt-get update + sudo apt-get upgrade + + 3. Add the following lines to `/boot/config.txt`: + - `enable_uart=1` + - `dtparam=spi=on` + - `dtoverlay=spi-bcm2835-overlay` + - `dtoverlay=mcp2515-can0,oscillator=12000000,interrupt=25` - Note: These oscillator and interrupt GPIO settings here are for the "RS485 CAN HAT" I have. There appear to be multiple versions, so they may be different from yours. Check the marking on the oscillator and the schematics. + + 4. Remove the following arguments from `/boot/cmdline.txt`: + - `console=serial0,115200` + + 5. Reboot. + + 6. Install the prerequisites: + + sudo apt-get install ipython3 python3-appdirs python3-yaml python3-usb python3-serial python3-can python3-scipy git openocd + # Optionally, to be able to compile the firmware: + sudo apt-get install gcc-arm-none-eabi + + 7. Install Teensyduino and teensy-loader-cli: + + sudo apt-get install libfontconfig libxft2 libusb-dev + + wget https://downloads.arduino.cc/arduino-1.8.12-linuxarm.tar.xz + tar -xf arduino-1.8.12-linuxarm.tar.xz + wget https://www.pjrc.com/teensy/td_151/TeensyduinoInstall.linuxarm + chmod +x TeensyduinoInstall.linuxarm + ./TeensyduinoInstall.linuxarm --dir=arduino-1.8.12 + sudo cp -R arduino-1.8.12 /usr/share/arduino + sudo ln -s /usr/share/arduino/arduino /usr/bin/arduino + + git clone https://github.com/PaulStoffregen/teensy_loader_cli + pushd teensy_loader_cli + sudo cp teensy_loader_cli /usr/bin/ + sudo ln -s /usr/bin/teensy_loader_cli /usr/bin/teensy-loader-cli + popd + + 8. Add the following lines to `/etc/udev/rules.d/49-stlinkv2`: + + SUBSYSTEMS=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="374b", MODE:="0666" + SUBSYSTEMS=="usb", ATTRS{idVendor}=="0483", ATTRS{idProduct}=="3748", MODE:="0666" + + 9. `sudo ../../odrivetool udev-setup` + + 10. `sudo udevadm trigger` + + 11. Run once after every reboot: `sudo ipython3 --pdb test_runner.py -- --setup-host --test-rig-yaml ../../test-rig-rpi.yaml` + +## SSH testing flow + +Here's one possible workflow for developing on the local host and testing on a remote SSH host. + +We assume that the ODrive repo is at `/path/to/ODriveFirmware` and your testing host is configured under the SSH name `odrv`. + +To flash and start remote debugging: + + 1. Start OpenOCD remotely, along with a tunnel to localhost: `ssh -t odrv -L3333:localhost:3333 bash -c "\"openocd '-f' 'interface/stlink-v2.cfg' '-f' 'target/stm32f4x_stlink.cfg'\""` + You can keep this open for multiple debug sessions. Press Ctrl+C to quit. + 2. Compile the firmware + 3. In VSCode, select the run configuration "Debug ODrive via external server" and press Run. In contrast to the other configurations, this will flash the new firmware before dropping you into the debugger. + +To run a test: + + rsync -avh -e ssh /path/to/ODriveFirmware odrv:/opt/odrivetest --exclude="Firmware/build" --exclude="Firmware/.tup" --exclude=".git" --delete + + ssh odrv + > cd /opt/odrivetest/tools/odrive/tests/ + > ipython3 --pdb uart_ascii_test.py -- --test-rig-yaml ../../test-rig-rpi.yaml -``` -./run_tests.py --skip-boring-tests --ignore top-odrive.yellow bottom-odrive.yellow -``` diff --git a/tools/odrive/enums.py b/tools/odrive/enums.py index 1292e48b..1198272e 100644 --- a/tools/odrive/enums.py +++ b/tools/odrive/enums.py @@ -32,8 +32,6 @@ class errors: ERROR_MIN_ENDSTOP_PRESSED = 0x1000 ERROR_MAX_ENDSTOP_PRESSED = 0x2000 ERROR_ESTOP_REQUESTED = 0x4000 - ERROR_DC_BUS_UNDER_CURRENT = 0x8000 - ERROR_DC_BUS_OVER_CURRENT = 0x10000 ERROR_HOMING_WITHOUT_ENDSTOP = 0x20000 class motor: @@ -50,6 +48,9 @@ class errors: ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200 ERROR_CURRENT_SENSE_SATURATION = 0x0400 ERROR_CURRENT_LIMIT_VIOLATION = 0x1000 + ERROR_BRAKE_DUTY_CYCLE_NAN = 0x2000 + ERROR_DC_BUS_OVER_REGEN_CURRENT = 0x4000 + ERROR_DC_BUS_OVER_CURRENT = 0x8000 class encoder: ERROR_NONE = 0 diff --git a/tools/odrive/tests/closed_loop_test.py b/tools/odrive/tests/closed_loop_test.py index 2b9cff42..dd26b6a5 100644 --- a/tools/odrive/tests/closed_loop_test.py +++ b/tools/odrive/tests/closed_loop_test.py @@ -10,8 +10,9 @@ from test_runner import * from odrive.enums import * -class TestClosedLoopControl(): +class TestClosedLoopControlBase(): """ + Base class for close loop control tests. """ def get_test_cases(self, testrig: TestRig): @@ -27,10 +28,7 @@ class TestClosedLoopControl(): if encoder.impl in testrig.get_connected_components(motor): yield (odrive.axes[num], motor, encoder) - def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): - axis = axis_ctx.handle - time.sleep(1.0) # wait for PLLs to stabilize - + def prepare(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): # Make sure there are no funny configurations active logger.debug('Setting up clean configuration...') axis_ctx.parent.erase_config_and_reboot() @@ -58,96 +56,162 @@ class TestClosedLoopControl(): test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_IDLE) test_assert_no_error(axis_ctx) - nominal_rps = 1.0 - nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps - logger.debug(f'Testing closed loop velocity control at {nominal_rps} rounds/s...') - axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL - axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH - axis_ctx.handle.controller.input_vel = 0 - - request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - axis_ctx.handle.controller.input_vel = nominal_vel - - data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=5.0) - - test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) - test_assert_no_error(axis_ctx) - request_state(axis_ctx, AXIS_STATE_IDLE) - - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.02) - test_assert_eq(offset, nominal_vel, accuracy = 0.05) - test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.3, inlier_range = nominal_vel * 0.5, max_outliers = len(data[:,0]) * 0.1) - - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data[:,(0,2)]) - test_assert_eq(slope, nominal_vel, accuracy = 0.01) - test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # Return a context that can be used in a with-statement. + class safe_terminator(): + def __enter__(self): + pass + def __exit__(self, exc_type, exc_val, exc_tb): + logger.debug('clearing config...') + axis_ctx.parent.erase_config_and_reboot() + return safe_terminator() - logger.debug(f'Testing closed loop position control...') +class TestClosedLoopControl(TestClosedLoopControlBase): + """ + Tests position and velocity control + """ + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): + nominal_rps = 1.0 + nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps + logger.debug(f'Testing closed loop velocity control at {nominal_rps} rounds/s...') + + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL + axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH + axis_ctx.handle.controller.input_vel = 0 + + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + axis_ctx.handle.controller.input_vel = nominal_vel + + data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=5.0) + + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_no_error(axis_ctx) + request_state(axis_ctx, AXIS_STATE_IDLE) + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.02) + test_assert_eq(offset, nominal_vel, accuracy = 0.05) + test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.3, inlier_range = nominal_vel * 0.5, max_outliers = len(data[:,0]) * 0.1) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data[:,(0,2)]) + test_assert_eq(slope, nominal_vel, accuracy = 0.01) + test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + + logger.debug(f'Testing closed loop position control...') + + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL + axis_ctx.handle.controller.input_pos = 0 + axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 5.0 # max 5 rps + axis_ctx.handle.encoder.set_linear_count(0) + + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + + # Test small position changes + axis_ctx.handle.controller.input_pos = 5000 + time.sleep(0.3) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, 5000, range=2000) # large range needed because of cogging torque + axis_ctx.handle.controller.input_pos = -5000 + time.sleep(0.3) + test_assert_no_error(axis_ctx) + test_assert_eq(axis_ctx.handle.encoder.pos_estimate, -5000, range=2000) + + axis_ctx.handle.controller.input_pos = 0 + time.sleep(0.3) + + nominal_vel = float(enc_ctx.yaml['cpr']) * 5.0 + axis_ctx.handle.controller.input_pos = nominal_vel * 2.0 # 10 turns (takes 2 seconds) + + # Test large position change with bounded velocity + data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=4.0) + + test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) + test_assert_no_error(axis_ctx) + request_state(axis_ctx, AXIS_STATE_IDLE) + + data_motion = data[data[:,0] < 1.9] + data_still = data[data[:,0] > 2.1] + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data_motion[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, nominal_vel, accuracy = 0.05) + test_curve_fit(data_motion[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data_motion[:,(0,2)]) + test_assert_eq(slope, nominal_vel, accuracy = 0.01) + test_curve_fit(data_motion[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.vel_estimate + slope, offset, fitted_curve = fit_line(data_still[:,(0,1)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, 0.0, range = nominal_vel * 0.05) + test_curve_fit(data_still[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + + # encoder.pos_estimate + slope, offset, fitted_curve = fit_line(data_still[:,(0,2)]) + test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) + test_assert_eq(offset, nominal_vel*2, range = nominal_vel * 0.02) + test_curve_fit(data_still[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.01, max_outliers = len(data[:,0]) * 0.01) + + +class TestRegenProtection(TestClosedLoopControlBase): + """ + Tries to brake with a disabled brake resistor. + This should result in a low level error disabling all power outputs. + """ + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + with self.prepare(axis_ctx, motor_ctx, enc_ctx, logger): + nominal_rps = 6.0 + nominal_vel = float(enc_ctx.yaml['cpr']) * nominal_rps - axis_ctx.handle.controller.config.control_mode = CTRL_MODE_POSITION_CONTROL - axis_ctx.handle.controller.input_pos = 0 - axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 5.0 # max 5 rps - axis_ctx.handle.encoder.set_linear_count(0) + # Accept a bit of noise on Ibus + axis_ctx.parent.handle.config.dc_max_negative_current = -0.2 - request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) + logger.debug(f'Brake control test from {nominal_rps} rounds/s...') + + axis_ctx.handle.controller.config.vel_limit = float(enc_ctx.yaml['cpr']) * 10.0 # max 10 rps + axis_ctx.handle.controller.config.control_mode = CTRL_MODE_VELOCITY_CONTROL + axis_ctx.handle.controller.config.input_mode = INPUT_MODE_PASSTHROUGH - # Test small position changes - axis_ctx.handle.controller.input_pos = 5000 - time.sleep(0.3) - test_assert_no_error(axis_ctx) - test_assert_eq(axis_ctx.handle.encoder.pos_estimate, 5000, range=2000) # large range needed because of cogging torque - axis_ctx.handle.controller.input_pos = -5000 - time.sleep(0.3) - test_assert_no_error(axis_ctx) - test_assert_eq(axis_ctx.handle.encoder.pos_estimate, -5000, range=2000) - - axis_ctx.handle.controller.input_pos = 0 - time.sleep(0.3) + request_state(axis_ctx, AXIS_STATE_CLOSED_LOOP_CONTROL) - nominal_vel = float(enc_ctx.yaml['cpr']) * 5.0 - axis_ctx.handle.controller.input_pos = nominal_vel * 2.0 # 10 turns (takes 2 seconds) - - # Test large position change with bounded velocity - data = record_log(lambda: [axis_ctx.handle.encoder.vel_estimate, axis_ctx.handle.encoder.pos_estimate], duration=4.0) - - test_assert_eq(axis_ctx.handle.current_state, AXIS_STATE_CLOSED_LOOP_CONTROL) - test_assert_no_error(axis_ctx) - request_state(axis_ctx, AXIS_STATE_IDLE) + # accelerate... + axis_ctx.handle.controller.input_vel = nominal_vel + time.sleep(1.0) + test_assert_no_error(axis_ctx) - data_motion = data[data[:,0] < 1.9] - data_still = data[data[:,0] > 2.1] + # ... and brake + axis_ctx.handle.controller.input_vel = 0 + time.sleep(1.0) + test_assert_no_error(axis_ctx) - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data_motion[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, nominal_vel, accuracy = 0.05) - test_curve_fit(data_motion[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # once more, but this time without brake resistor + axis_ctx.parent.handle.config.brake_resistance = 0 - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data_motion[:,(0,2)]) - test_assert_eq(slope, nominal_vel, accuracy = 0.01) - test_curve_fit(data_motion[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) + # accelerate... + axis_ctx.handle.controller.input_vel = nominal_vel + time.sleep(1.0) + test_assert_no_error(axis_ctx) - # encoder.vel_estimate - slope, offset, fitted_curve = fit_line(data_still[:,(0,1)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, 0.0, range = nominal_vel * 0.05) - test_curve_fit(data_still[:,(0,1)], fitted_curve, max_mean_err = nominal_vel * 0.05, inlier_range = nominal_vel * 0.1, max_outliers = len(data[:,0]) * 0.01) - - # encoder.pos_estimate - slope, offset, fitted_curve = fit_line(data_still[:,(0,2)]) - test_assert_eq(slope, 0.0, range = nominal_vel * 0.05) - test_assert_eq(offset, nominal_vel*2, range = nominal_vel * 0.02) - test_curve_fit(data_still[:,(0,2)], fitted_curve, max_mean_err = nominal_vel * 0.01, inlier_range = nominal_vel * 0.01, max_outliers = len(data[:,0]) * 0.01) + # ... and brake + axis_ctx.handle.controller.input_vel = 0 # this should fail almost instantaneously + time.sleep(0.1) + test_assert_eq(axis_ctx.handle.error, errors.axis.ERROR_MOTOR_DISARMED | errors.axis.ERROR_BRAKE_RESISTOR_DISARMED) + test_assert_eq(axis_ctx.handle.motor.error, errors.motor.ERROR_DC_BUS_OVER_REGEN_CURRENT) if __name__ == '__main__': test_runner.run([ - TestClosedLoopControl() + TestClosedLoopControl(), + TestRegenProtection(), ]) diff --git a/tools/odrive/tests/encoder_test.py b/tools/odrive/tests/encoder_test.py index b9af8ed6..59eac451 100644 --- a/tools/odrive/tests/encoder_test.py +++ b/tools/odrive/tests/encoder_test.py @@ -43,22 +43,22 @@ class TestEncoderBase(): # encoder.shadow_count slope, offset, fitted_curve = fit_line(data[:,(0,1)]) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,1)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.count_in_cpr slope, offset, fitted_curve = fit_sawtooth(data[:,(0,2)], true_cpr if reverse else 0, 0 if reverse else true_cpr) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,2)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.phase slope, offset, fitted_curve = fit_sawtooth(data[:,(0,3)], pi if reverse else -pi, -pi if reverse else pi, sigma=5) test_assert_eq(slope / 7, 2*pi*true_rps, accuracy=0.05) - test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,3)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.pos_estimate slope, offset, fitted_curve = fit_line(data[:,(0,4)]) test_assert_eq(slope, true_cps, accuracy=0.005) - test_curve_fit(data[:,(0,4)], fitted_curve, max_mean_err = true_cpr * 0.01, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) + test_curve_fit(data[:,(0,4)], fitted_curve, max_mean_err = true_cpr * 0.02, inlier_range = true_cpr * 0.02, max_outliers = len(data[:,0]) * 0.02) # encoder.pos_cpr slope, offset, fitted_curve = fit_sawtooth(data[:,(0,5)], true_cpr if reverse else 0, 0 if reverse else true_cpr) @@ -120,7 +120,7 @@ class TestIncrementalEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, logger: Logger): true_cps = 8192*-0.5 # counts per second generated by the virtual encoder code = teensy_incremental_encoder_emulation_code.replace("{enc_a}", str(teensy_gpio_a.num)).replace("{enc_b}", str(teensy_gpio_b.num)) @@ -180,7 +180,7 @@ class TestSinCosEncoder(TestEncoderBase): yield (odrive.encoders[0], valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_sin: TeensyGpio, teensy_gpio_cos: TeensyGpio, logger: Logger): code = teensy_sin_cos_encoder_emulation_code.replace("{enc_sin}", str(teensy_gpio_sin.num)).replace("{enc_cos}", str(teensy_gpio_cos.num)) teensy.compile_and_program(code) @@ -247,7 +247,7 @@ class TestHallEffectEncoder(TestEncoderBase): yield (encoder, valid_combinations) - def run_test(self, enc: EncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, teensy: TeensyComponent, teensy_gpio_a: TeensyGpio, teensy_gpio_b: TeensyGpio, teensy_gpio_c: TeensyGpio, logger: Logger): true_cpr = 90 true_rps = -1.0 @@ -432,7 +432,7 @@ class TestSpiEncoder(TestEncoderBase): yield (encoder, 7, valid_combinations) - def run_test(self, enc: EncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger): + def run_test(self, enc: ODriveEncoderComponent, odrive_ncs_gpio: int, teensy: TeensyComponent, teensy_gpio_sck: TeensyGpio, teensy_gpio_miso: TeensyGpio, teensy_gpio_mosi: TeensyGpio, teensy_gpio_ncs: TeensyGpio, teensy_gpio_reset: TeensyGpio, reset_gpio: LinuxGpioComponent, logger: Logger): true_cpr = 16384 true_rps = 1.0 @@ -452,6 +452,11 @@ class TestSpiEncoder(TestEncoderBase): enc.handle.config.mode = self.mode enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio enc.handle.config.cpr = true_cpr + # Also put the other encoder into SPI mode to make it more interesting + other_enc = enc.parent.encoders[1 - enc.num] + other_enc.handle.config.mode = self.mode + other_enc.handle.config.abs_spi_cs_gpio_pin = odrive_ncs_gpio + other_enc.handle.config.cpr = true_cpr enc.parent.save_config_and_reboot() time.sleep(1.0) @@ -499,8 +504,8 @@ class TestSpiEncoder(TestEncoderBase): if __name__ == '__main__': test_runner.run([ - #TestIncrementalEncoder(), - #TestSinCosEncoder(), + TestIncrementalEncoder(), + TestSinCosEncoder(), TestHallEffectEncoder(), TestSpiEncoder(ENCODER_MODE_SPI_ABS_AMS), TestSpiEncoder(ENCODER_MODE_SPI_ABS_CUI), diff --git a/tools/odrive/tests/not_a_test.py b/tools/odrive/tests/not_a_test.py new file mode 100644 index 00000000..7be3e995 --- /dev/null +++ b/tools/odrive/tests/not_a_test.py @@ -0,0 +1,30 @@ + +import test_runner + +from fibre.utils import Logger +from test_runner import * + +class EncoderPassthrough(): + """ + Does nothing except passing encoder0 through. + """ + + def get_test_cases(self, testrig: TestRig): + for odrive in testrig.get_components(ODriveComponent): + for num in range(1): + encoders = testrig.get_connected_components({ + 'a': (odrive.encoders[num].a, False), + 'b': (odrive.encoders[num].b, False), + 'z': (odrive.encoders[num].z, False) + }, EncoderComponent) + motors = testrig.get_connected_components(odrive.axes[num], MotorComponent) + + for motor, encoder in itertools.product(motors, encoders): + if encoder.impl in testrig.get_connected_components(motor): + yield (odrive.axes[num], motor, encoder) + + def run_test(self, axis_ctx: ODriveAxisComponent, motor_ctx: MotorComponent, enc_ctx: EncoderComponent, logger: Logger): + logger.debug(f'Encoder {axis_ctx.num} was passed through') + +if __name__ == '__main__': + test_runner.run(EncoderPassthrough()) diff --git a/tools/odrive/tests/test_runner.py b/tools/odrive/tests/test_runner.py index 56b9e663..d1697ec9 100644 --- a/tools/odrive/tests/test_runner.py +++ b/tools/odrive/tests/test_runner.py @@ -410,7 +410,7 @@ class TeensyComponent(Component): env['ARDUINO_COMPILE_DESTINATION'] = hexfile run_shell( ['arduino', '--board', 'teensy:avr:teensy40', '--verify', sketchfile], - logger, env = env, timeout = 60) + logger, env = env, timeout = 120) def program(self, hex_file_path: str, logger: Logger): """