Mirrors_Framework/ODrive
1
0
Fork 0
mirror of https://github.com/odriverobotics/ODrive.git synced 2026-09-21 23:44:48 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

make axis allocation static

Browse Source
This commit is contained in:
Samuel Sadok
2020-07-21 13:02:03 +02:00
parent b3ddadb360
commit 938f088fc2
20 changed files with 184 additions and 206 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/Board/v3/Inc/board.h
+1 -38
View File
@@ -77,6 +77,7 @@ using TOpAmp = Drv8301;
#include <MotorControl/motor.hpp>
#include <MotorControl/encoder.hpp>
extern std::array<Axis, AXIS_COUNT> 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
Firmware/Board/v3/board.cpp
+45
View File
@@ -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<Axis, AXIS_COUNT> 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[] = {
Firmware/MotorControl/axis.cpp
+20 -17
View File
@@ -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<Axis*>(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() {
Firmware/MotorControl/axis.hpp
+11 -8
View File
@@ -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_;
Firmware/MotorControl/controller.cpp
+6 -6
View File
@@ -4,10 +4,10 @@
#include <algorithm>
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;
Firmware/MotorControl/controller.hpp
+5 -2
View File
@@ -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;
Firmware/MotorControl/endstop.cpp
+2 -7
View File
@@ -1,11 +1,5 @@
#include <odrive_main.h>
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) {
Firmware/MotorControl/endstop.hpp
+6 -6
View File
@@ -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();
Firmware/MotorControl/low_level.cpp
+10 -10
View File
@@ -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;
}
}
Firmware/MotorControl/main.cpp
+34 -56
View File
@@ -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<Axis*, AXIS_COUNT> 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));
Firmware/MotorControl/motor.hpp
-1
View File
@@ -123,7 +123,6 @@ public:
void tim_update_cb();
// hardware config
TIM_HandleTypeDef* const timer_;
const uint16_t control_deadline_;
const float shunt_conductance_;
Firmware/MotorControl/odrive_main.h
+1 -2
View File
@@ -109,7 +109,6 @@ class Axis;
class Motor;
class ODriveCAN;
extern std::array<Axis*, AXIS_COUNT> 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
Firmware/MotorControl/sensorless_estimator.cpp
-4
View File
@@ -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
Firmware/MotorControl/sensorless_estimator.hpp
+1 -3
View File
@@ -9,12 +9,10 @@ public:
float pm_flux_linkage = 1.58e-3f; // [V / (rad/s)] { 5.51328895422 / (<pole pairs> * <rpm/v>) }
};
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;
Firmware/MotorControl/thermistor.cpp
+5 -6
View File
@@ -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();
}
Firmware/MotorControl/thermistor.hpp
+2 -2
View File
@@ -64,9 +64,9 @@ public:
};
virtual ~OffboardThermistorCurrentLimiter() = default;
OffboardThermistorCurrentLimiter(Config_t& config);
OffboardThermistorCurrentLimiter();
Config_t& config_;
Config_t config_;
private:
void decode_pin();
Firmware/MotorControl/trapTraj.cpp
-2
View File
@@ -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
Firmware/MotorControl/trapTraj.hpp
+1 -2
View File
@@ -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_;
Firmware/communication/ascii_protocol.cpp
+32 -32
View File
@@ -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) {
Firmware/communication/can_simple.cpp
+2 -2
View File
@@ -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 {