diff --git a/fw/BUILD b/fw/BUILD index 4e866210..227ea303 100644 --- a/fw/BUILD +++ b/fw/BUILD @@ -1,6 +1,6 @@ # -*- python -*- -# Copyright 2018-2020 Josh Pieper, jjp@pobox.com. +# Copyright 2018-2022 Josh Pieper, jjp@pobox.com. # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. @@ -26,8 +26,15 @@ COPTS = [ cc_library( name = "common", hdrs = [ + "bldc_servo_position.h", + "bldc_servo_structs.h", + "ccm.h", + "error.h", "foc.h", "math.h", + "measured_hw_rev.h", + "pid.h", + "simple_pi.h", "torque_model.h", "stm32_i2c_timing.h", ], @@ -35,7 +42,10 @@ cc_library( "foc.cc", ], deps = [ + "@com_github_mjbots_mjlib//mjlib/base:limit", + "@com_github_mjbots_mjlib//mjlib/base:visitor", "@com_github_mjbots_mjlib//mjlib/micro:atomic_event_queue", + "@com_github_mjbots_mjlib//mjlib/micro:error_code", ], copts = COPTS, ) @@ -83,7 +93,6 @@ MOTEUS_SOURCES = [ "bootloader.h", "drv8323.h", "drv8323.cc", - "error.h", "error.cc", "hardfault.s", "firmware_info.h", @@ -94,8 +103,6 @@ MOTEUS_SOURCES = [ "moteus_hw.h", "millisecond_timer.h", "motor_driver.h", - "pid.h", - "simple_pi.h", "stm32_i2c.h", "stm32_spi.h", "stm32_serial.h", @@ -112,13 +119,11 @@ MOTEUS_DEPS = [ ":git_info", "@com_github_mjbots_mjlib//mjlib/base:assert", "@com_github_mjbots_mjlib//mjlib/base:inplace_function", - "@com_github_mjbots_mjlib//mjlib/base:limit", "@com_github_mjbots_mjlib//mjlib/base:windowed_average", "@com_github_mjbots_mjlib//mjlib/micro:async_exclusive", "@com_github_mjbots_mjlib//mjlib/micro:async_stream", "@com_github_mjbots_mjlib//mjlib/micro:callback_table", "@com_github_mjbots_mjlib//mjlib/micro:command_manager", - "@com_github_mjbots_mjlib//mjlib/micro:error_code", "@com_github_mjbots_mjlib//mjlib/micro:persistent_config", "@com_github_mjbots_mjlib//mjlib/micro:pool_ptr", "@com_github_mjbots_mjlib//mjlib/micro:telemetry_manager", @@ -176,6 +181,7 @@ genrule( cc_test( name = "test", srcs = [ + "test/bldc_servo_position_test.cc", "test/foc_test.cc", "test/math_test.cc", "test/stm32_i2c_timing_test.cc", diff --git a/fw/bldc_servo.cc b/fw/bldc_servo.cc index 169f7892..bdb7b017 100644 --- a/fw/bldc_servo.cc +++ b/fw/bldc_servo.cc @@ -26,6 +26,7 @@ #include "mjlib/base/assert.h" #include "mjlib/base/windowed_average.h" +#include "fw/bldc_servo_position.h" #include "fw/foc.h" #include "fw/math.h" #include "fw/moteus_hw.h" @@ -1778,89 +1779,15 @@ class BldcServo::Impl { float max_torque_Nm, float feedforward_Nm, float velocity) MOTEUS_CCM_ATTRIBUTE { - // We go to some lengths in our conversions to and from - // control_position so as to avoid converting a float directly to - // an int64, which calls out to a system library that is pretty - // slow. - - if (!std::isnan(data->position)) { - status_.control_position = - static_cast(65536ll * 65536ll) * - static_cast( - static_cast(motor_scale16_ * data->position)); - data->position = std::numeric_limits::quiet_NaN(); - } else if (!status_.control_position) { - status_.control_position = status_.unwrapped_position_raw; - } - - auto velocity_command = velocity; - - // This limits our usable velocity to 20kHz modulo the position - // scale at a 40kHz switching frequency. 1.2 million RPM should - // be enough for anybody? - status_.control_position = - (*status_.control_position + - 65536ll * static_cast( - (65536.0f * motor_scale16_ * velocity_command) / - rate_config_.rate_hz)); - - if (std::isfinite(config_.max_position_slip)) { - const int64_t current_position = status_.unwrapped_position_raw; - const int64_t slip = - static_cast(65536ll * 65536ll) * - static_cast(motor_scale16_ * config_.max_position_slip); - - const int64_t error = - current_position - *status_.control_position; - if (error < -slip) { - *status_.control_position = current_position + slip; - } - if (error > slip) { - *status_.control_position = current_position - slip; - } - } - - bool hit_limit = false; - - const auto saturate = [&](auto value, auto compare) MOTEUS_CCM_ATTRIBUTE { - if (std::isnan(value)) { return; } - const auto limit_value = ( - static_cast(65536ll * 65536ll) * - static_cast( - static_cast(motor_scale16_ * value))); - if (compare(*status_.control_position, limit_value)) { - status_.control_position = limit_value; - hit_limit = true; - } - }; - saturate(position_config_.position_min, [](auto l, auto r) { return l < r; }); - saturate(position_config_.position_max, [](auto l, auto r) { return l > r; }); - - if (!std::isnan(data->stop_position)) { - const int64_t stop_position_raw = - static_cast(65536ll * 65536ll) * - static_cast( - static_cast(motor_scale16_ * data->stop_position)); - - auto sign = [](auto value) MOTEUS_CCM_ATTRIBUTE -> float { - if (value < 0) { return -1.0f; } - if (value > 0) { return 1.0f; } - return 0.0f; - }; - if (sign(*status_.control_position - - stop_position_raw) * velocity_command > 0.0f) { - // We are moving away from the stop position. Force it to be - // there and zero out our velocity command. - status_.control_position = stop_position_raw; - data->velocity = 0.0f; - hit_limit = true; - } - } - - if (hit_limit) { - // We have hit a limit. Assume a velocity of 0. - velocity_command = 0.0f; - } + const float velocity_command = + BldcServoPosition::UpdateCommand( + &status_, + &config_, + &position_config_, + motor_scale16_, + rate_config_.rate_hz, + data, + velocity); // At this point, our control position and velocity are known. @@ -2089,6 +2016,7 @@ class BldcServo::Impl { Motor motor_; Config config_; PositionConfig position_config_; + TIM_TypeDef* timer_ = nullptr; volatile uint32_t* timer_sr_ = nullptr; volatile uint32_t* timer_cr1_ = nullptr; @@ -2175,8 +2103,10 @@ class BldcServo::Impl { float torque_constant_ = 0.01f; int32_t position_constant_ = 0; + // 65536.0f / unwrapped_position_scale_ float motor_scale16_ = 0; + float adc_scale_ = 0.0f; float adjusted_pwm_comp_off_ = 0.0f; float adjusted_max_power_W_ = 0.0f; diff --git a/fw/bldc_servo.h b/fw/bldc_servo.h index d54f4031..9075e70e 100644 --- a/fw/bldc_servo.h +++ b/fw/bldc_servo.h @@ -26,6 +26,7 @@ #include "fw/abs_port.h" #include "fw/as5047.h" +#include "fw/bldc_servo_structs.h" #include "fw/error.h" #include "fw/millisecond_timer.h" #include "fw/moteus_hw.h" @@ -74,470 +75,12 @@ class BldcServo { void PollMillisecond(); - struct Vec3 { - float a = 0.0f; - float b = 0.0f; - float c = 0.0f; - - template - void Serialize(Archive* ar) { - ar->Visit(MJ_NVP(a)); - ar->Visit(MJ_NVP(b)); - ar->Visit(MJ_NVP(c)); - } - }; - - struct Motor { - uint8_t poles = 0; - - // Invert the sense of the encoder. - uint8_t invert = 0; - - // Invert the order of phase movement. - uint8_t phase_invert = 0; - - float resistance_ohm = 0.0f; - - // Hz is electrical - float v_per_hz = 0.0f; - - float unwrapped_position_scale = 1.0f; - - // Electrical phase offset in radians as a function of encoder - // position. - std::array offset = {}; - - // After applying inversion, add this value to the position. - uint16_t position_offset = 0; - - // These control the higher order motor torque model. - // - // When above the cutoff current, the torque is calculated as: - // - // cutoff * torque_constant + tscale * log2(1 + (I - cutoff) * iscale) - // - // This models the "rotation" region of magnetic permeability. By - // default, the cutoff current is set unreasonably large, so that - // the controller will not use this region to estimate torque. - float rotation_current_cutoff_A = 10000.0; - float rotation_current_scale = 0.05; - float rotation_torque_scale = 14.7; - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(poles)); - a->Visit(MJ_NVP(invert)); - a->Visit(MJ_NVP(phase_invert)); - a->Visit(MJ_NVP(resistance_ohm)); - a->Visit(MJ_NVP(v_per_hz)); - a->Visit(MJ_NVP(unwrapped_position_scale)); - a->Visit(MJ_NVP(offset)); - a->Visit(MJ_NVP(position_offset)); - a->Visit(MJ_NVP(rotation_current_cutoff_A)); - a->Visit(MJ_NVP(rotation_current_scale)); - a->Visit(MJ_NVP(rotation_torque_scale)); - } - }; - - struct Config { - uint16_t pwm_rate_hz = - (g_measured_hw_rev <= 2) ? 60000 : - 40000; - - float i_gain = 20.0f; // should match csa_gain from drv8323 - float current_sense_ohm = 0.0005; - - // PWM rise time compensation - float pwm_comp_off = (g_measured_hw_rev <= 6) ? 0.015 : 0.055; - float pwm_comp_mag = (g_measured_hw_rev <= 6) ? 0.005 : 0.005; - float pwm_scale = (g_measured_hw_rev <= 6 ) ? 1.0f : 1.0f; - - // We pick a default maximum voltage based on the board revision. - float max_voltage = (g_measured_hw_rev <= 5) ? 37.0f : 46.0f; - float max_power_W = 450.0f; - - float derate_temperature = 50.0f; - float fault_temperature = 75.0f; - - float velocity_threshold = 0.09f; - float position_derate = 0.02f; - - // The current ADCs are driven by the drv8323's op-amp, which is - // very low impedance. They thus don't need a lot of sampling - // time. (They are on "slow" pins though, which means we can't - // get away with minimal sampling). - uint16_t adc_cur_cycles = 2; // 2, 6, 12, 24, 47, 92, 247, 640 - - // However, the aux channels, (voltage and temperature), are just - // driven by resistor networks (relatively high impedance). They - // need a larger sampling time. - uint16_t adc_aux_cycles = 47; - - // We use the same PID constants for D and Q current control - // loops. - SimplePI::Config pid_dq; - PID::Config pid_position; - - // If true, then the currents in A that are calculated for the D - // and Q phase are instead directly commanded as voltages on the - // phase terminals. This is primarily useful for high resistance - // motors like gimbal motors when the sense resistors are - // configured for a low resistance motor. - bool voltage_mode_control = false; - - // If true, then the controller acts as a cheap gimbal controller - // and does not use the encoder at all. Instead, the desired - // position is used to command an open loop fixed voltage. - bool fixed_voltage_mode = false; - float fixed_voltage_control_V = 0.0f; - - float max_position_slip = std::numeric_limits::quiet_NaN(); - - float default_timeout_s = 0.1f; - float timeout_max_torque_Nm = 5.0f; - - // Selects the behavior when in the timeout mode. The available - // options map to top level modes, although only the following are - // valid: - // 0 - "stopped" - motor driver disengaged - // 12 - "zero velocity" - derivative only position control - // 15 - "brake" - all motor phases shorted to ground - uint8_t timeout_mode = 12; - - // Similar to 'max_voltage', the flux braking default voltage is - // board rev dependent. - float flux_brake_min_voltage = (g_measured_hw_rev <= 5) ? 34.5f : 43.5f; - float flux_brake_resistance_ohm = 0.025f; - - float max_current_A = 100.0f; - float derate_current_A = -20.0f; - - // When the maximum velocity exceeds this value, a current limit - // will begin to be applied. When it reaches max_velocity + - // max_velocity_derate, the maximum allowed current will be 0. - float max_velocity = 500.0; - float max_velocity_derate = 2.0; - - uint16_t velocity_filter_length = 256; - uint16_t cooldown_cycles = 128; - - // If true, then the initial position will be taken from the ABS - // port's configured value shortly after startup. - bool rezero_from_abs = false; - - // A bitfield that selects one of several things to emit from the - // debug UART at full control rate. - uint32_t emit_debug = 0; - - struct EncoderFilter { - bool enabled = false; - float kp = 0.0f; - float ki = 0.0f; - int32_t debug_override = -1; - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(enabled)); - a->Visit(MJ_NVP(kp)); - a->Visit(MJ_NVP(ki)); - a->Visit(MJ_NVP(debug_override)); - } - }; - EncoderFilter encoder_filter; - - Config() { - pid_dq.kp = 0.005f; - pid_dq.ki = 30.0f; - - pid_position.kp = 4.0f; - pid_position.ki = 1.0f; - pid_position.ilimit = 0.0f; - pid_position.kd = 0.05f; - pid_position.sign = -1.0f; - } - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(pwm_rate_hz)); - a->Visit(MJ_NVP(i_gain)); - a->Visit(MJ_NVP(current_sense_ohm)); - a->Visit(MJ_NVP(pwm_comp_off)); - a->Visit(MJ_NVP(pwm_comp_mag)); - a->Visit(MJ_NVP(pwm_scale)); - a->Visit(MJ_NVP(max_voltage)); - a->Visit(MJ_NVP(max_power_W)); - a->Visit(MJ_NVP(derate_temperature)); - a->Visit(MJ_NVP(fault_temperature)); - a->Visit(MJ_NVP(velocity_threshold)); - a->Visit(MJ_NVP(position_derate)); - a->Visit(MJ_NVP(adc_cur_cycles)); - a->Visit(MJ_NVP(adc_aux_cycles)); - a->Visit(MJ_NVP(pid_dq)); - a->Visit(MJ_NVP(pid_position)); - a->Visit(MJ_NVP(voltage_mode_control)); - a->Visit(MJ_NVP(fixed_voltage_mode)); - a->Visit(MJ_NVP(fixed_voltage_control_V)); - a->Visit(MJ_NVP(max_position_slip)); - a->Visit(MJ_NVP(default_timeout_s)); - a->Visit(MJ_NVP(timeout_max_torque_Nm)); - a->Visit(MJ_NVP(timeout_mode)); - a->Visit(MJ_NVP(flux_brake_min_voltage)); - a->Visit(MJ_NVP(flux_brake_resistance_ohm)); - a->Visit(MJ_NVP(max_current_A)); - a->Visit(MJ_NVP(derate_current_A)); - a->Visit(MJ_NVP(max_velocity)); - a->Visit(MJ_NVP(max_velocity_derate)); - a->Visit(MJ_NVP(velocity_filter_length)); - a->Visit(MJ_NVP(cooldown_cycles)); - a->Visit(MJ_NVP(rezero_from_abs)); - a->Visit(MJ_NVP(emit_debug)); - a->Visit(MJ_NVP(encoder_filter)); - } - }; - - // This will commonly be different across every device, so it is - // separate to minimize resets due to schemas changing during - // development. - struct PositionConfig { - float position_min = -0.01f; - float position_max = 0.01f; - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(position_min)); - a->Visit(MJ_NVP(position_max)); - } - }; - - enum Mode { - // In this mode, the entire motor driver will be disabled. - // - // When exiting this state, the current offset will be - // recalibrated. - kStopped = 0, - - // This stage cannot be commanded directly, but will be entered - // upon any fault. Here, the motor driver remains enabled, but - // the output stage power is removed. The only valid transition - // from this state is to kStopped. - kFault = 1, - - // This mode may not be commanded directly. It is used when - // transitioning from kStopped to another mode. - kEnabling = 2, - - // This mode may not be commanded directly, but is used when - // transitioning from kStopped to another mode. - kCalibrating = 3, - - // This mode may not be commanded directly, but is used when - // transitioning from kStopped to another mode. - kCalibrationComplete = 4, - - // Directly control the PWM of all 3 phases. - kPwm = 5, - - // Control the voltage of all three phases - kVoltage = 6, - - // Control the phase and voltage magnitude - kVoltageFoc = 7, - - // Control d and q voltage - kVoltageDq = 8, - - // Control d and q current - kCurrent = 9, - - // Control absolute position - kPosition = 10, - - // This state can be commanded directly, and will also be entered - // automatically upon a watchdog timeout from kPosition. When in - // this state, the controller will apply the selected fallback - // control mode. - // - // The only way to exit this state is through a stop command. - kPositionTimeout = 11, - - // Control to zero velocity through a derivative only version of - // the position mode. - kZeroVelocity = 12, - - // This applies the PID controller only to stay within a - // particular position region, and applies 0 torque when within - // that region. - kStayWithinBounds = 13, - - // This mode applies a fixed voltage square waveform in the D axis - // in order to measure inductance assuming a motor with - // approximately equal D/Q axis inductances. - kMeasureInductance = 14, - - // All phases are pulled to ground. - kBrake = 15, - - kNumModes, - }; - - struct Status { - Mode mode = kStopped; - errc fault = errc::kSuccess; - - uint16_t adc_cur1_raw = 0; - uint16_t adc_cur2_raw = 0; - uint16_t adc_cur3_raw = 0; - uint16_t adc_voltage_sense_raw = 0; - uint16_t adc_fet_temp_raw = 0; - uint16_t adc_motor_temp_raw = 0; - - uint16_t position_raw = 0; - uint16_t position_unfilt = 0; - - uint16_t adc_cur1_offset = 2048; - uint16_t adc_cur2_offset = 2048; - uint16_t adc_cur3_offset = 2048; - - float cur1_A = 0.0f; - float cur2_A = 0.0f; - float cur3_A = 0.0f; - - float bus_V = 0.0f; - float filt_bus_V = std::numeric_limits::quiet_NaN(); - float filt_1ms_bus_V = std::numeric_limits::quiet_NaN(); - float position = 0; - float fet_temp_C = 0.0f; - float filt_fet_temp_C = std::numeric_limits::quiet_NaN(); - - float electrical_theta = 0.0f; - - float d_A = 0.0f; - float q_A = 0.0f; - - // This is a fixed point number, with the upper 32 bits being - // measured in encoder ticks, and the bottom 32 bits being - // fractional. - int64_t unwrapped_position_raw = 0; - float unwrapped_position = 0.0f; - float velocity = 0.0f; - float torque_Nm = 0.0f; - - SimplePI::State pid_d; - SimplePI::State pid_q; - PID::State pid_position; - - // This is measured in the same units as unwrapped_position_raw. - std::optional control_position; - float position_to_set = 0.0; - float timeout_s = 0.0; - bool rezeroed = false; - - float sin = 0.0f; - float cos = 0.0f; - uint16_t cooldown_count = 0; - uint32_t final_timer = 0; - uint32_t total_timer = 0; - - float meas_ind_old_d_A = 0.0f; - int8_t meas_ind_phase = 0; - float meas_ind_integrator = 0.0f; - -#ifdef MOTEUS_PERFORMANCE_MEASURE - struct Dwt { - uint32_t adc_done = 0; - uint32_t start_pos_sample = 0; - uint32_t done_pos_sample = 0; - uint32_t done_temp_sample = 0; - uint32_t sense = 0; - uint32_t curstate = 0; - uint32_t control_sel_mode = 0; - uint32_t control_done_pos = 0; - uint32_t control_done_cur = 0; - uint32_t control = 0; - uint32_t done = 0; - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(adc_done)); - a->Visit(MJ_NVP(start_pos_sample)); - a->Visit(MJ_NVP(done_pos_sample)); - a->Visit(MJ_NVP(done_temp_sample)); - a->Visit(MJ_NVP(sense)); - a->Visit(MJ_NVP(curstate)); - a->Visit(MJ_NVP(control_sel_mode)); - a->Visit(MJ_NVP(control_done_pos)); - a->Visit(MJ_NVP(control_done_cur)); - a->Visit(MJ_NVP(control)); - a->Visit(MJ_NVP(done)); - } - }; - - Dwt dwt; -#endif - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(mode)); - a->Visit(MJ_NVP(fault)); - - a->Visit(MJ_NVP(adc_cur1_raw)); - a->Visit(MJ_NVP(adc_cur2_raw)); - a->Visit(MJ_NVP(adc_cur3_raw)); - a->Visit(MJ_NVP(adc_voltage_sense_raw)); - a->Visit(MJ_NVP(adc_fet_temp_raw)); - a->Visit(MJ_NVP(adc_motor_temp_raw)); - - a->Visit(MJ_NVP(position_raw)); - a->Visit(MJ_NVP(position_unfilt)); - - a->Visit(MJ_NVP(adc_cur1_offset)); - a->Visit(MJ_NVP(adc_cur2_offset)); - a->Visit(MJ_NVP(adc_cur3_offset)); - - a->Visit(MJ_NVP(cur1_A)); - a->Visit(MJ_NVP(cur2_A)); - a->Visit(MJ_NVP(cur3_A)); - - a->Visit(MJ_NVP(bus_V)); - a->Visit(MJ_NVP(filt_bus_V)); - a->Visit(MJ_NVP(filt_1ms_bus_V)); - a->Visit(MJ_NVP(position)); - a->Visit(MJ_NVP(fet_temp_C)); - a->Visit(MJ_NVP(filt_fet_temp_C)); - a->Visit(MJ_NVP(electrical_theta)); - - a->Visit(MJ_NVP(d_A)); - a->Visit(MJ_NVP(q_A)); - - a->Visit(MJ_NVP(unwrapped_position_raw)); - a->Visit(MJ_NVP(unwrapped_position)); - a->Visit(MJ_NVP(velocity)); - a->Visit(MJ_NVP(torque_Nm)); - - a->Visit(MJ_NVP(pid_d)); - a->Visit(MJ_NVP(pid_q)); - a->Visit(MJ_NVP(pid_position)); - - a->Visit(MJ_NVP(control_position)); - a->Visit(MJ_NVP(position_to_set)); - a->Visit(MJ_NVP(timeout_s)); - a->Visit(MJ_NVP(rezeroed)); - - a->Visit(MJ_NVP(sin)); - a->Visit(MJ_NVP(cos)); - a->Visit(MJ_NVP(cooldown_count)); - a->Visit(MJ_NVP(final_timer)); - a->Visit(MJ_NVP(total_timer)); - - a->Visit(MJ_NVP(meas_ind_old_d_A)); - a->Visit(MJ_NVP(meas_ind_phase)); - a->Visit(MJ_NVP(meas_ind_integrator)); - -#ifdef MOTEUS_PERFORMANCE_MEASURE - a->Visit(MJ_NVP(dwt)); -#endif - } - }; + using Mode = BldcServoMode; + using Status = BldcServoStatus; + using CommandData = BldcServoCommandData; + using Motor = BldcServoMotor; + using Config = BldcServoConfig; + using PositionConfig = BldcServoPositionConfig; // Intermediate control outputs. struct Control { @@ -583,88 +126,6 @@ class BldcServo { } }; - struct CommandData { - Mode mode = kStopped; - - // For kPwm mode. - Vec3 pwm; // 0-1.0 - - // For kVoltage mode - Vec3 phase_v; - - // For kVoltageFoc - float theta = 0.0f; - float voltage = 0.0f; - - // For kVoltageDq and kMeasureInductance - float d_V = 0.0f; - float q_V = 0.0f; - - // For kFoc mode. - float i_d_A = 0.0f; - float i_q_A = 0.0f; - - // For kPosition mode - float position = 0.0f; // kNaN means start at the current position. - float velocity = 0.0f; - - float max_torque_Nm = 100.0f; - float stop_position = std::numeric_limits::quiet_NaN(); - float feedforward_Nm = 0.0f; - - float kp_scale = 1.0f; - float kd_scale = 1.0f; - - float timeout_s = 0.0f; - - // For kStayWithinBounds - float bounds_min = 0.0f; - float bounds_max = 0.0f; - - // For kMeasureInductance - int8_t meas_ind_period = 4; - - // If set, then force the position to be the given value. - std::optional set_position; - - // If set, then rezero the position as if from boot. Select a - // position closest to the given value. - std::optional rezero_position; - - template - void Serialize(Archive* a) { - a->Visit(MJ_NVP(mode)); - - a->Visit(MJ_NVP(pwm)); - - a->Visit(MJ_NVP(phase_v)); - - a->Visit(MJ_NVP(theta)); - a->Visit(MJ_NVP(voltage)); - - a->Visit(MJ_NVP(d_V)); - a->Visit(MJ_NVP(q_V)); - - a->Visit(MJ_NVP(i_d_A)); - a->Visit(MJ_NVP(i_q_A)); - - a->Visit(MJ_NVP(position)); - a->Visit(MJ_NVP(velocity)); - a->Visit(MJ_NVP(max_torque_Nm)); - a->Visit(MJ_NVP(stop_position)); - a->Visit(MJ_NVP(feedforward_Nm)); - a->Visit(MJ_NVP(kp_scale)); - a->Visit(MJ_NVP(kd_scale)); - a->Visit(MJ_NVP(timeout_s)); - a->Visit(MJ_NVP(bounds_min)); - a->Visit(MJ_NVP(bounds_max)); - a->Visit(MJ_NVP(meas_ind_period)); - - a->Visit(MJ_NVP(set_position)); - a->Visit(MJ_NVP(rezero_position)); - } - }; - void Start(); void Command(const CommandData&); @@ -679,36 +140,3 @@ class BldcServo { }; } - -namespace mjlib { -namespace base { - -template <> -struct IsEnum { - static constexpr bool value = true; - - using M = moteus::BldcServo::Mode; - static std::array, M::kNumModes> map() { - return { { - { M::kStopped, "stopped" }, - { M::kFault, "fault" }, - { M::kEnabling, "enabling" }, - { M::kCalibrating, "calibrating" }, - { M::kCalibrationComplete, "calib_complete" }, - { M::kPwm, "pwm" }, - { M::kVoltage, "voltage" }, - { M::kVoltageFoc, "voltage_foc" }, - { M::kVoltageDq, "voltage_dq" }, - { M::kCurrent, "current" }, - { M::kPosition, "position" }, - { M::kPositionTimeout, "pos_timeout" }, - { M::kZeroVelocity, "zero_vel" }, - { M::kStayWithinBounds, "within" }, - { M::kMeasureInductance, "meas_ind" }, - { M::kBrake, "brake" }, - }}; - } -}; - -} -} diff --git a/fw/bldc_servo_position.h b/fw/bldc_servo_position.h new file mode 100644 index 00000000..7b909a1a --- /dev/null +++ b/fw/bldc_servo_position.h @@ -0,0 +1,127 @@ +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "fw/bldc_servo_structs.h" +#include "fw/ccm.h" +#include "fw/measured_hw_rev.h" + +namespace moteus { + +// We put these functions into a class merely so that we can leave +// them in the header and also have a section definition. +class BldcServoPosition { + public: + + static float UpdateCommand( + BldcServoStatus* status, + const BldcServoConfig* config, + const BldcServoPositionConfig* position_config, + float motor_scale16, + float rate_hz, + BldcServoCommandData* data, + float velocity) MOTEUS_CCM_ATTRIBUTE { + + // We go to some lengths in our conversions to and from + // control_position so as to avoid converting a float directly to + // an int64, which calls out to a system library that is pretty + // slow. + + if (!std::isnan(data->position)) { + status->control_position = + static_cast(65536ll * 65536ll) * + static_cast( + static_cast(motor_scale16 * data->position)); + data->position = std::numeric_limits::quiet_NaN(); + } else if (!status->control_position) { + status->control_position = status->unwrapped_position_raw; + } + + auto velocity_command = velocity; + + // This limits our usable velocity to 20kHz modulo the position + // scale at a 40kHz switching frequency. 1.2 million RPM should + // be enough for anybody? + status->control_position = + (*status->control_position + + 65536ll * static_cast( + (65536.0f * motor_scale16 * velocity_command) / + rate_hz)); + + if (std::isfinite(config->max_position_slip)) { + const int64_t current_position = status->unwrapped_position_raw; + const int64_t slip = + static_cast(65536ll * 65536ll) * + static_cast(motor_scale16 * config->max_position_slip); + + const int64_t error = + current_position - *status->control_position; + if (error < -slip) { + *status->control_position = current_position + slip; + } + if (error > slip) { + *status->control_position = current_position - slip; + } + } + + bool hit_limit = false; + + const auto saturate = [&](auto value, auto compare) MOTEUS_CCM_ATTRIBUTE { + if (std::isnan(value)) { return; } + const auto limit_value = ( + static_cast(65536ll * 65536ll) * + static_cast( + static_cast(motor_scale16 * value))); + if (compare(*status->control_position, limit_value)) { + status->control_position = limit_value; + hit_limit = true; + } + }; + saturate(position_config->position_min, + [](auto l, auto r) { return l < r; }); + saturate(position_config->position_max, + [](auto l, auto r) { return l > r; }); + + if (!std::isnan(data->stop_position)) { + const int64_t stop_position_raw = + static_cast(65536ll * 65536ll) * + static_cast( + static_cast(motor_scale16 * data->stop_position)); + + auto sign = [](auto value) MOTEUS_CCM_ATTRIBUTE -> float { + if (value < 0) { return -1.0f; } + if (value > 0) { return 1.0f; } + return 0.0f; + }; + if (sign(*status->control_position - + stop_position_raw) * velocity_command > 0.0f) { + // We are moving away from the stop position. Force it to be + // there and zero out our velocity command. + status->control_position = stop_position_raw; + data->velocity = 0.0f; + hit_limit = true; + } + } + + if (hit_limit) { + // We have hit a limit. Assume a velocity of 0. + velocity_command = 0.0f; + } + + return velocity_command; + } +}; + +} diff --git a/fw/bldc_servo_structs.h b/fw/bldc_servo_structs.h new file mode 100644 index 00000000..35ec7075 --- /dev/null +++ b/fw/bldc_servo_structs.h @@ -0,0 +1,609 @@ +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include "mjlib/base/visitor.h" + +#include "fw/error.h" +#include "fw/measured_hw_rev.h" +#include "fw/pid.h" +#include "fw/simple_pi.h" + +namespace moteus { + +struct Vec3 { + float a = 0.0f; + float b = 0.0f; + float c = 0.0f; + + template + void Serialize(Archive* ar) { + ar->Visit(MJ_NVP(a)); + ar->Visit(MJ_NVP(b)); + ar->Visit(MJ_NVP(c)); + } +}; + +enum BldcServoMode { + // In this mode, the entire motor driver will be disabled. + // + // When exiting this state, the current offset will be + // recalibrated. + kStopped = 0, + + // This stage cannot be commanded directly, but will be entered + // upon any fault. Here, the motor driver remains enabled, but + // the output stage power is removed. The only valid transition + // from this state is to kStopped. + kFault = 1, + + // This mode may not be commanded directly. It is used when + // transitioning from kStopped to another mode. + kEnabling = 2, + + // This mode may not be commanded directly, but is used when + // transitioning from kStopped to another mode. + kCalibrating = 3, + + // This mode may not be commanded directly, but is used when + // transitioning from kStopped to another mode. + kCalibrationComplete = 4, + + // Directly control the PWM of all 3 phases. + kPwm = 5, + + // Control the voltage of all three phases + kVoltage = 6, + + // Control the phase and voltage magnitude + kVoltageFoc = 7, + + // Control d and q voltage + kVoltageDq = 8, + + // Control d and q current + kCurrent = 9, + + // Control absolute position + kPosition = 10, + + // This state can be commanded directly, and will also be entered + // automatically upon a watchdog timeout from kPosition. When in + // this state, the controller will apply the selected fallback + // control mode. + // + // The only way to exit this state is through a stop command. + kPositionTimeout = 11, + + // Control to zero velocity through a derivative only version of + // the position mode. + kZeroVelocity = 12, + + // This applies the PID controller only to stay within a + // particular position region, and applies 0 torque when within + // that region. + kStayWithinBounds = 13, + + // This mode applies a fixed voltage square waveform in the D axis + // in order to measure inductance assuming a motor with + // approximately equal D/Q axis inductances. + kMeasureInductance = 14, + + // All phases are pulled to ground. + kBrake = 15, + + kNumModes, +}; + +struct BldcServoStatus { + BldcServoMode mode = kStopped; + errc fault = errc::kSuccess; + + uint16_t adc_cur1_raw = 0; + uint16_t adc_cur2_raw = 0; + uint16_t adc_cur3_raw = 0; + uint16_t adc_voltage_sense_raw = 0; + uint16_t adc_fet_temp_raw = 0; + uint16_t adc_motor_temp_raw = 0; + + uint16_t position_raw = 0; + uint16_t position_unfilt = 0; + + uint16_t adc_cur1_offset = 2048; + uint16_t adc_cur2_offset = 2048; + uint16_t adc_cur3_offset = 2048; + + float cur1_A = 0.0f; + float cur2_A = 0.0f; + float cur3_A = 0.0f; + + float bus_V = 0.0f; + float filt_bus_V = std::numeric_limits::quiet_NaN(); + float filt_1ms_bus_V = std::numeric_limits::quiet_NaN(); + float position = 0; + float fet_temp_C = 0.0f; + float filt_fet_temp_C = std::numeric_limits::quiet_NaN(); + + float electrical_theta = 0.0f; + + float d_A = 0.0f; + float q_A = 0.0f; + + // This is a fixed point number, with the upper 32 bits being + // measured in encoder ticks, and the bottom 32 bits being + // fractional. + int64_t unwrapped_position_raw = 0; + float unwrapped_position = 0.0f; + float velocity = 0.0f; + float torque_Nm = 0.0f; + + SimplePI::State pid_d; + SimplePI::State pid_q; + PID::State pid_position; + + // This is measured in the same units as unwrapped_position_raw. + std::optional control_position; + float position_to_set = 0.0; + float timeout_s = 0.0; + bool rezeroed = false; + + float sin = 0.0f; + float cos = 0.0f; + uint16_t cooldown_count = 0; + uint32_t final_timer = 0; + uint32_t total_timer = 0; + + float meas_ind_old_d_A = 0.0f; + int8_t meas_ind_phase = 0; + float meas_ind_integrator = 0.0f; + +#ifdef MOTEUS_PERFORMANCE_MEASURE + struct Dwt { + uint32_t adc_done = 0; + uint32_t start_pos_sample = 0; + uint32_t done_pos_sample = 0; + uint32_t done_temp_sample = 0; + uint32_t sense = 0; + uint32_t curstate = 0; + uint32_t control_sel_mode = 0; + uint32_t control_done_pos = 0; + uint32_t control_done_cur = 0; + uint32_t control = 0; + uint32_t done = 0; + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(adc_done)); + a->Visit(MJ_NVP(start_pos_sample)); + a->Visit(MJ_NVP(done_pos_sample)); + a->Visit(MJ_NVP(done_temp_sample)); + a->Visit(MJ_NVP(sense)); + a->Visit(MJ_NVP(curstate)); + a->Visit(MJ_NVP(control_sel_mode)); + a->Visit(MJ_NVP(control_done_pos)); + a->Visit(MJ_NVP(control_done_cur)); + a->Visit(MJ_NVP(control)); + a->Visit(MJ_NVP(done)); + } + }; + + Dwt dwt; +#endif + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(mode)); + a->Visit(MJ_NVP(fault)); + + a->Visit(MJ_NVP(adc_cur1_raw)); + a->Visit(MJ_NVP(adc_cur2_raw)); + a->Visit(MJ_NVP(adc_cur3_raw)); + a->Visit(MJ_NVP(adc_voltage_sense_raw)); + a->Visit(MJ_NVP(adc_fet_temp_raw)); + a->Visit(MJ_NVP(adc_motor_temp_raw)); + + a->Visit(MJ_NVP(position_raw)); + a->Visit(MJ_NVP(position_unfilt)); + + a->Visit(MJ_NVP(adc_cur1_offset)); + a->Visit(MJ_NVP(adc_cur2_offset)); + a->Visit(MJ_NVP(adc_cur3_offset)); + + a->Visit(MJ_NVP(cur1_A)); + a->Visit(MJ_NVP(cur2_A)); + a->Visit(MJ_NVP(cur3_A)); + + a->Visit(MJ_NVP(bus_V)); + a->Visit(MJ_NVP(filt_bus_V)); + a->Visit(MJ_NVP(filt_1ms_bus_V)); + a->Visit(MJ_NVP(position)); + a->Visit(MJ_NVP(fet_temp_C)); + a->Visit(MJ_NVP(filt_fet_temp_C)); + a->Visit(MJ_NVP(electrical_theta)); + + a->Visit(MJ_NVP(d_A)); + a->Visit(MJ_NVP(q_A)); + + a->Visit(MJ_NVP(unwrapped_position_raw)); + a->Visit(MJ_NVP(unwrapped_position)); + a->Visit(MJ_NVP(velocity)); + a->Visit(MJ_NVP(torque_Nm)); + + a->Visit(MJ_NVP(pid_d)); + a->Visit(MJ_NVP(pid_q)); + a->Visit(MJ_NVP(pid_position)); + + a->Visit(MJ_NVP(control_position)); + a->Visit(MJ_NVP(position_to_set)); + a->Visit(MJ_NVP(timeout_s)); + a->Visit(MJ_NVP(rezeroed)); + + a->Visit(MJ_NVP(sin)); + a->Visit(MJ_NVP(cos)); + a->Visit(MJ_NVP(cooldown_count)); + a->Visit(MJ_NVP(final_timer)); + a->Visit(MJ_NVP(total_timer)); + + a->Visit(MJ_NVP(meas_ind_old_d_A)); + a->Visit(MJ_NVP(meas_ind_phase)); + a->Visit(MJ_NVP(meas_ind_integrator)); + +#ifdef MOTEUS_PERFORMANCE_MEASURE + a->Visit(MJ_NVP(dwt)); +#endif + } +}; + +struct BldcServoCommandData { + BldcServoMode mode = kStopped; + + // For kPwm mode. + Vec3 pwm; // 0-1.0 + + // For kVoltage mode + Vec3 phase_v; + + // For kVoltageFoc + float theta = 0.0f; + float voltage = 0.0f; + + // For kVoltageDq and kMeasureInductance + float d_V = 0.0f; + float q_V = 0.0f; + + // For kFoc mode. + float i_d_A = 0.0f; + float i_q_A = 0.0f; + + // For kPosition mode + float position = 0.0f; // kNaN means start at the current position. + float velocity = 0.0f; + + float max_torque_Nm = 100.0f; + float stop_position = std::numeric_limits::quiet_NaN(); + float feedforward_Nm = 0.0f; + + float kp_scale = 1.0f; + float kd_scale = 1.0f; + + float timeout_s = 0.0f; + + // For kStayWithinBounds + float bounds_min = 0.0f; + float bounds_max = 0.0f; + + // For kMeasureInductance + int8_t meas_ind_period = 4; + + // If set, then force the position to be the given value. + std::optional set_position; + + // If set, then rezero the position as if from boot. Select a + // position closest to the given value. + std::optional rezero_position; + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(mode)); + + a->Visit(MJ_NVP(pwm)); + + a->Visit(MJ_NVP(phase_v)); + + a->Visit(MJ_NVP(theta)); + a->Visit(MJ_NVP(voltage)); + + a->Visit(MJ_NVP(d_V)); + a->Visit(MJ_NVP(q_V)); + + a->Visit(MJ_NVP(i_d_A)); + a->Visit(MJ_NVP(i_q_A)); + + a->Visit(MJ_NVP(position)); + a->Visit(MJ_NVP(velocity)); + a->Visit(MJ_NVP(max_torque_Nm)); + a->Visit(MJ_NVP(stop_position)); + a->Visit(MJ_NVP(feedforward_Nm)); + a->Visit(MJ_NVP(kp_scale)); + a->Visit(MJ_NVP(kd_scale)); + a->Visit(MJ_NVP(timeout_s)); + a->Visit(MJ_NVP(bounds_min)); + a->Visit(MJ_NVP(bounds_max)); + a->Visit(MJ_NVP(meas_ind_period)); + + a->Visit(MJ_NVP(set_position)); + a->Visit(MJ_NVP(rezero_position)); + } +}; + +struct BldcServoMotor { + uint8_t poles = 0; + + // Invert the sense of the encoder. + uint8_t invert = 0; + + // Invert the order of phase movement. + uint8_t phase_invert = 0; + + float resistance_ohm = 0.0f; + + // Hz is electrical + float v_per_hz = 0.0f; + + float unwrapped_position_scale = 1.0f; + + // Electrical phase offset in radians as a function of encoder + // position. + std::array offset = {}; + + // After applying inversion, add this value to the position. + uint16_t position_offset = 0; + + // These control the higher order motor torque model. + // + // When above the cutoff current, the torque is calculated as: + // + // cutoff * torque_constant + tscale * log2(1 + (I - cutoff) * iscale) + // + // This models the "rotation" region of magnetic permeability. By + // default, the cutoff current is set unreasonably large, so that + // the controller will not use this region to estimate torque. + float rotation_current_cutoff_A = 10000.0; + float rotation_current_scale = 0.05; + float rotation_torque_scale = 14.7; + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(poles)); + a->Visit(MJ_NVP(invert)); + a->Visit(MJ_NVP(phase_invert)); + a->Visit(MJ_NVP(resistance_ohm)); + a->Visit(MJ_NVP(v_per_hz)); + a->Visit(MJ_NVP(unwrapped_position_scale)); + a->Visit(MJ_NVP(offset)); + a->Visit(MJ_NVP(position_offset)); + a->Visit(MJ_NVP(rotation_current_cutoff_A)); + a->Visit(MJ_NVP(rotation_current_scale)); + a->Visit(MJ_NVP(rotation_torque_scale)); + } +}; + +struct BldcServoConfig { + uint16_t pwm_rate_hz = + (g_measured_hw_rev <= 2) ? 60000 : + 40000; + + float i_gain = 20.0f; // should match csa_gain from drv8323 + float current_sense_ohm = 0.0005; + + // PWM rise time compensation + float pwm_comp_off = (g_measured_hw_rev <= 6) ? 0.015 : 0.055; + float pwm_comp_mag = (g_measured_hw_rev <= 6) ? 0.005 : 0.005; + float pwm_scale = (g_measured_hw_rev <= 6 ) ? 1.0f : 1.0f; + + // We pick a default maximum voltage based on the board revision. + float max_voltage = (g_measured_hw_rev <= 5) ? 37.0f : 46.0f; + float max_power_W = 450.0f; + + float derate_temperature = 50.0f; + float fault_temperature = 75.0f; + + float velocity_threshold = 0.09f; + float position_derate = 0.02f; + + // The current ADCs are driven by the drv8323's op-amp, which is + // very low impedance. They thus don't need a lot of sampling + // time. (They are on "slow" pins though, which means we can't + // get away with minimal sampling). + uint16_t adc_cur_cycles = 2; // 2, 6, 12, 24, 47, 92, 247, 640 + + // However, the aux channels, (voltage and temperature), are just + // driven by resistor networks (relatively high impedance). They + // need a larger sampling time. + uint16_t adc_aux_cycles = 47; + + // We use the same PID constants for D and Q current control + // loops. + SimplePI::Config pid_dq; + PID::Config pid_position; + + // If true, then the currents in A that are calculated for the D + // and Q phase are instead directly commanded as voltages on the + // phase terminals. This is primarily useful for high resistance + // motors like gimbal motors when the sense resistors are + // configured for a low resistance motor. + bool voltage_mode_control = false; + + // If true, then the controller acts as a cheap gimbal controller + // and does not use the encoder at all. Instead, the desired + // position is used to command an open loop fixed voltage. + bool fixed_voltage_mode = false; + float fixed_voltage_control_V = 0.0f; + + float max_position_slip = std::numeric_limits::quiet_NaN(); + + float default_timeout_s = 0.1f; + float timeout_max_torque_Nm = 5.0f; + + // Selects the behavior when in the timeout mode. The available + // options map to top level modes, although only the following are + // valid: + // 0 - "stopped" - motor driver disengaged + // 12 - "zero velocity" - derivative only position control + // 15 - "brake" - all motor phases shorted to ground + uint8_t timeout_mode = 12; + + // Similar to 'max_voltage', the flux braking default voltage is + // board rev dependent. + float flux_brake_min_voltage = (g_measured_hw_rev <= 5) ? 34.5f : 43.5f; + float flux_brake_resistance_ohm = 0.025f; + + float max_current_A = 100.0f; + float derate_current_A = -20.0f; + + // When the maximum velocity exceeds this value, a current limit + // will begin to be applied. When it reaches max_velocity + + // max_velocity_derate, the maximum allowed current will be 0. + float max_velocity = 500.0; + float max_velocity_derate = 2.0; + + uint16_t velocity_filter_length = 256; + uint16_t cooldown_cycles = 128; + + // If true, then the initial position will be taken from the ABS + // port's configured value shortly after startup. + bool rezero_from_abs = false; + + // A bitfield that selects one of several things to emit from the + // debug UART at full control rate. + uint32_t emit_debug = 0; + + struct EncoderFilter { + bool enabled = false; + float kp = 0.0f; + float ki = 0.0f; + int32_t debug_override = -1; + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(enabled)); + a->Visit(MJ_NVP(kp)); + a->Visit(MJ_NVP(ki)); + a->Visit(MJ_NVP(debug_override)); + } + }; + EncoderFilter encoder_filter; + + BldcServoConfig() { + pid_dq.kp = 0.005f; + pid_dq.ki = 30.0f; + + pid_position.kp = 4.0f; + pid_position.ki = 1.0f; + pid_position.ilimit = 0.0f; + pid_position.kd = 0.05f; + pid_position.sign = -1.0f; + } + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(pwm_rate_hz)); + a->Visit(MJ_NVP(i_gain)); + a->Visit(MJ_NVP(current_sense_ohm)); + a->Visit(MJ_NVP(pwm_comp_off)); + a->Visit(MJ_NVP(pwm_comp_mag)); + a->Visit(MJ_NVP(pwm_scale)); + a->Visit(MJ_NVP(max_voltage)); + a->Visit(MJ_NVP(max_power_W)); + a->Visit(MJ_NVP(derate_temperature)); + a->Visit(MJ_NVP(fault_temperature)); + a->Visit(MJ_NVP(velocity_threshold)); + a->Visit(MJ_NVP(position_derate)); + a->Visit(MJ_NVP(adc_cur_cycles)); + a->Visit(MJ_NVP(adc_aux_cycles)); + a->Visit(MJ_NVP(pid_dq)); + a->Visit(MJ_NVP(pid_position)); + a->Visit(MJ_NVP(voltage_mode_control)); + a->Visit(MJ_NVP(fixed_voltage_mode)); + a->Visit(MJ_NVP(fixed_voltage_control_V)); + a->Visit(MJ_NVP(max_position_slip)); + a->Visit(MJ_NVP(default_timeout_s)); + a->Visit(MJ_NVP(timeout_max_torque_Nm)); + a->Visit(MJ_NVP(timeout_mode)); + a->Visit(MJ_NVP(flux_brake_min_voltage)); + a->Visit(MJ_NVP(flux_brake_resistance_ohm)); + a->Visit(MJ_NVP(max_current_A)); + a->Visit(MJ_NVP(derate_current_A)); + a->Visit(MJ_NVP(max_velocity)); + a->Visit(MJ_NVP(max_velocity_derate)); + a->Visit(MJ_NVP(velocity_filter_length)); + a->Visit(MJ_NVP(cooldown_cycles)); + a->Visit(MJ_NVP(rezero_from_abs)); + a->Visit(MJ_NVP(emit_debug)); + a->Visit(MJ_NVP(encoder_filter)); + } +}; + +// This will commonly be different across every device, so it is +// separate to minimize resets due to schemas changing during +// development. +struct BldcServoPositionConfig { + float position_min = -0.01f; + float position_max = 0.01f; + + template + void Serialize(Archive* a) { + a->Visit(MJ_NVP(position_min)); + a->Visit(MJ_NVP(position_max)); + } +}; + +} + +namespace mjlib { +namespace base { + +template <> +struct IsEnum { + static constexpr bool value = true; + + using M = moteus::BldcServoMode; + static std::array, M::kNumModes> map() { + return { { + { M::kStopped, "stopped" }, + { M::kFault, "fault" }, + { M::kEnabling, "enabling" }, + { M::kCalibrating, "calibrating" }, + { M::kCalibrationComplete, "calib_complete" }, + { M::kPwm, "pwm" }, + { M::kVoltage, "voltage" }, + { M::kVoltageFoc, "voltage_foc" }, + { M::kVoltageDq, "voltage_dq" }, + { M::kCurrent, "current" }, + { M::kPosition, "position" }, + { M::kPositionTimeout, "pos_timeout" }, + { M::kZeroVelocity, "zero_vel" }, + { M::kStayWithinBounds, "within" }, + { M::kMeasureInductance, "meas_ind" }, + { M::kBrake, "brake" }, + }}; + } +}; + +} +} diff --git a/fw/board_debug.cc b/fw/board_debug.cc index 2cd7aa8e..a74f4d71 100644 --- a/fw/board_debug.cc +++ b/fw/board_debug.cc @@ -1,4 +1,4 @@ -// Copyright 2018-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -140,7 +140,7 @@ class BoardDebug::Impl { motor_cal_mode_ = kNoMotorCal; BldcServo::CommandData command; - command.mode = BldcServo::kStopped; + command.mode = BldcServo::Mode::kStopped; bldc_->Command(command); @@ -166,7 +166,7 @@ class BoardDebug::Impl { motor_cal_mode_ = kNoMotorCal; BldcServo::CommandData command; - command.mode = BldcServo::kStopped; + command.mode = BldcServo::Mode::kStopped; bldc_->Command(command); @@ -198,7 +198,7 @@ class BoardDebug::Impl { } BldcServo::CommandData command; - command.mode = BldcServo::kVoltageFoc; + command.mode = BldcServo::Mode::kVoltageFoc; command.theta = (cal_phase_ / 65536.0f) * 2.0f * kPi; command.voltage = cal_magnitude_; @@ -232,7 +232,7 @@ class BoardDebug::Impl { if (cmd_text == "stop") { BldcServo::CommandData command; - command.mode = BldcServo::kStopped; + command.mode = BldcServo::Mode::kStopped; bldc_->Command(command); WriteOk(response); @@ -250,7 +250,7 @@ class BoardDebug::Impl { } BldcServo::CommandData command; - command.mode = BldcServo::kPwm; + command.mode = BldcServo::Mode::kPwm; command.pwm.a = std::strtof(pwm1_str.data(), nullptr); command.pwm.b = std::strtof(pwm2_str.data(), nullptr); command.pwm.c = std::strtof(pwm3_str.data(), nullptr); @@ -273,7 +273,7 @@ class BoardDebug::Impl { const float magnitude = std::strtof(magnitude_str.data(), nullptr); BldcServo::CommandData command; - command.mode = BldcServo::kVoltageFoc; + command.mode = BldcServo::Mode::kVoltageFoc; command.theta = phase; command.voltage = magnitude; @@ -342,7 +342,7 @@ class BoardDebug::Impl { } BldcServo::CommandData command; - command.mode = BldcServo::kVoltage; + command.mode = BldcServo::Mode::kVoltage; command.phase_v.a = std::strtof(a_str.data(), nullptr); command.phase_v.b = std::strtof(b_str.data(), nullptr); command.phase_v.c = std::strtof(c_str.data(), nullptr); @@ -365,7 +365,7 @@ class BoardDebug::Impl { const float q_V = std::strtof(q_str.data(), nullptr); BldcServo::CommandData command; - command.mode = BldcServo::kVoltageDq; + command.mode = BldcServo::Mode::kVoltageDq; command.d_V = d_V; command.q_V = q_V; @@ -388,7 +388,7 @@ class BoardDebug::Impl { const float q = std::strtof(q_str.data(), nullptr); BldcServo::CommandData command; - command.mode = BldcServo::kCurrent; + command.mode = BldcServo::Mode::kCurrent; command.i_d_A = d; command.i_q_A = q; @@ -424,10 +424,10 @@ class BoardDebug::Impl { } command.mode = - (cmd_text == "pos") ? BldcServo::kPosition : - (cmd_text == "tmt") ? BldcServo::kPositionTimeout : - (cmd_text == "zero") ? BldcServo::kZeroVelocity : - BldcServo::kStopped; + (cmd_text == "pos") ? BldcServo::Mode::kPosition : + (cmd_text == "tmt") ? BldcServo::Mode::kPositionTimeout : + (cmd_text == "zero") ? BldcServo::Mode::kZeroVelocity : + BldcServo::Mode::kStopped; command.position = pos; command.velocity = vel; @@ -462,7 +462,7 @@ class BoardDebug::Impl { return; } - command.mode = BldcServo::kStayWithinBounds; + command.mode = BldcServo::Mode::kStayWithinBounds; command.bounds_min = min_pos; command.bounds_max = max_pos; @@ -493,7 +493,7 @@ class BoardDebug::Impl { BldcServo::CommandData command; - command.mode = BldcServo::kMeasureInductance; + command.mode = BldcServo::Mode::kMeasureInductance; command.d_V = volt; command.meas_ind_period = period; @@ -506,7 +506,7 @@ class BoardDebug::Impl { if (cmd_text == "brake") { BldcServo::CommandData command; - command.mode = BldcServo::kBrake; + command.mode = BldcServo::Mode::kBrake; bldc_->Command(command); WriteOk(response); return; @@ -522,7 +522,7 @@ class BoardDebug::Impl { const float index_value = std::strtof(pos_value.data(), nullptr); BldcServo::CommandData command; - command.mode = BldcServo::kStopped; + command.mode = BldcServo::Mode::kStopped; command.set_position = index_value; @@ -533,7 +533,7 @@ class BoardDebug::Impl { if (cmd_text == "rezero") { BldcServo::CommandData command; - command.mode = BldcServo::kStopped; + command.mode = BldcServo::Mode::kStopped; const auto pos_value = tokenizer.next(); command.rezero_position = diff --git a/fw/ccm.h b/fw/ccm.h new file mode 100644 index 00000000..a16cbcbb --- /dev/null +++ b/fw/ccm.h @@ -0,0 +1,21 @@ +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#if defined(TARGET_STM32G4) +#define MOTEUS_CCM_ATTRIBUTE __attribute__ ((section (".ccmram"))) +#else +#define MOTEUS_CCM_ATTRIBUTE +#endif diff --git a/fw/drv8323.cc b/fw/drv8323.cc index b883f850..0660117a 100644 --- a/fw/drv8323.cc +++ b/fw/drv8323.cc @@ -1,4 +1,4 @@ -// Copyright 2018-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -21,6 +21,7 @@ #include "mjlib/base/inplace_function.h" +#include "fw/ccm.h" #include "fw/moteus_hw.h" #include "fw/stm32_spi.h" diff --git a/fw/drv8323.h b/fw/drv8323.h index 4c874fe3..b67f7b1c 100644 --- a/fw/drv8323.h +++ b/fw/drv8323.h @@ -1,4 +1,4 @@ -// Copyright 2018-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -22,6 +22,7 @@ #include "mjlib/micro/pool_ptr.h" #include "mjlib/micro/telemetry_manager.h" +#include "fw/measured_hw_rev.h" #include "fw/millisecond_timer.h" #include "fw/motor_driver.h" #include "fw/moteus_hw.h" diff --git a/fw/error.h b/fw/error.h index 2317a2fe..d24f0c1e 100644 --- a/fw/error.h +++ b/fw/error.h @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Josh Pieper, jjp@pobox.com. +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,6 +14,7 @@ #pragma once +#include #include #include "mjlib/base/visitor.h" diff --git a/fw/firmware_info.cc b/fw/firmware_info.cc index 3fb887d1..cd99a9eb 100644 --- a/fw/firmware_info.cc +++ b/fw/firmware_info.cc @@ -14,6 +14,7 @@ #include "fw/firmware_info.h" +#include "fw/measured_hw_rev.h" #include "fw/moteus_hw.h" namespace moteus { diff --git a/fw/measured_hw_rev.h b/fw/measured_hw_rev.h new file mode 100644 index 00000000..359eebe1 --- /dev/null +++ b/fw/measured_hw_rev.h @@ -0,0 +1,22 @@ +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +namespace moteus { + +// The measured version of MOTEUS_HW_REV +extern volatile uint8_t g_measured_hw_rev; + +} diff --git a/fw/moteus_controller.cc b/fw/moteus_controller.cc index 01bcbed8..d60c8b91 100644 --- a/fw/moteus_controller.cc +++ b/fw/moteus_controller.cc @@ -1,4 +1,4 @@ -// Copyright 2018-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2018-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -428,7 +428,7 @@ class MoteusController::Impl : public multiplex::MicroServer::Server { case Register::kRezero: { command_.rezero_position = ReadPosition(value); - command_.mode = BldcServo::kStopped; + command_.mode = BldcServo::Mode::kStopped; command_valid_ = true; return 0; } diff --git a/fw/moteus_hw.h b/fw/moteus_hw.h index ccfe9dd8..b464004c 100644 --- a/fw/moteus_hw.h +++ b/fw/moteus_hw.h @@ -16,9 +16,6 @@ namespace moteus { -// The measured version of MOTEUS_HW_REV -extern volatile uint8_t g_measured_hw_rev; - // r1 silk // #define MOTEUS_HW_REV 0 @@ -210,13 +207,6 @@ constexpr int kCompatibleHwRev[] = { #define MOTEUS_ABS_SCL PB_8 #define MOTEUS_ABS_SDA PB_9 -#if defined(TARGET_STM32G4) -#define MOTEUS_CCM_ATTRIBUTE __attribute__ ((section (".ccmram"))) -#else -#error "Unknown target" -#endif - - #define MOTEUS_MODEL_NUMBER ((MOTEUS_HW_REV) << 8 | 0x00) #define MOTEUS_FIRMWARE_VERSION 0x000104 diff --git a/fw/pid.h b/fw/pid.h index fc2cc954..a47c18a5 100644 --- a/fw/pid.h +++ b/fw/pid.h @@ -1,4 +1,4 @@ -// Copyright 2015-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2015-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,7 +20,7 @@ #include "mjlib/base/limit.h" #include "mjlib/base/visitor.h" -#include "fw/moteus_hw.h" +#include "fw/ccm.h" namespace moteus { diff --git a/fw/simple_pi.h b/fw/simple_pi.h index 0d0b4093..2c873883 100644 --- a/fw/simple_pi.h +++ b/fw/simple_pi.h @@ -1,4 +1,4 @@ -// Copyright 2015-2021 Josh Pieper, jjp@pobox.com. +// Copyright 2015-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,7 +20,7 @@ #include "mjlib/base/limit.h" #include "mjlib/base/visitor.h" -#include "fw/moteus_hw.h" +#include "fw/ccm.h" namespace moteus { diff --git a/fw/stm32_spi.h b/fw/stm32_spi.h index 92586ee5..91f4c3e8 100644 --- a/fw/stm32_spi.h +++ b/fw/stm32_spi.h @@ -20,6 +20,8 @@ #include "hal/spi_api.h" +#include "fw/ccm.h" + namespace moteus { class Stm32Spi { diff --git a/fw/test/bldc_servo_position_test.cc b/fw/test/bldc_servo_position_test.cc new file mode 100644 index 00000000..9cbcc75a --- /dev/null +++ b/fw/test/bldc_servo_position_test.cc @@ -0,0 +1,223 @@ +// Copyright 2022 Josh Pieper, jjp@pobox.com. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "fw/bldc_servo_position.h" + +#include + +using namespace moteus; + +namespace tt = boost::test_tools; + +namespace { +constexpr float NaN = std::numeric_limits::quiet_NaN(); + +struct Context { + BldcServoStatus status; + BldcServoConfig config; + BldcServoPositionConfig position_config; + float motor_scale16 = 65536.0f / 1.0f; + float rate_hz = 40000.0f; + BldcServoCommandData data; + + Context() { + position_config.position_min = NaN; + position_config.position_max = NaN; + + data.mode = BldcServoMode::kPosition; + set_position(3.2f); + } + + void set_position(float val) { + status.unwrapped_position = val; + status.unwrapped_position_raw = to_raw(status.unwrapped_position); + } + + int64_t to_raw(float val) const { + return static_cast(val * motor_scale16) << 32; + } + + float from_raw(int64_t val) const { + return (val >> 32) / motor_scale16; + } + + float Call() { + return BldcServoPosition::UpdateCommand( + &status, + &config, + &position_config, + motor_scale16, + rate_hz, + &data, + data.velocity); + } +}; +} + + +BOOST_AUTO_TEST_CASE(StartupPositionCapture) { + // When starting, we capture the current position if no command + // position is given. + Context ctx; + + BOOST_TEST(!ctx.status.control_position); + ctx.data.position = NaN; + ctx.data.velocity = 0.0f; + ctx.Call(); + BOOST_TEST(ctx.status.control_position.value() == ctx.status.unwrapped_position_raw); +} + +BOOST_AUTO_TEST_CASE(StartupPositionSet) { + // When starting, if a command position is given, we use that. + Context ctx; + + BOOST_TEST(!ctx.status.control_position); + ctx.data.position = 2.0f; + ctx.data.velocity = 0.0f; + ctx.Call(); + BOOST_TEST(ctx.status.control_position.value() != ctx.status.unwrapped_position_raw); + BOOST_TEST(ctx.status.control_position.value() == ctx.to_raw(2.0f)); +} + +BOOST_AUTO_TEST_CASE(RunningPositionSet) { + // When running, we still take actual command positions immediately. + Context ctx; + + ctx.status.control_position = ctx.to_raw(4.0f); + ctx.data.position = 2.0f; + ctx.data.velocity = 0.0f; + ctx.Call(); + + BOOST_TEST(ctx.status.control_position.value() == ctx.to_raw(2.0f)); +} + +BOOST_AUTO_TEST_CASE(RunningPositionCapture) { + // When running, an unset position means we keep the old one. + Context ctx; + + ctx.status.control_position = ctx.to_raw(4.0f); + ctx.data.position = NaN; + ctx.data.velocity = 0.0f; + ctx.Call(); + + BOOST_TEST(ctx.status.control_position.value() == ctx.to_raw(4.0f)); +} + +BOOST_AUTO_TEST_CASE(PositionLimit) { + struct TestCase { + float position_min; + float position_max; + float control_position; + + float expected_output; + }; + + TestCase test_cases[] = { + { NaN, NaN, 0.0f, 0.0f }, + { NaN, NaN, 100.0f, 100.0f }, + { NaN, NaN, -100.0f, -100.0f }, + { NaN, 3.0f, 0.0f, 0.0f }, + { NaN, 3.0f, 3.0f, 3.0f }, + { NaN, 3.0f, 10.0f, 3.0f }, + { NaN, 3.0f, -10.0f, -10.0f }, + { 1.0f, 3.0f, -10.0f, 1.0f }, + { 1.0f, 3.0f, 1.0f, 1.0f }, + { 1.0f, 3.0f, 1.5f, 1.5f }, + { 1.0f, 3.0f, 3.0f, 3.0f }, + { 1.0f, 3.0f, 4.0f, 3.0f }, + { 1.0f, NaN, 4.0f, 4.0f }, + }; + + for (const auto& test_case : test_cases) { + BOOST_TEST_CONTEXT("Case " + << test_case.position_min + << " " << test_case.position_max + << " " << test_case.control_position) { + Context ctx; + ctx.position_config.position_min = test_case.position_min; + ctx.position_config.position_max = test_case.position_max; + ctx.data.position = NaN; + ctx.data.velocity = 0.0f; + ctx.status.control_position = ctx.to_raw(test_case.control_position); + + ctx.Call(); + + BOOST_TEST(ctx.status.control_position.value() == + ctx.to_raw(test_case.expected_output)); + } + } +} + +BOOST_AUTO_TEST_CASE(PositionVelocity, * boost::unit_test::tolerance(1e-3f)) { + Context ctx; + + ctx.data.position = NaN; + ctx.data.velocity = 1.0f; + ctx.status.control_position = ctx.to_raw(3.0f); + for (int i = 0; i < ctx.rate_hz; i++) { + const float result = ctx.Call(); + BOOST_TEST(result == 1.0f); + } + + BOOST_TEST(ctx.from_raw(ctx.status.control_position.value()) == 4.0f); + + ctx.data.stop_position = 4.5f; + for (int i = 0; i < ctx.rate_hz; i++) { + const float result = ctx.Call(); + if (i < ctx.rate_hz / 2) { + BOOST_TEST(result == 1.0f); + } else { + BOOST_TEST(result == 0.0f); + } + } + BOOST_TEST(ctx.from_raw(ctx.status.control_position.value()) == 4.5f); +} + +BOOST_AUTO_TEST_CASE(PositionSlip, * boost::unit_test::tolerance(1e-3f)) { + struct TestCase { + float max_position_slip; + float control_position; + float observed_position; + + float expected_position; + }; + + TestCase test_cases[] = { + { NaN, 0.0f, 0.0f, 0.0f }, + { NaN, 1.0f, 0.0f, 1.0f }, + { NaN, -5.0f, 2.0f, -5.0f }, + { 2.0f, -5.0f, 2.0f, 0.0f }, + { 3.0f, -5.0f, 2.0f, -1.0f }, + { 3.0f, 6.0f, 2.0f, 5.0f }, + }; + + for (const TestCase& test_case : test_cases) { + BOOST_TEST_CONTEXT("Case " + << test_case.max_position_slip << " " + << test_case.control_position << " " + << test_case.observed_position) { + Context ctx; + ctx.data.position = NaN; + + ctx.config.max_position_slip = test_case.max_position_slip; + ctx.status.control_position = ctx.to_raw(test_case.control_position); + ctx.set_position(test_case.observed_position); + + ctx.Call(); + + BOOST_TEST(ctx.from_raw(ctx.status.control_position.value()) == + test_case.expected_position); + } + } +} diff --git a/fw/test/test_main.cc b/fw/test/test_main.cc index 1ff47cb6..c4bfe93d 100644 --- a/fw/test/test_main.cc +++ b/fw/test/test_main.cc @@ -1,4 +1,4 @@ -// Copyright 2014-2020 Josh Pieper, jjp@pobox.com. +// Copyright 2014-2022 Josh Pieper, jjp@pobox.com. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,3 +14,7 @@ #define BOOST_TEST_MODULE moteus #include + +namespace moteus { +volatile uint8_t g_measured_hw_rev = 7; +}