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

Get CAN_SIMPLE to compile

Browse Source
This commit is contained in:
Unknown
2018-10-05 22:20:04 -04:00
parent c88d3f7a01
commit 4fa09d6bbc
6 changed files with 156 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Firmware/MotorControl/axis.cpp
+22 -21
View File
@@ -3,8 +3,9 @@
#include <functional>
#include "gpio.h"
#include "utils.h"
#include "odrive_main.h"
#include "utils.h"
#include "communication/interface_can.hpp"
Axis::Axis(const AxisHardwareConfig_t& hw_config,
Config_t& config,
@@ -19,8 +20,7 @@ Axis::Axis(const AxisHardwareConfig_t& hw_config,
sensorless_estimator_(sensorless_estimator),
controller_(controller),
motor_(motor),
trap_(trap)
{
trap_(trap) {
encoder_.axis_ = this;
sensorless_estimator_.axis_ = this;
controller_.axis_ = this;
@@ -46,7 +46,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, 4*512);
osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, 4 * 512);
thread_id_ = osThreadCreate(osThread(thread_def), this);
thread_id_valid_ = true;
}
@@ -85,7 +85,7 @@ void Axis::set_step_dir_enabled(bool enable) {
// Subscribe to rising edges of the step GPIO
GPIO_subscribe(hw_config_.step_port, hw_config_.step_pin, GPIO_PULLDOWN,
step_cb_wrapper, this);
step_cb_wrapper, this);
enable_step_dir_ = true;
} else {
@@ -136,7 +136,9 @@ bool Axis::do_updates() {
// Sub-components should use set_error which will propegate to this error_
encoder_.update();
sensorless_estimator_.update();
return check_for_errors();
bool ret = check_for_errors();
odCAN->send_heartbeat(this);
return ret;
}
float Axis::get_temp() {
@@ -148,7 +150,7 @@ float Axis::get_temp() {
bool Axis::run_sensorless_spin_up() {
// Early Spin-up: spiral up current
float x = 0.0f;
run_control_loop([&](){
run_control_loop([&]() {
float phase = wrap_pm_pi(config_.ramp_up_distance * x);
float I_mag = config_.spin_up_current * x;
x += current_meas_period / config_.ramp_up_time;
@@ -158,11 +160,11 @@ bool Axis::run_sensorless_spin_up() {
});
if (error_ != ERROR_NONE)
return false;
// Late Spin-up: accelerate
float vel = config_.ramp_up_distance / config_.ramp_up_time;
float phase = wrap_pm_pi(config_.ramp_up_distance);
run_control_loop([&](){
run_control_loop([&]() {
vel += config_.spin_up_acceleration * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
float I_mag = config_.spin_up_current;
@@ -181,7 +183,7 @@ 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_step_dir_enabled(config_.enable_step_dir);
run_control_loop([this](){
run_control_loop([this]() {
if (controller_.config_.control_mode >= Controller::CTRL_MODE_POSITION_CONTROL)
return error_ |= ERROR_POS_CTRL_DURING_SENSORLESS, false;
@@ -190,7 +192,7 @@ bool Axis::run_sensorless_control_loop() {
if (!controller_.update(sensorless_estimator_.pll_pos_, sensorless_estimator_.vel_estimate_, &current_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
if (!motor_.update(current_setpoint, sensorless_estimator_.phase_))
return false; // set_error should update axis.error_
return false; // set_error should update axis.error_
return true;
});
set_step_dir_enabled(false);
@@ -199,13 +201,13 @@ 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_
return true;
});
set_step_dir_enabled(false);
@@ -216,7 +218,7 @@ bool Axis::run_idle_loop() {
// run_control_loop ignores missed modulation timing updates
// if and only if we're in AXIS_STATE_IDLE
safety_critical_disarm_motor_pwm(motor_);
run_control_loop([this](){
run_control_loop([this]() {
return true;
});
return check_for_errors();
@@ -224,7 +226,6 @@ bool Axis::run_idle_loop() {
// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {
// Allocate the map for anti-cogging algorithm and initialize all values to 0.0f
// TODO: Move this somewhere else
// TODO: respect changes of CPR
@@ -238,7 +239,7 @@ void Axis::run_state_machine_loop() {
// arm!
motor_.arm();
for (;;) {
// Load the task chain if a specific request is pending
if (requested_state_ != AXIS_STATE_UNDEFINED) {
@@ -265,7 +266,7 @@ void Axis::run_state_machine_loop() {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
@@ -296,7 +297,7 @@ void Axis::run_state_machine_loop() {
break;
case AXIS_STATE_SENSORLESS_CONTROL:
status = run_sensorless_spin_up(); // TODO: restart if desired
status = run_sensorless_spin_up(); // TODO: restart if desired
if (status)
status = run_sensorless_control_loop();
break;
@@ -307,12 +308,12 @@ void Axis::run_state_machine_loop() {
case AXIS_STATE_IDLE:
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
status = motor_.arm(); // done with idling - try to arm the motor
break;
default:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
status = false; // this will set the state to idle
break;
}