diff --git a/Firmware/Board/v3/Inc/board.h b/Firmware/Board/v3/Inc/board.h index 2ee28877..58b2d31b 100644 --- a/Firmware/Board/v3/Inc/board.h +++ b/Firmware/Board/v3/Inc/board.h @@ -77,6 +77,7 @@ using TOpAmp = Drv8301; #include #include +extern std::array axes; extern Motor motors[AXIS_COUNT]; extern OnboardThermistorCurrentLimiter fet_thermistors[AXIS_COUNT]; extern Encoder encoders[AXIS_COUNT]; @@ -103,44 +104,6 @@ static const float current_meas_period = CURRENT_MEAS_PERIOD; #define CURRENT_MEAS_HZ ( (float)(TIM_1_8_CLOCK_HZ) / (float)(2*TIM_1_8_PERIOD_CLOCKS*(TIM_1_8_RCR+1)) ) static const int current_meas_hz = CURRENT_MEAS_HZ; - -typedef struct { - uint16_t step_gpio_pin; - uint16_t dir_gpio_pin; - osPriority thread_priority; -} AxisHardwareConfig_t; - -typedef struct { - AxisHardwareConfig_t axis_config; -} BoardHardwareConfig_t; - -extern const BoardHardwareConfig_t hw_configs[AXIS_COUNT]; - -//TODO stick this in a C file -#ifdef __MAIN_CPP__ - -const BoardHardwareConfig_t hw_configs[AXIS_COUNT] = { { - //M0 - .axis_config = { - .step_gpio_pin = 1, - .dir_gpio_pin = 2, - .thread_priority = (osPriority)(osPriorityHigh + (osPriority)1), - }, -},{ - //M1 - .axis_config = { -#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 - .step_gpio_pin = 7, - .dir_gpio_pin = 8, -#else - .step_gpio_pin = 3, - .dir_gpio_pin = 4, -#endif - .thread_priority = osPriorityHigh, - }, -} }; -#endif - #if HW_VERSION_VOLTAGE >= 48 #define VBUS_S_DIVIDER_RATIO 19.0f #elif HW_VERSION_VOLTAGE == 24 diff --git a/Firmware/Board/v3/board.cpp b/Firmware/Board/v3/board.cpp index 38143014..d636dc25 100644 --- a/Firmware/Board/v3/board.cpp +++ b/Firmware/Board/v3/board.cpp @@ -94,6 +94,51 @@ Encoder encoders[AXIS_COUNT] = { } }; +// TODO: this has no hardware dependency and should be allocated depending on config +Endstop endstops[2 * AXIS_COUNT]; + +SensorlessEstimator sensorless_estimators[AXIS_COUNT]; +Controller controllers[AXIS_COUNT]; +TrapezoidalTrajectory trap[AXIS_COUNT]; +OffboardThermistorCurrentLimiter motor_thermistors[AXIS_COUNT]; + +std::array axes{{ + { + 0, // axis_num + 1, // step_gpio_pin + 2, // dir_gpio_pin + (osPriority)(osPriorityHigh + (osPriority)1), // thread_priority + encoders[0], // encoder + sensorless_estimators[0], // sensorless_estimator + controllers[0], // controller + fet_thermistors[0], // fet_thermistor + motor_thermistors[0], // motor_thermistor + motors[0], // motor + trap[0], // trap + endstops[0], endstops[1], // min_endstop, max_endstop + }, + { + 1, // axis_num +#if HW_VERSION_MAJOR == 3 && HW_VERSION_MINOR >= 5 + 7, // step_gpio_pin + 8, // dir_gpio_pin +#else + 3, // step_gpio_pin + 4, // dir_gpio_pin +#endif + osPriorityHigh, // thread_priority + encoders[1], // encoder + sensorless_estimators[1], // sensorless_estimator + controllers[1], // controller + fet_thermistors[1], // fet_thermistor + motor_thermistors[1], // motor_thermistor + motors[1], // motor + trap[1], // trap + endstops[2], endstops[3], // min_endstop, max_endstop + }, +}}; + + #if (HW_VERSION_MINOR == 1) || (HW_VERSION_MINOR == 2) Stm32Gpio gpios[] = { diff --git a/Firmware/MotorControl/axis.cpp b/Firmware/MotorControl/axis.cpp index aca94dd9..94ccd5be 100644 --- a/Firmware/MotorControl/axis.cpp +++ b/Firmware/MotorControl/axis.cpp @@ -8,8 +8,9 @@ #include "communication/interface_can.hpp" Axis::Axis(int axis_num, - const AxisHardwareConfig_t& hw_config, - Config_t& config, + uint16_t default_step_gpio_pin, + uint16_t default_dir_gpio_pin, + osPriority thread_priority, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, @@ -20,8 +21,9 @@ Axis::Axis(int axis_num, Endstop& min_endstop, Endstop& max_endstop) : axis_num_(axis_num), - hw_config_(hw_config), - config_(config), + default_step_gpio_pin_(default_step_gpio_pin), + default_dir_gpio_pin_(default_dir_gpio_pin), + thread_priority_(thread_priority), encoder_(encoder), sensorless_estimator_(sensorless_estimator), controller_(controller), @@ -47,8 +49,6 @@ Axis::Axis(int axis_num, trap_traj_.axis_ = this; min_endstop_.axis_ = this; max_endstop_.axis_ = this; - decode_step_dir_pins(); - watchdog_feed(); } Axis::LockinConfig_t Axis::default_calibration() { @@ -83,6 +83,19 @@ static void step_cb_wrapper(void* ctx) { reinterpret_cast(ctx)->step_cb(); } +bool Axis::apply_config() { + config_.parent = this; + decode_step_dir_pins(); + watchdog_feed(); + return true; +} + +void Axis::clear_config() { + config_ = {}; + config_.step_gpio_pin = default_step_gpio_pin_; + config_.dir_gpio_pin = default_dir_gpio_pin_; + config_.can_node_id = axis_num_; +} // @brief Sets up all components of the axis, // such as gate driver and encoder hardware. @@ -97,7 +110,7 @@ static void run_state_machine_loop_wrapper(void* ctx) { // @brief Starts run_state_machine_loop in a new thread void Axis::start_thread() { - osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t)); + osThreadDef(thread_def, run_state_machine_loop_wrapper, thread_priority_, 0, stack_size_ / sizeof(StackType_t)); thread_id_ = osThreadCreate(osThread(thread_def), this); thread_id_valid_ = true; } @@ -123,16 +136,6 @@ void Axis::step_cb() { controller_.input_pos_ += dir * config_.counts_per_step; controller_.input_pos_updated(); } -}; - -void Axis::load_default_step_dir_pin_config( - const AxisHardwareConfig_t& hw_config, Config_t* config) { - config->step_gpio_pin = hw_config.step_gpio_pin; - config->dir_gpio_pin = hw_config.dir_gpio_pin; -} - -void Axis::load_default_can_id(const int& id, Config_t& config){ - config.can_node_id = id; } void Axis::decode_step_dir_pins() { diff --git a/Firmware/MotorControl/axis.hpp b/Firmware/MotorControl/axis.hpp index e63cc98a..e74bcdbc 100644 --- a/Firmware/MotorControl/axis.hpp +++ b/Firmware/MotorControl/axis.hpp @@ -78,8 +78,9 @@ public: }; Axis(int axis_num, - const AxisHardwareConfig_t& hw_config, - Config_t& config, + uint16_t default_step_gpio_pin, + uint16_t default_dir_gpio_pin, + osPriority thread_priority, Encoder& encoder, SensorlessEstimator& sensorless_estimator, Controller& controller, @@ -90,6 +91,9 @@ public: Endstop& min_endstop, Endstop& max_endstop); + bool apply_config(); + void clear_config(); + bool setup(); void start_thread(); void signal_current_meas(); @@ -99,10 +103,6 @@ public: void set_step_dir_active(bool enable); void decode_step_dir_pins(); - static void load_default_step_dir_pin_config( - const AxisHardwareConfig_t& hw_config, Config_t* config); - static void load_default_can_id(const int& id, Config_t& config); - bool check_DRV_fault(); bool check_PSU_brownout(); bool do_checks(); @@ -199,9 +199,12 @@ public: void run_state_machine_loop(); + // hardware config int axis_num_; - const AxisHardwareConfig_t& hw_config_; - Config_t& config_; + uint16_t default_step_gpio_pin_; + uint16_t default_dir_gpio_pin_; + osPriority thread_priority_; + Config_t config_; Encoder& encoder_; SensorlessEstimator& sensorless_estimator_; diff --git a/Firmware/MotorControl/controller.cpp b/Firmware/MotorControl/controller.cpp index c15b1cbe..259eadb3 100644 --- a/Firmware/MotorControl/controller.cpp +++ b/Firmware/MotorControl/controller.cpp @@ -4,10 +4,10 @@ #include -Controller::Controller(Config_t& config) : - config_(config) -{ +bool Controller::apply_config() { + config_.parent = this; update_filter_gains(); + return true; } void Controller::reset() { @@ -32,7 +32,7 @@ void Controller::input_pos_updated() { bool Controller::select_encoder(size_t encoder_num) { if (encoder_num < AXIS_COUNT) { - Axis* ax = axes[encoder_num]; + Axis* ax = &axes[encoder_num]; if (config_.setpoints_in_cpr) { pos_estimate_src_ = &ax->encoder_.pos_cpr_; pos_wrap_src_ = &ax->encoder_.config_.cpr; @@ -181,8 +181,8 @@ bool Controller::update(float* torque_setpoint_output) { } break; case INPUT_MODE_MIRROR: { if (config_.axis_to_mirror < AXIS_COUNT) { - pos_setpoint_ = axes[config_.axis_to_mirror]->encoder_.pos_estimate_ * config_.mirror_ratio; - vel_setpoint_ = axes[config_.axis_to_mirror]->encoder_.vel_estimate_ * config_.mirror_ratio; + pos_setpoint_ = axes[config_.axis_to_mirror].encoder_.pos_estimate_ * config_.mirror_ratio; + vel_setpoint_ = axes[config_.axis_to_mirror].encoder_.vel_estimate_ * config_.mirror_ratio; } else { set_error(ERROR_INVALID_MIRROR_AXIS); return false; diff --git a/Firmware/MotorControl/controller.hpp b/Firmware/MotorControl/controller.hpp index b1b80afd..ada8f922 100644 --- a/Firmware/MotorControl/controller.hpp +++ b/Firmware/MotorControl/controller.hpp @@ -44,7 +44,10 @@ public: void set_input_filter_bandwidth(float value) { input_filter_bandwidth = value; parent->update_filter_gains(); } }; - explicit Controller(Config_t& config); + Controller() {} + + bool apply_config(); + void reset(); void set_error(Error error); @@ -62,7 +65,7 @@ public: void update_filter_gains(); bool update(float* torque_setpoint); - Config_t& config_; + Config_t config_; Axis* axis_ = nullptr; // set by Axis constructor Error error_ = ERROR_NONE; diff --git a/Firmware/MotorControl/endstop.cpp b/Firmware/MotorControl/endstop.cpp index 585b4bf2..67948f95 100644 --- a/Firmware/MotorControl/endstop.cpp +++ b/Firmware/MotorControl/endstop.cpp @@ -1,11 +1,5 @@ #include -Endstop::Endstop(Endstop::Config_t& config) - : config_(config) { - update_config(); - debounceTimer_.setIncrement(current_meas_period); -} - void Endstop::update() { debounceTimer_.update(); @@ -29,9 +23,10 @@ bool Endstop::get_state() { return endstop_state_; } -void Endstop::update_config() { +bool Endstop::apply_config() { set_enabled(config_.enabled); debounceTimer_.setIncrement(config_.debounce_ms * 0.001f); + return true; } void Endstop::set_enabled(bool enable) { diff --git a/Firmware/MotorControl/endstop.hpp b/Firmware/MotorControl/endstop.hpp index f108dffe..0346c595 100644 --- a/Firmware/MotorControl/endstop.hpp +++ b/Firmware/MotorControl/endstop.hpp @@ -14,17 +14,17 @@ class Endstop { // custom setters Endstop* parent = nullptr; - void set_gpio_num(uint16_t value) { gpio_num = value; parent->update_config(); } - void set_enabled(uint32_t value) { enabled = value; parent->update_config(); } - void set_debounce_ms(uint32_t value) { debounce_ms = value; parent->update_config(); } + void set_gpio_num(uint16_t value) { gpio_num = value; parent->apply_config(); } + void set_enabled(uint32_t value) { enabled = value; parent->apply_config(); } + void set_debounce_ms(uint32_t value) { debounce_ms = value; parent->apply_config(); } }; - explicit Endstop(Endstop::Config_t& config); + Endstop() {} - Endstop::Config_t& config_; + Endstop::Config_t config_; Axis* axis_ = nullptr; - void update_config(); + bool apply_config(); void set_enabled(bool enabled); void update(); diff --git a/Firmware/MotorControl/low_level.cpp b/Firmware/MotorControl/low_level.cpp index bb94ba7c..899f0c21 100644 --- a/Firmware/MotorControl/low_level.cpp +++ b/Firmware/MotorControl/low_level.cpp @@ -76,8 +76,8 @@ bool brake_resistor_saturated = false; void low_level_fault(Motor::Error error) { // Disable all motors NOW! for (size_t i = 0; i < AXIS_COUNT; ++i) { - safety_critical_disarm_motor_pwm(axes[i]->motor_); - axes[i]->motor_.error_ |= error; + safety_critical_disarm_motor_pwm(axes[i].motor_); + axes[i].motor_.error_ |= error; } safety_critical_disarm_brake_resistor(); @@ -162,7 +162,7 @@ void safety_critical_disarm_brake_resistor() { htim2.Instance->CCR3 = 0; htim2.Instance->CCR4 = TIM_APB1_PERIOD_CLOCKS + 1; for (size_t i = 0; i < AXIS_COUNT; ++i) { - safety_critical_disarm_motor_pwm(axes[i]->motor_); + safety_critical_disarm_motor_pwm(axes[i].motor_); } cpu_exit_critical(mask); } @@ -190,7 +190,7 @@ void safety_critical_apply_brake_resistor_timings(uint32_t low_off, uint32_t hig void start_adc_pwm() { // Disarm motors for (size_t i = 0; i < AXIS_COUNT; ++i) { - safety_critical_disarm_motor_pwm(axes[i]->motor_); + safety_critical_disarm_motor_pwm(axes[i].motor_); } for (Motor& motor: motors) { @@ -395,9 +395,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { // Motor 1 is on Timer 8, which triggers ADC 2 and 3 on a regular conversion // If the corresponding timer is counting up, we just sampled in SVM vector 0, i.e. real current // If we are counting down, we just sampled in SVM vector 7, with zero current - Axis& axis = injected ? *axes[0] : *axes[1]; + Axis& axis = injected ? axes[0] : axes[1]; int axis_num = injected ? 0 : 1; - Axis& other_axis = injected ? *axes[1] : *axes[0]; + Axis& other_axis = injected ? axes[1] : axes[0]; bool counting_down = axis.motor_.timer_->Instance->CR1 & TIM_CR1_DIR; bool current_meas_not_DC_CAL = !counting_down; @@ -409,9 +409,9 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { bool update_timings = false; if (hadc == &hadc2) { - if (&axis == axes[1] && counting_down) + if (&axis == &axes[1] && counting_down) update_timings = true; // update timings of M0 - else if (&axis == axes[0] && !counting_down) + else if (&axis == &axes[0] && !counting_down) update_timings = true; // update timings of M1 // TODO: this is out of place here. However when moving it somewhere @@ -484,8 +484,8 @@ void pwm_trig_adc_cb(ADC_HandleTypeDef* hadc, bool injected) { void update_brake_current() { 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 (axes[i].motor_.armed_state_ == Motor::ARMED_STATE_ARMED) { + Ibus_sum += axes[i].motor_.current_control_.Ibus; } } diff --git a/Firmware/MotorControl/main.cpp b/Firmware/MotorControl/main.cpp index fec1dd4c..e91adf1f 100644 --- a/Firmware/MotorControl/main.cpp +++ b/Firmware/MotorControl/main.cpp @@ -31,15 +31,6 @@ extern char _estack; // provided by the linker script ODriveCAN::Config_t can_config; -SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT]; -Controller::Config_t controller_configs[AXIS_COUNT]; -OffboardThermistorCurrentLimiter::Config_t motor_thermistor_configs[AXIS_COUNT]; -Axis::Config_t axis_configs[AXIS_COUNT]; -TrapezoidalTrajectory::Config_t trap_configs[AXIS_COUNT]; -Endstop::Config_t min_endstop_configs[AXIS_COUNT]; -Endstop::Config_t max_endstop_configs[AXIS_COUNT]; - -std::array axes; ODriveCAN *odCAN = nullptr; ODrive odrv{}; @@ -52,15 +43,15 @@ static bool config_pop_all() { config_manager.pop(&can_config); for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = config_manager.pop(&encoders[i].config_) && - config_manager.pop(&sensorless_configs[i]) && - config_manager.pop(&controller_configs[i]) && - config_manager.pop(&trap_configs[i]) && - config_manager.pop(&min_endstop_configs[i]) && - config_manager.pop(&max_endstop_configs[i]) && + config_manager.pop(&axes[i].sensorless_estimator_.config_) && + config_manager.pop(&axes[i].controller_.config_) && + config_manager.pop(&axes[i].trap_traj_.config_) && + config_manager.pop(&axes[i].min_endstop_.config_) && + config_manager.pop(&axes[i].max_endstop_.config_) && config_manager.pop(&motors[i].config_) && config_manager.pop(&fet_thermistors[i].config_) && - config_manager.pop(&motor_thermistor_configs[i]) && - config_manager.pop(&axis_configs[i]); + config_manager.pop(&axes[i].motor_thermistor_.config_) && + config_manager.pop(&axes[i].config_); } return success; } @@ -71,15 +62,15 @@ static bool config_push_all() { config_manager.push(&can_config); for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = config_manager.push(&encoders[i].config_) && - config_manager.push(&sensorless_configs[i]) && - config_manager.push(&controller_configs[i]) && - config_manager.push(&trap_configs[i]) && - config_manager.push(&min_endstop_configs[i]) && - config_manager.push(&max_endstop_configs[i]) && + config_manager.push(&axes[i].sensorless_estimator_.config_) && + config_manager.push(&axes[i].controller_.config_) && + config_manager.push(&axes[i].trap_traj_.config_) && + config_manager.push(&axes[i].min_endstop_.config_) && + config_manager.push(&axes[i].max_endstop_.config_) && config_manager.push(&motors[i].config_) && config_manager.push(&fet_thermistors[i].config_) && - config_manager.push(&motor_thermistor_configs[i]) && - config_manager.push(&axis_configs[i]); + config_manager.push(&axes[i].motor_thermistor_.config_) && + config_manager.push(&axes[i].config_); } return success; } @@ -89,18 +80,17 @@ static void config_clear_all() { can_config = {}; for (size_t i = 0; i < AXIS_COUNT; ++i) { encoders[i].config_ = {}; - sensorless_configs[i] = {}; - controller_configs[i] = {}; - trap_configs[i] = {}; + axes[i].sensorless_estimator_.config_ = {}; + axes[i].controller_.config_ = {}; + axes[i].trap_traj_.config_ = {}; + axes[i].min_endstop_.config_ = {}; + axes[i].max_endstop_.config_ = {}; + axes[i].controller_.config_ = {}; + axes[i].controller_.config_.load_encoder_axis = i; motors[i].config_ = {}; fet_thermistors[i].config_ = {}; - axis_configs[i] = {}; - // Default step/dir pins are different, so we need to explicitly load them - Axis::load_default_step_dir_pin_config(hw_configs[i].axis_config, &axis_configs[i]); - Axis::load_default_can_id(i, axis_configs[i]); - min_endstop_configs[i] = {}; - max_endstop_configs[i] = {}; - controller_configs[i].load_encoder_axis = i; + axes[i].motor_thermistor_.config_ = {}; + axes[i].clear_config(); } } @@ -108,7 +98,11 @@ static bool config_apply_all() { bool success = true; for (size_t i = 0; (i < AXIS_COUNT) && success; ++i) { success = encoders[i].apply_config(motors[i].config_.motor_type) - && motors[i].apply_config(); + && motors[i].apply_config() + && axes[i].controller_.apply_config() + && axes[i].min_endstop_.apply_config() + && axes[i].max_endstop_.apply_config() + && axes[i].apply_config(); } return success; } @@ -175,7 +169,7 @@ extern "C" { void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed portCHAR *pcTaskName) { for(auto& axis : axes){ - safety_critical_disarm_motor_pwm(axis->motor_); + safety_critical_disarm_motor_pwm(axis.motor_); } safety_critical_disarm_brake_resistor(); for (;;); // TODO: safe action @@ -186,7 +180,7 @@ void vApplicationIdleHook(void) { odrv.system_stats_.uptime = xTaskGetTickCount(); odrv.system_stats_.min_heap_space = xPortGetMinimumEverFreeHeapSize(); uint32_t min_stack_space[AXIS_COUNT]; - std::transform(axes.begin(), axes.end(), std::begin(min_stack_space), [](auto& axis) { return uxTaskGetStackHighWaterMark(axes[1]->thread_id_) * sizeof(StackType_t); }); + std::transform(axes.begin(), axes.end(), std::begin(min_stack_space), [](auto& axis) { return uxTaskGetStackHighWaterMark(axis.thread_id_) * sizeof(StackType_t); }); odrv.system_stats_.min_stack_space_axis = *std::min_element(std::begin(min_stack_space), std::end(min_stack_space)); odrv.system_stats_.min_stack_space_usb = uxTaskGetStackHighWaterMark(usb_thread) * sizeof(StackType_t); odrv.system_stats_.min_stack_space_uart = uxTaskGetStackHighWaterMark(uart_thread) * sizeof(StackType_t); @@ -195,7 +189,7 @@ void vApplicationIdleHook(void) { odrv.system_stats_.min_stack_space_can = uxTaskGetStackHighWaterMark(odCAN->thread_id_) * sizeof(StackType_t); // Actual usage, in bytes, so we don't have to math - odrv.system_stats_.stack_usage_axis = axes[0]->stack_size_ - odrv.system_stats_.min_stack_space_axis; + odrv.system_stats_.stack_usage_axis = axes[0].stack_size_ - odrv.system_stats_.min_stack_space_axis; odrv.system_stats_.stack_usage_usb = stack_size_usb_thread - odrv.system_stats_.min_stack_space_usb; odrv.system_stats_.stack_usage_uart = stack_size_uart_thread - odrv.system_stats_.min_stack_space_uart; odrv.system_stats_.stack_usage_usb_irq = stack_size_usb_irq_thread - odrv.system_stats_.min_stack_space_usb_irq; @@ -228,13 +222,13 @@ static void rtos_main(void*) { // Setup hardware for all components for (size_t i = 0; i < AXIS_COUNT; ++i) { - if (!axes[i]->setup()) { + if (!axes[i].setup()) { for (;;); // TODO: proper error handling } } for(auto& axis : axes){ - axis->encoder_.setup(); + axis.encoder_.setup(); } // Start PWM and enable adc interrupts/callbacks @@ -252,7 +246,7 @@ static void rtos_main(void*) { // procedures and then run the actual controller loops. // TODO: generalize for AXIS_COUNT != 2 for (size_t i = 0; i < AXIS_COUNT; ++i) { - axes[i]->start_thread(); + axes[i].start_thread(); } start_analog_thread(); @@ -492,22 +486,6 @@ extern "C" int main(void) { // Construct all objects. odCAN = new ODriveCAN(can_config, &hcan1); - for (size_t i = 0; i < AXIS_COUNT; ++i) { - SensorlessEstimator *sensorless_estimator = new SensorlessEstimator(sensorless_configs[i]); - Controller *controller = new Controller(controller_configs[i]); - OffboardThermistorCurrentLimiter *motor_thermistor = new OffboardThermistorCurrentLimiter(motor_thermistor_configs[i]); - TrapezoidalTrajectory *trap = new TrapezoidalTrajectory(trap_configs[i]); - Endstop *min_endstop = new Endstop(min_endstop_configs[i]); - Endstop *max_endstop = new Endstop(max_endstop_configs[i]); - axes[i] = new Axis(i, hw_configs[i].axis_config, axis_configs[i], - encoders[i], *sensorless_estimator, *controller, fet_thermistors[i], *motor_thermistor, motors[i], *trap, *min_endstop, *max_endstop); - - controller_configs[i].parent = controller; - motor_thermistor_configs[i].parent = motor_thermistor; - min_endstop_configs[i].parent = min_endstop; - max_endstop_configs[i].parent = max_endstop; - axis_configs[i].parent = axes[i]; - } // Create main thread osThreadDef(defaultTask, rtos_main, osPriorityNormal, 0, stack_size_default_task / sizeof(StackType_t)); diff --git a/Firmware/MotorControl/motor.hpp b/Firmware/MotorControl/motor.hpp index ca099dba..9c3c863d 100644 --- a/Firmware/MotorControl/motor.hpp +++ b/Firmware/MotorControl/motor.hpp @@ -123,7 +123,6 @@ public: void tim_update_cb(); // hardware config - TIM_HandleTypeDef* const timer_; const uint16_t control_deadline_; const float shunt_conductance_; diff --git a/Firmware/MotorControl/odrive_main.h b/Firmware/MotorControl/odrive_main.h index 9dd87ded..c4ba1399 100644 --- a/Firmware/MotorControl/odrive_main.h +++ b/Firmware/MotorControl/odrive_main.h @@ -109,7 +109,6 @@ class Axis; class Motor; class ODriveCAN; -extern std::array axes; extern ODriveCAN *odCAN; // if you use the oscilloscope feature you can bump up this value @@ -178,7 +177,7 @@ public: return cnt += delta; } - Axis& get_axis(int num) { return *axes[num]; } + Axis& get_axis(int num) { return axes[num]; } ODriveCAN& get_can() { return *odCAN; } float& vbus_voltage_ = ::vbus_voltage; // TODO: make this the actual variable diff --git a/Firmware/MotorControl/sensorless_estimator.cpp b/Firmware/MotorControl/sensorless_estimator.cpp index aebbc09b..d70c7d2d 100644 --- a/Firmware/MotorControl/sensorless_estimator.cpp +++ b/Firmware/MotorControl/sensorless_estimator.cpp @@ -1,10 +1,6 @@ #include "odrive_main.h" -SensorlessEstimator::SensorlessEstimator(Config_t& config) : - config_(config) - {}; - bool SensorlessEstimator::update() { // Algorithm based on paper: Sensorless Control of Surface-Mount Permanent-Magnet Synchronous Motors Based on a Nonlinear Observer // http://cas.ensmp.fr/~praly/Telechargement/Journaux/2010-IEEE_TPEL-Lee-Hong-Nam-Ortega-Praly-Astolfi.pdf diff --git a/Firmware/MotorControl/sensorless_estimator.hpp b/Firmware/MotorControl/sensorless_estimator.hpp index 95992ae0..9f59b28a 100644 --- a/Firmware/MotorControl/sensorless_estimator.hpp +++ b/Firmware/MotorControl/sensorless_estimator.hpp @@ -9,12 +9,10 @@ public: float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / ( * ) } }; - explicit SensorlessEstimator(Config_t& config); - bool update(); Axis* axis_ = nullptr; // set by Axis constructor - Config_t& config_; + Config_t config_; // TODO: expose on protocol Error error_ = ERROR_NONE; diff --git a/Firmware/MotorControl/thermistor.cpp b/Firmware/MotorControl/thermistor.cpp index eb182420..3baf78a0 100644 --- a/Firmware/MotorControl/thermistor.cpp +++ b/Firmware/MotorControl/thermistor.cpp @@ -59,14 +59,13 @@ OnboardThermistorCurrentLimiter::OnboardThermistorCurrentLimiter(uint16_t adc_ch { } -OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter(Config_t& config) : +OffboardThermistorCurrentLimiter::OffboardThermistorCurrentLimiter() : ThermistorCurrentLimiter(UINT16_MAX, - &config.thermistor_poly_coeffs[0], + &config_.thermistor_poly_coeffs[0], num_coeffs_, - config.temp_limit_lower, - config.temp_limit_upper, - config.enabled), - config_(config) + config_.temp_limit_lower, + config_.temp_limit_upper, + config_.enabled) { decode_pin(); } diff --git a/Firmware/MotorControl/thermistor.hpp b/Firmware/MotorControl/thermistor.hpp index 000f8f79..a7c29cfe 100644 --- a/Firmware/MotorControl/thermistor.hpp +++ b/Firmware/MotorControl/thermistor.hpp @@ -64,9 +64,9 @@ public: }; virtual ~OffboardThermistorCurrentLimiter() = default; - OffboardThermistorCurrentLimiter(Config_t& config); + OffboardThermistorCurrentLimiter(); - Config_t& config_; + Config_t config_; private: void decode_pin(); diff --git a/Firmware/MotorControl/trapTraj.cpp b/Firmware/MotorControl/trapTraj.cpp index dd7f64a2..b3afd946 100644 --- a/Firmware/MotorControl/trapTraj.cpp +++ b/Firmware/MotorControl/trapTraj.cpp @@ -15,8 +15,6 @@ float sign_hard(float val) { // Vmax, Amax, Dmax and jmax Kinematic bounds // Ar, Dr and Vr Reached values of acceleration and velocity -TrapezoidalTrajectory::TrapezoidalTrajectory(Config_t& config) : config_(config) {} - bool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax) { float dX = Xf - Xi; // Distance to travel diff --git a/Firmware/MotorControl/trapTraj.hpp b/Firmware/MotorControl/trapTraj.hpp index c3df57b9..0de020b6 100644 --- a/Firmware/MotorControl/trapTraj.hpp +++ b/Firmware/MotorControl/trapTraj.hpp @@ -15,13 +15,12 @@ public: float Ydd; }; - explicit TrapezoidalTrajectory(Config_t& config); bool planTrapezoidal(float Xf, float Xi, float Vi, float Vmax, float Amax, float Dmax); Step_t eval(float t); Axis* axis_ = nullptr; // set by Axis constructor - Config_t& config_; + Config_t config_; float Xi_; float Xf_; diff --git a/Firmware/communication/ascii_protocol.cpp b/Firmware/communication/ascii_protocol.cpp index 5efbed46..31589f0c 100644 --- a/Firmware/communication/ascii_protocol.cpp +++ b/Firmware/communication/ascii_protocol.cpp @@ -103,15 +103,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - Axis* axis = axes[motor_number]; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis->controller_.input_pos_ = pos_setpoint; + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = pos_setpoint; if (numscan >= 3) - axis->controller_.input_vel_ = vel_feed_forward; + axis.controller_.input_vel_ = vel_feed_forward; if (numscan >= 4) - axis->controller_.input_torque_ = torque_feed_forward; - axis->controller_.input_pos_updated(); - axis->watchdog_feed(); + axis.controller_.input_torque_ = torque_feed_forward; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); } } else if (cmd[0] == 'q') { // position control with limits @@ -123,15 +123,15 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - Axis* axis = axes[motor_number]; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis->controller_.input_pos_ = pos_setpoint; + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = pos_setpoint; if (numscan >= 3) - axis->controller_.config_.vel_limit = vel_limit; + axis.controller_.config_.vel_limit = vel_limit; if (numscan >= 4) - axis->motor_.config_.torque_lim = torque_lim; - axis->controller_.input_pos_updated(); - axis->watchdog_feed(); + axis.motor_.config_.torque_lim = torque_lim; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); } } else if (cmd[0] == 'v') { // velocity control @@ -143,12 +143,12 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - Axis* axis = axes[motor_number]; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; - axis->controller_.input_vel_ = vel_setpoint; + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL; + axis.controller_.input_vel_ = vel_setpoint; if (numscan >= 3) - axis->controller_.input_torque_ = torque_feed_forward; - axis->watchdog_feed(); + axis.controller_.input_torque_ = torque_feed_forward; + axis.watchdog_feed(); } } else if (cmd[0] == 'c') { // torque control @@ -160,10 +160,10 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - Axis* axis = axes[motor_number]; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL; - axis->controller_.input_torque_ = torque_setpoint; - axis->watchdog_feed(); + Axis& axis = axes[motor_number]; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_TORQUE_CONTROL; + axis.controller_.input_torque_ = torque_setpoint; + axis.watchdog_feed(); } } else if (cmd[0] == 't') { // trapezoidal trajectory @@ -175,12 +175,12 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { - Axis* axis = axes[motor_number]; - axis->controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; - axis->controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; - axis->controller_.input_pos_ = goal_point; - axis->controller_.input_pos_updated(); - axis->watchdog_feed(); + Axis& axis = axes[motor_number]; + axis.controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ; + axis.controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL; + axis.controller_.input_pos_ = goal_point; + axis.controller_.input_pos_updated(); + axis.watchdog_feed(); } } else if (cmd[0] == 'f') { // feedback @@ -192,8 +192,8 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& respond(response_channel, use_checksum, "invalid motor %u", motor_number); } else { respond(response_channel, use_checksum, "%f %f", - (double)axes[motor_number]->encoder_.pos_estimate_, - (double)axes[motor_number]->encoder_.vel_estimate_); + (double)axes[motor_number].encoder_.pos_estimate_, + (double)axes[motor_number].encoder_.vel_estimate_); } } else if (cmd[0] == 'h') { // Help @@ -276,7 +276,7 @@ void ASCII_protocol_process_line(const uint8_t* buffer, size_t len, StreamSink& } else if (motor_number >= AXIS_COUNT) { respond(response_channel, use_checksum, "invalid motor %u", motor_number); }else { - axes[motor_number]->watchdog_feed(); + axes[motor_number].watchdog_feed(); } } else if (cmd[0] != 0) { diff --git a/Firmware/communication/can_simple.cpp b/Firmware/communication/can_simple.cpp index 18d39838..b3c29ff9 100644 --- a/Firmware/communication/can_simple.cpp +++ b/Firmware/communication/can_simple.cpp @@ -26,8 +26,8 @@ void CANSimple::handle_can_message(can_Message_t& msg) { bool validAxis = false; for (uint8_t i = 0; i < AXIS_COUNT; i++) { - if ((axes[i]->config_.can_node_id == nodeID) && (axes[i]->config_.can_node_id_extended == msg.isExt)) { - axis = axes[i]; + if ((axes[i].config_.can_node_id == nodeID) && (axes[i].config_.can_node_id_extended == msg.isExt)) { + axis = &axes[i]; if (!validAxis) { validAxis = true; } else {