Odrive0.5.1-FOC电机控制 axis.cpp代码实现（四）

于 2025-04-11 14:39:11 发布
阅读量342 收藏 10
点赞数 5
分类专栏： FOC电机驱动-Odrive核心代码讲解 文章标签： arm开发 嵌入式硬件 单片机 学习 笔记
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/MOS_JBET/article/details/147143429
版权

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是电机启动时的关键过程，它通过三个阶段让电机从静止状态平滑过渡到目标速度：
  1. 电流斜坡上升阶段（Ramp）
  2. 加速阶段（Accelerate）
  3. 恒速阶段（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）功能，是精确定位系统的关键操作。该函数实现了完整的回零流程：
  1. 第一阶段：向限位开关方向运动直到触发
  2. 第二阶段：移动到预设的零点位置
  3. 恢复原始配置并标记回零完成
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电机轴的状态机主循环，是控制系统任务调度的核心。该状态机实现了：
  1. 多任务链式执行：支持启动序列、全校准序列等复杂流程
  2. 11种轴状态处理：包括校准、回零、闭环控制等
  3. 错误恢复机制：自动降级到空闲状态
  4. 状态过渡管理：确保模式切换的安全性
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;
        }
    }
}
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值