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

Make Endstops objects instead of structs in Axis

Browse Source
This commit is contained in:
Unknown
2018-09-08 18:25:51 -04:00
parent 21bdf453c3
commit 9b0bc6f840
9 changed files with 123 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
CHANGELOG.md
+2
View File
@@ -3,6 +3,8 @@ Please add a note of your changes below this heading if you make a Pull Request.
## Unreleased
### Added
* `min_endstop` and `max_endstop` objects can be configured on GPIO
* Axes can be homed if `min_endstop` is enabled
* Encoder position count "homed" to zero when index is found.
### Changed
Firmware/MotorControl/axis.cpp
+13 -84
View File
@@ -11,31 +11,30 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor)
Motor& motor,
Endstop& min_endstop,
Endstop& max_endstop)
: hw_config_(hw_config),
config_(config),
encoder_(encoder),
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor)
motor_(motor),
min_endstop_(min_endstop),
max_endstop_(max_endstop)
{
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
motor_.axis_ = this;
min_endstop_.axis_ = this;
max_endstop_.axis_ = this;
}
static void step_cb_wrapper(void* ctx) {
reinterpret_cast<Axis*>(ctx)->step_cb();
}
static void min_endstop_cb_wrapper(void* ctx){
reinterpret_cast<Axis*>(ctx)->min_endstop_cb();
}
static void max_endstop_cb_wrapper(void* ctx){
reinterpret_cast<Axis*>(ctx)->max_endstop_cb();
}
// @brief Sets up all components of the axis,
// such as gate driver and encoder hardware.
@@ -101,73 +100,6 @@ void Axis::set_step_dir_enabled(bool enable) {
}
}
void Axis::min_endstop_cb(){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num);
if(config_.min_endstop.enabled){
min_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
if(config_.min_endstop.is_active_high == false)
min_endstop_state_ = !min_endstop_state_;
} else {
min_endstop_state_ = false;
}
}
void Axis::set_min_endstop_enabled(bool enable){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.min_endstop.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.min_endstop.gpio_num);
if(enable){
HAL_GPIO_DeInit(gpio_port, gpio_pin);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
uint32_t pull_up_down = config_.min_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;
uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode,
min_endstop_cb_wrapper, this);
}
else {
GPIO_unsubscribe(gpio_port, gpio_pin);
}
}
void Axis::max_endstop_cb(){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num);
if(config_.max_endstop.enabled){
max_endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
if(config_.max_endstop.is_active_high == false)
max_endstop_state_ = !max_endstop_state_;
} else {
max_endstop_state_ = false;
}
}
void Axis::set_max_endstop_enabled(bool enable){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.max_endstop.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.max_endstop.gpio_num);
if(enable){
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
uint32_t pull_up_down = config_.max_endstop.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;
uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING; // Need to track pin state, not just homing edges
GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode,
max_endstop_cb_wrapper, this);
}
else {
GPIO_unsubscribe(gpio_port, gpio_pin);
}
}
bool Axis::check_for_errors() {
// Maybe we should update this to only trigger on new errors?
// The danger with that is we could fail to bail on uncleared errors that still prevent
@@ -282,20 +214,20 @@ bool Axis::run_closed_loop_control_loop() {
// Handle the homing case
if (homing_state_ == HOMING_STATE_HOMING) {
if (min_endstop_state_) {
encoder_.set_linear_count(config_.min_endstop.offset);
if (min_endstop_.endstop_state_) {
encoder_.set_linear_count(min_endstop_.config_.offset);
controller_.set_pos_setpoint(0.0f, 0.0f, 0.0f);
homing_state_ = HOMING_STATE_MOVE_TO_ZERO;
}
} else if (homing_state_ == HOMING_STATE_MOVE_TO_ZERO) {
if(!min_endstop_state_){
if(!min_endstop_.endstop_state_){
homing_state_ = HOMING_STATE_IDLE;
}
} else {
// Check for endstop presses
if (config_.min_endstop.enabled && min_endstop_state_) {
if (min_endstop_.config_.enabled && min_endstop_.endstop_state_) {
return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false;
} else if (config_.max_endstop.enabled && max_endstop_state_) {
} else if (max_endstop_.config_.enabled && max_endstop_.endstop_state_) {
return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false;
}
}
@@ -317,9 +249,6 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
set_min_endstop_enabled(config_.min_endstop.enabled);
set_max_endstop_enabled(config_.max_endstop.enabled);
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
// TODO: Move this somewhere else
// TODO: respect changes of CPR
Firmware/MotorControl/axis.hpp
+9 -31
View File
@@ -26,13 +26,6 @@ enum HomingState_t {
HOMING_STATE_MOVE_TO_ZERO
};
struct Endstop_t {
uint16_t gpio_num;
bool enabled = false;
int32_t offset = 0;
bool is_active_high = false;
};
struct AxisConfig_t {
bool startup_motor_calibration = false; //<! run motor calibration at startup, skip otherwise
bool startup_encoder_index_search = false; //<! run encoder index search after startup, skip otherwise
@@ -52,8 +45,6 @@ struct AxisConfig_t {
float spin_up_current = 10.0f; // [A]
float spin_up_acceleration = 400.0f; // [rad/s^2]
float spin_up_target_vel = 400.0f; // [rad/s]
Endstop_t min_endstop;
Endstop_t max_endstop;
};
class Axis {
@@ -84,7 +75,9 @@ public:
Encoder& encoder,
SensorlessEstimator& sensorless_estimator,
Controller& controller,
Motor& motor);
Motor& motor,
Endstop& min_endstop,
Endstop& max_endstop);
void setup();
void start_thread();
@@ -93,10 +86,7 @@ public:
void step_cb();
void set_step_dir_enabled(bool enable);
void min_endstop_cb();
void set_min_endstop_enabled(bool enable);
void max_endstop_cb();
void set_max_endstop_enabled(bool enable);
bool check_DRV_fault();
bool check_PSU_brownout();
@@ -173,6 +163,8 @@ public:
SensorlessEstimator& sensorless_estimator_;
Controller& controller_;
Motor& motor_;
Endstop& min_endstop_;
Endstop& max_endstop_;
osThreadId thread_id_;
volatile bool thread_id_valid_ = false;
@@ -184,8 +176,6 @@ public:
AxisState_t task_chain_[10] = { AXIS_STATE_UNDEFINED };
AxisState_t& current_state_ = task_chain_[0];
uint32_t loop_counter_ = 0;
bool min_endstop_state_ = false;
bool max_endstop_state_ = false;
HomingState_t homing_state_ = HOMING_STATE_IDLE;
// Communication protocol definitions
@@ -196,8 +186,6 @@ public:
make_protocol_ro_property("current_state", &current_state_),
make_protocol_property("requested_state", &requested_state_),
make_protocol_ro_property("loop_counter", &loop_counter_),
make_protocol_ro_property("min_endstop_state", &min_endstop_state_),
make_protocol_ro_property("max_endstop_state", &max_endstop_state_),
make_protocol_ro_property("homing_state", &homing_state_),
make_protocol_object("config",
make_protocol_property("startup_motor_calibration", &config_.startup_motor_calibration),
@@ -212,24 +200,14 @@ public:
make_protocol_property("ramp_up_distance", &config_.ramp_up_distance),
make_protocol_property("spin_up_current", &config_.spin_up_current),
make_protocol_property("spin_up_acceleration", &config_.spin_up_acceleration),
make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel),
make_protocol_object("min_endstop",
make_protocol_property("gpio_num", &config_.min_endstop.gpio_num),
make_protocol_property("enabled", &config_.min_endstop.enabled),
make_protocol_property("offset", &config_.min_endstop.offset),
make_protocol_property("is_active_high", &config_.min_endstop.is_active_high)
),
make_protocol_object("max_endstop",
make_protocol_property("gpio_num", &config_.max_endstop.gpio_num),
make_protocol_property("enabled", &config_.max_endstop.enabled),
make_protocol_property("offset", &config_.max_endstop.offset),
make_protocol_property("is_active_high", &config_.max_endstop.is_active_high)
)
make_protocol_property("spin_up_target_vel", &config_.spin_up_target_vel)
),
make_protocol_function("get_temp", *this, &Axis::get_temp),
make_protocol_object("motor", motor_.make_protocol_definitions()),
make_protocol_object("controller", controller_.make_protocol_definitions()),
make_protocol_object("encoder", encoder_.make_protocol_definitions()),
make_protocol_object("min_endstop", min_endstop_.make_protocol_definitions()),
make_protocol_object("max_endstop", max_endstop_.make_protocol_definitions()),
make_protocol_object("sensorless_estimator", sensorless_estimator_.make_protocol_definitions())
);
}
Firmware/MotorControl/controller.cpp
+1 -1
View File
@@ -54,7 +54,7 @@ void Controller::start_anticogging_calibration() {
// Slowly drive in the negative direction at homing_speed until the min endstop is pressed
// When pressed, set the linear count to the offset (default 0), and then
bool Controller::home_axis() {
if (axis_->config_.min_endstop.enabled) {
if (axis_->min_endstop_.config_.enabled) {
set_vel_setpoint(-config_.homing_speed, 0.0f);
axis_->homing_state_ = HOMING_STATE_HOMING;
} else {
Firmware/MotorControl/endstop.cpp
+43
View File
@@ -0,0 +1,43 @@
#include <odrive_main.h>
Endstop::Endstop(EndstopConfig_t &config)
: config_(config) {
}
static void endstop_cb_wrapper(void* ctx){
reinterpret_cast<Endstop*>(ctx)->endstop_cb();
}
void Endstop::endstop_cb(){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
if(config_.enabled){
endstop_state_ = HAL_GPIO_ReadPin(gpio_port, gpio_pin);
if(config_.is_active_high == false)
endstop_state_ = !endstop_state_;
} else {
endstop_state_ = false;
}
}
void Endstop::set_endstop_enabled(bool enable){
uint16_t gpio_pin = get_gpio_pin_by_pin(config_.gpio_num);
GPIO_TypeDef* gpio_port = get_gpio_port_by_pin(config_.gpio_num);
if(enable){
HAL_GPIO_DeInit(gpio_port, gpio_pin);
GPIO_InitTypeDef GPIO_InitStruct;
GPIO_InitStruct.Pin = gpio_pin;
GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(gpio_port, &GPIO_InitStruct);
uint32_t pull_up_down = config_.is_active_high ? GPIO_PULLDOWN : GPIO_PULLUP;
uint32_t interrupt_mode = GPIO_MODE_IT_RISING_FALLING;
GPIO_subscribe(gpio_port, gpio_pin, pull_up_down, interrupt_mode,
endstop_cb_wrapper, this);
}
else {
GPIO_unsubscribe(gpio_port, gpio_pin);
}
}
Firmware/MotorControl/endstop.hpp
+37
View File
@@ -0,0 +1,37 @@
#ifndef __ENDSTOP_HPP
#define __ENDSTOP_HPP
struct EndstopConfig_t {
uint16_t gpio_num;
bool enabled = false;
int32_t offset = 0;
bool is_active_high = false;
};
class Endstop {
public:
Endstop(EndstopConfig_t& config);
EndstopConfig_t config_;
Axis* axis_ = nullptr;
bool endstop_state_ = false;
void set_endstop_enabled(bool enable);
void endstop_cb();
auto make_protocol_definitions(){
return make_protocol_member_list(
make_protocol_object("config",
make_protocol_property("gpio_num", &config_.gpio_num),
make_protocol_property("enabled", &config_.enabled),
make_protocol_property("offset", &config_.offset),
make_protocol_property("is_active_high", &config_.is_active_high)
)
);
}
private:
uint16_t debounce_timer_ = 0;
};
#endif
Firmware/MotorControl/main.cpp
+16 -4
View File
@@ -14,6 +14,8 @@ SensorlessEstimator::Config_t sensorless_configs[AXIS_COUNT];
ControllerConfig_t controller_configs[AXIS_COUNT];
MotorConfig_t motor_configs[AXIS_COUNT];
AxisConfig_t axis_configs[AXIS_COUNT];
EndstopConfig_t min_endstop_configs[AXIS_COUNT];
EndstopConfig_t max_endstop_configs[AXIS_COUNT];
bool user_config_loaded_;
SystemStats_t system_stats_ = { 0 };
@@ -26,7 +28,9 @@ typedef Config<
SensorlessEstimator::Config_t[AXIS_COUNT],
ControllerConfig_t[AXIS_COUNT],
MotorConfig_t[AXIS_COUNT],
AxisConfig_t[AXIS_COUNT]> ConfigFormat;
AxisConfig_t[AXIS_COUNT],
EndstopConfig_t[AXIS_COUNT],
EndstopConfig_t[AXIS_COUNT]> ConfigFormat;
void save_configuration(void) {
if (ConfigFormat::safe_store_config(
@@ -35,7 +39,9 @@ void save_configuration(void) {
&sensorless_configs,
&controller_configs,
&motor_configs,
&axis_configs)) {
&axis_configs,
&min_endstop_configs,
&max_endstop_configs)) {
//printf("saving configuration failed\r\n"); osDelay(5);
} else {
user_config_loaded_ = true;
@@ -51,7 +57,9 @@ void load_configuration(void) {
&sensorless_configs,
&controller_configs,
&motor_configs,
&axis_configs)) {
&axis_configs,
&min_endstop_configs,
&max_endstop_configs)) {
//If loading failed, restore defaults
board_config = BoardConfig_t();
for (size_t i = 0; i < AXIS_COUNT; ++i) {
@@ -60,6 +68,8 @@ void load_configuration(void) {
controller_configs[i] = ControllerConfig_t();
motor_configs[i] = MotorConfig_t();
axis_configs[i] = AxisConfig_t();
min_endstop_configs[i] = EndstopConfig_t();
max_endstop_configs[i] = EndstopConfig_t();
}
} else {
user_config_loaded_ = true;
@@ -162,8 +172,10 @@ int odrive_main(void) {
Motor *motor = new Motor(hw_configs[i].motor_config,
hw_configs[i].gate_driver_config,
motor_configs[i]);
Endstop *min_endstop = new Endstop(min_endstop_configs[i]);
Endstop *max_endstop = new Endstop(max_endstop_configs[i]);
axes[i] = new Axis(hw_configs[i].axis_config, axis_configs[i],
*encoder, *sensorless_estimator, *controller, *motor);
*encoder, *sensorless_estimator, *controller, *motor, *min_endstop, *max_endstop);
}
// Start ADC for temperature measurements and user measurements
Firmware/MotorControl/odrive_main.h
+1
View File
@@ -109,6 +109,7 @@ inline ENUMTYPE operator ~ (ENUMTYPE a) { return static_cast<ENUMTYPE>(~static_c
#include <sensorless_estimator.hpp>
#include <controller.hpp>
#include <motor.hpp>
#include <endstop.hpp>
#include <axis.hpp>
#include <communication/communication.h>
Firmware/Tupfile.lua
+1
View File
@@ -153,6 +153,7 @@ build{
'MotorControl/axis.cpp',
'MotorControl/motor.cpp',
'MotorControl/encoder.cpp',
'MotorControl/endstop.cpp',
'MotorControl/controller.cpp',
'MotorControl/sensorless_estimator.cpp',
'MotorControl/main.cpp',