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

Fix bugs in homing by actually testing and using the debugger...

Browse Source
This commit is contained in:
Unknown
2018-09-02 01:59:35 -04:00
parent dd74eed54b
commit 21bdf453c3
3 changed files with 36 additions and 22 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+27 -14
View File
@@ -118,6 +118,7 @@ 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;
@@ -125,7 +126,7 @@ void Axis::set_min_endstop_enabled(bool enable){
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 = config_.min_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING;
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);
}
@@ -158,7 +159,7 @@ void Axis::set_max_endstop_enabled(bool enable){
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 = config_.max_endstop.is_active_high ? GPIO_MODE_IT_RISING : GPIO_MODE_IT_FALLING;
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);
}
@@ -251,8 +252,6 @@ bool Axis::run_sensorless_spin_up() {
// Note run_sensorless_control_loop and run_closed_loop_control_loop are very similar and differ only in where we get the estimate from.
bool Axis::run_sensorless_control_loop() {
set_min_endstop_enabled(config_.min_endstop.enabled);
set_max_endstop_enabled(config_.max_endstop.enabled);
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
@@ -273,20 +272,32 @@ bool Axis::run_sensorless_control_loop() {
bool Axis::run_closed_loop_control_loop() {
set_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
run_control_loop([this]() {
// Note that all estimators are updated in the loop prefix in run_control_loop
float current_setpoint;
if (!controller_.update(encoder_.pos_estimate_, encoder_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
return error_ |= ERROR_CONTROLLER_FAILED, false; //TODO: Make controller.set_error
if (!motor_.update(current_setpoint, encoder_.phase_))
return false; // set_error should update axis.error_
return false; // set_error should update axis.error_
// Check for endstop presses
if(config_.min_endstop.enabled && min_endstop_state_) {
return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false;
} else if(config_.max_endstop.enabled && max_endstop_state_) {
return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false;
// Handle the homing case
if (homing_state_ == HOMING_STATE_HOMING) {
if (min_endstop_state_) {
encoder_.set_linear_count(config_.min_endstop.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_){
homing_state_ = HOMING_STATE_IDLE;
}
} else {
// Check for endstop presses
if (config_.min_endstop.enabled && min_endstop_state_) {
return error_ |= ERROR_MIN_ENDSTOP_PRESSED, false;
} else if (config_.max_endstop.enabled && max_endstop_state_) {
return error_ |= ERROR_MAX_ENDSTOP_PRESSED, false;
}
}
return true;
});
@@ -306,6 +317,8 @@ 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
@@ -333,9 +346,9 @@ void Axis::run_state_machine_loop() {
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_closed_loop_control){
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
if(config_.startup_homing)
task_chain_[pos++] = AXIS_STATE_HOMING;
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
}
else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
Firmware/MotorControl/axis.hpp
+8
View File
@@ -20,6 +20,12 @@ enum AxisState_t {
AXIS_STATE_HOMING = 9 //<! run axis homing function
};
enum HomingState_t {
HOMING_STATE_IDLE,
HOMING_STATE_HOMING,
HOMING_STATE_MOVE_TO_ZERO
};
struct Endstop_t {
uint16_t gpio_num;
bool enabled = false;
@@ -180,6 +186,7 @@ public:
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
auto make_protocol_definitions() {
@@ -191,6 +198,7 @@ public:
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),
make_protocol_property("startup_encoder_index_search", &config_.startup_encoder_index_search),
Firmware/MotorControl/controller.cpp
+1 -8
View File
@@ -56,17 +56,10 @@ void Controller::start_anticogging_calibration() {
bool Controller::home_axis() {
if (axis_->config_.min_endstop.enabled) {
set_vel_setpoint(-config_.homing_speed, 0.0f);
axis_->homing_state_ = HOMING_STATE_HOMING;
} else {
return false;
}
axis_->run_control_loop([&](){
if(axis_->min_endstop_state_){
axis_->encoder_.set_linear_count(axis_->config_.min_endstop.offset);
set_pos_setpoint(0.0f, 0.0f, 0.0f);
}
return !axis_->min_endstop_state_;
});
return true;
}