16.watchdog_feed()
这个函数必须定期调用,否则系统会认为“挂掉”了。
void Axis::watchdog_feed() {
watchdog_current_value_ = get_watchdog_reset(); //返回的是一个固定值(例如 1000),代表你设定的容忍超时周期数。
}
17.watchdog_check()
如果 watchdog_current_value_ 变成 0,就说明你很久没有调用 watchdog_feed()。在这种情况下系统会设置 ERROR_WATCHDOG_TIMER_EXPIRED,可能进入错误处理或强制停止电机。
bool Axis::watchdog_check() {
if (!config_.enable_watchdog) return true;
if (watchdog_current_value_ > 0) {
watchdog_current_value_--;
return true;
} else {
error_ |= ERROR_WATCHDOG_TIMER_EXPIRED;
return false;
}
}
18.run_lockin_spin(const LockinConfig_t &lockin_config)
这段代码实现了ODrive电机控制中的"锁定旋转"(Lock-in Spin)功能,主要用于电机启动时的转子位置识别和速度同步。以下是对代码的深度解析:
Lock-in Spin是电机启动时的关键过程,它通过三个阶段让电机从静止状态平滑过渡到目标速度:
- 电流斜坡上升阶段(Ramp)
- 加速阶段(Accelerate)
- 恒速阶段(Constant Velocity)
bool Axis::run_lockin_spin(const LockinConfig_t &lockin_config) {
// 电流斜坡阶段(Ramp):使用线性斜坡逐渐增加电流(从0到设定值),x从0到1线性变化,控制斜坡进度,保持速度为0,仅建立初始磁场。
lockin_state_ = LOCKIN_STATE_RAMP;
float x = 0.0f;
run_control_loop([&]() {
float phase = wrap_pm_pi(lockin_config.ramp_distance * x);
float torque = lockin_config.current * motor_.config_.torque_constant * x;
x += current_meas_period / lockin_config.ramp_time;
if (!motor_.update(torque, phase, 0.0f))
return false;
return x < 1.0f;
});
// Spin states
float distance = lockin_config.ramp_distance;
float phase = wrap_pm_pi(distance);
float vel = distance / lockin_config.ramp_time;
// 终止条件判断,灵活支持多种终止条件组合,vel_override用于强制检查速度条件
auto spin_done = [&](bool vel_override = false) -> bool {
bool done = false;
if (lockin_config.finish_on_vel || vel_override)
done = done || std::abs(vel) >= std::abs(lockin_config.vel);
if (lockin_config.finish_on_distance)
done = done || std::abs(distance) >= std::abs(lockin_config.finish_distance);
if (lockin_config.finish_on_enc_idx)
done = done || encoder_.index_found_;
return done;
};
// 加速阶段(Accelerate)
lockin_state_ = LOCKIN_STATE_ACCELERATE;
// 按照设定的加速度持续增加速度,实时更新位置和相位信息
run_control_loop([&]() {
vel += lockin_config.accel * current_meas_period;
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period);
if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel))
return false;
return !spin_done(true); //vel_override to go to next phase
});
if (!encoder_.index_found_)
encoder_.set_idx_subscribe(true);
// 使用spin_done(true)检查是否达到转换条件
if (!spin_done()) {
// 恒速阶段(Constant Velocity)保持恒定速度运行
lockin_state_ = LOCKIN_STATE_CONST_VEL;
vel = lockin_config.vel; // reset to actual specified vel to avoid small integration error
run_control_loop([&]() {
distance += vel * current_meas_period;
phase = wrap_pm_pi(phase + vel * current_meas_period); //使用wrap_pm_pi将相位保持在[-π, π]范围内,避免数值溢出问题
if (!motor_.update(lockin_config.current * motor_.config_.torque_constant, phase, vel))
return false;
return !spin_done();
});
}
lockin_state_ = LOCKIN_STATE_INACTIVE;
return check_for_errors();
}
19.run_sensorless_control_loop()
这段代码实现了ODrive在无传感器模式(Sensorless Mode)下的主控制循环,是电机在没有位置传感器(如编码器)时实现闭环控制的核心逻辑。
bool Axis::run_sensorless_control_loop() {
//明确告诉控制器只使用速度估计值
//完全禁用位置相关反馈(无传感器模式下不可靠)
controller_.pos_estimate_linear_src_ = nullptr; // 禁用线性位置反馈
controller_.pos_estimate_circular_src_ = nullptr; // 禁用角度位置反馈
controller_.pos_estimate_valid_src_ = nullptr; // 禁用位置有效性检测
controller_.vel_estimate_src_ = &sensorless_estimator_.vel_estimate_; // 使用无传感器速度估计
controller_.vel_estimate_valid_src_ = &sensorless_estimator_.vel_estimate_valid_;// 速度有效性标志
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint)) //执行控制算法计算目标转矩
return error_ |= ERROR_CONTROLLER_FAILED, false;
//sensorless_estimator_.phase_:转子角度估计 sensorless_estimator_.vel_estimate_:转速估计
if (!motor_.update(torque_setpoint, sensorless_estimator_.phase_, sensorless_estimator_.vel_estimate_)) //实现FOC控制,输出PWM信号
return false; // set_error should update axis.error_
return true;
});
return check_for_errors();
}
20.run_closed_loop_control_loop()
这段代码实现了ODrive的闭环控制主循环,是带编码器反馈的精确位置/速度控制的核心。
bool Axis::run_closed_loop_control_loop() {
//编码器选择与验证,支持多编码器配置(如主/从轴)
if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
// 初始位置安全设置:将初始设定值设为当前位置,避免"跳跃"响应
// 圆形位置模式(如旋转轴)
if (controller_.config_.circular_setpoints) {
if (!controller_.pos_estimate_circular_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
controller_.input_pos_ = *controller_.pos_estimate_circular_src_;
}
}
// 线性位置模式(如直线运动)
else {
if (!controller_.pos_estimate_linear_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
controller_.input_pos_ = *controller_.pos_estimate_linear_src_;
}
}
controller_.input_pos_updated();
// Avoid integrator windup issues
controller_.vel_integrator_torque_ = 0.0f; //积分器复位:防止从上次控制残留积分量,确保启动时控制输出平稳
set_step_dir_active(config_.enable_step_dir); //兼容传统步进电机驱动器接口 启动时使能
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint)) //执行PID控制算法
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs; //(2*M_PI)*机械转速(转/秒)*电机极对数 输出:电角速度(rad/s)
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel)) //实现磁场定向控制(FOC)
return false; // set_error should update axis.error_
return true;
});
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on); //兼容传统步进电机驱动器接口 结束时配置
return check_for_errors();
}
21.run_homing()
这段代码实现了ODrive的自动回零(Homing)功能,是精确定位系统的关键操作。该函数实现了完整的回零流程:
- 第一阶段:向限位开关方向运动直到触发
- 第二阶段:移动到预设的零点位置
- 恢复原始配置并标记回零完成
bool Axis::run_homing() {
// 保存当前控制模式
Controller::ControlMode stored_control_mode = controller_.config_.control_mode;
Controller::InputMode stored_input_mode = controller_.config_.input_mode;
// 检查限位开关使能状态
if (!min_endstop_.config_.enabled) {
return error_ |= ERROR_HOMING_WITHOUT_ENDSTOP, false;
}
// 配置为速度控制模式
controller_.config_.control_mode = Controller::CONTROL_MODE_VELOCITY_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_VEL_RAMP;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = -controller_.config_.homing_speed; // 设置恒定向限位运动
controller_.input_torque_ = 0.0f;
homing_.is_homed = false;
if (!controller_.select_encoder(controller_.config_.load_encoder_axis)) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
// To avoid any transient on startup, we intialize the setpoint to be the current position
// note - input_pos_ is not set here. It is set to 0 earlier in this method and velocity control is used.
if (controller_.config_.circular_setpoints) {
if (!controller_.pos_estimate_circular_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_circular_src_;
}
}
else {
if (!controller_.pos_estimate_linear_src_) {
return error_ |= ERROR_CONTROLLER_FAILED, false;
}
else {
controller_.pos_setpoint_ = *controller_.pos_estimate_linear_src_;
}
}
// Avoid integrator windup issues
controller_.vel_integrator_torque_ = 0.0f;
//寻找限位
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return !min_endstop_.get_state(); // 遇到限位时停止
});
error_ &= ~ERROR_MIN_ENDSTOP_PRESSED; // clear this error since we deliberately drove into the endstop
controller_.pos_setpoint_ = min_endstop_.config_.offset; // 设置目标位置为限位偏移量
controller_.vel_setpoint_ = 0.0f; // Change directions without decelerating
// 重置编码器计数:将软件位置与硬件编码器计数同步,考虑到了编码器每转脉冲数(CPR)
encoder_.set_linear_count((int32_t)(controller_.pos_setpoint_ * encoder_.config_.cpr));
// 切换为位置控制+梯形轨迹
controller_.config_.control_mode = Controller::CONTROL_MODE_POSITION_CONTROL;
controller_.config_.input_mode = Controller::INPUT_MODE_TRAP_TRAJ;
controller_.input_pos_ = 0.0f;
controller_.input_pos_updated();
controller_.input_vel_ = 0.0f;
controller_.input_torque_ = 0.0f;
//精确定位零点
run_control_loop([this](){
// Note that all estimators are updated in the loop prefix in run_control_loop
float torque_setpoint;
if (!controller_.update(&torque_setpoint))
return error_ |= ERROR_CONTROLLER_FAILED, false;
float phase_vel = (2*M_PI) * encoder_.vel_estimate_ * motor_.config_.pole_pairs;
if (!motor_.update(torque_setpoint, encoder_.phase_, phase_vel))
return false; // set_error should update axis.error_
return !controller_.trajectory_done_; // 到达目标位置时停止
});
// 恢复原始控制模式
controller_.config_.control_mode = stored_control_mode;
controller_.config_.input_mode = stored_input_mode;
homing_.is_homed = true; // 标记回零完成
return check_for_errors();
}
22.run_idle_loop()
这段代码实现了ODrive的空闲状态控制循环,是电机轴在不执行具体任务时的基础运行模式。
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_); //安全解除PWM使能
set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on); //步进/方向接口管理
run_control_loop([this]() {
return true;
});
return check_for_errors();
}
23.run_state_machine_loop()
这段代码实现了ODrive电机轴的状态机主循环,是控制系统任务调度的核心。该状态机实现了:
- 多任务链式执行:支持启动序列、全校准序列等复杂流程
- 11种轴状态处理:包括校准、回零、闭环控制等
- 错误恢复机制:自动降级到空闲状态
- 状态过渡管理:确保模式切换的安全性
void Axis::run_state_machine_loop() {
// 预使能电机(安全准备)
motor_.arm();
for (;;) {
// 任务链构建
if (requested_state_ != AXIS_STATE_UNDEFINED) {
size_t pos = 0;
if (requested_state_ == AXIS_STATE_STARTUP_SEQUENCE) {
if (config_.startup_motor_calibration)
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
if (config_.startup_encoder_index_search && encoder_.config_.use_index)
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
if (config_.startup_encoder_offset_calibration)
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
if (config_.startup_homing)
task_chain_[pos++] = AXIS_STATE_HOMING;
if (config_.startup_closed_loop_control)
task_chain_[pos++] = AXIS_STATE_CLOSED_LOOP_CONTROL;
else if (config_.startup_sensorless_control)
task_chain_[pos++] = AXIS_STATE_SENSORLESS_CONTROL;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ == AXIS_STATE_FULL_CALIBRATION_SEQUENCE) {
task_chain_[pos++] = AXIS_STATE_MOTOR_CALIBRATION;
if (encoder_.config_.use_index)
task_chain_[pos++] = AXIS_STATE_ENCODER_INDEX_SEARCH;
task_chain_[pos++] = AXIS_STATE_ENCODER_OFFSET_CALIBRATION;
task_chain_[pos++] = AXIS_STATE_IDLE;
} else if (requested_state_ != AXIS_STATE_UNDEFINED) {
task_chain_[pos++] = requested_state_;
task_chain_[pos++] = AXIS_STATE_IDLE;
}
task_chain_[pos++] = AXIS_STATE_UNDEFINED; // TODO: bounds checking
requested_state_ = AXIS_STATE_UNDEFINED;
// Auto-clear any invalid state error
error_ &= ~ERROR_INVALID_STATE;
}
bool status;
// 状态处理核心
switch (current_state_) {
case AXIS_STATE_MOTOR_CALIBRATION: {
status = motor_.run_calibration();
} break;
case AXIS_STATE_ENCODER_INDEX_SEARCH: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
if (encoder_.config_.idx_search_unidirectional && motor_.config_.direction==0)
goto invalid_state_label;
status = encoder_.run_index_search();
} break;
case AXIS_STATE_ENCODER_DIR_FIND: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = encoder_.run_direction_find();
} break;
case AXIS_STATE_HOMING: {
status = run_homing();
} break;
case AXIS_STATE_ENCODER_OFFSET_CALIBRATION: {
if (!motor_.is_calibrated_)
goto invalid_state_label;
status = encoder_.run_offset_calibration();
} break;
case AXIS_STATE_LOCKIN_SPIN: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(config_.general_lockin);
} break;
case AXIS_STATE_SENSORLESS_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
status = run_lockin_spin(config_.sensorless_ramp); // TODO: restart if desired
if (status) {
// call to controller.reset() that happend when arming means that vel_setpoint
// is zeroed. So we make the setpoint the spinup target for smooth transition.
controller_.vel_setpoint_ = config_.sensorless_ramp.vel / (2.0f * M_PI * motor_.config_.pole_pairs);
status = run_sensorless_control_loop();
}
} break;
case AXIS_STATE_CLOSED_LOOP_CONTROL: {
if (!motor_.is_calibrated_ || motor_.config_.direction==0)
goto invalid_state_label;
if (!encoder_.is_ready_)
goto invalid_state_label;
watchdog_feed();
status = run_closed_loop_control_loop();
} break;
case AXIS_STATE_IDLE: {
run_idle_loop();
status = motor_.arm(); // done with idling - try to arm the motor
} break;
default:
invalid_state_label:
error_ |= ERROR_INVALID_STATE;
status = false; // this will set the state to idle
break;
}
// If the state failed, go to idle, else advance task chain
if (!status) {
std::fill(task_chain_.begin(), task_chain_.end(), AXIS_STATE_UNDEFINED);
current_state_ = AXIS_STATE_IDLE;
} else {
std::rotate(task_chain_.begin(), task_chain_.begin() + 1, task_chain_.end());
task_chain_.back() = AXIS_STATE_UNDEFINED;
}
}
}