Odrive0.5.1-FOC电机控制 axis.cpp代码实现(三)

6.Axis::start_thread()

start_thread():在 FreeRTOS 中新建一个线程,运行 run_state_machine_loop()(状态机控制主循环)。

void Axis::start_thread() {
    osThreadDef(thread_def, run_state_machine_loop_wrapper, hw_config_.thread_priority, 0, stack_size_ / sizeof(StackType_t));
    thread_id_ = osThreadCreate(osThread(thread_def), this);
    thread_id_valid_ = true;
}

7.signal_current_meas()

signal_current_meas():在中断中发出信号,告诉主控制线程电流测量完成。

void Axis::signal_current_meas() {
    if (thread_id_valid_)
        osSignalSet(thread_id_, M_SIGNAL_PH_CURRENT_MEAS);
}

8.wait_for_current_meas()

wait_for_current_meas():主线程阻塞等待电流测量信号,用于实现同步控制。

bool Axis::wait_for_current_meas() {
    return osSignalWait(M_SIGNAL_PH_CURRENT_MEAS, PH_CURRENT_MEAS_TIMEOUT).status == osEventSignal;
}

9.step_cb()

每当接收到一个脉冲(step)信号时,执行该回调

void Axis::step_cb() {
    const bool dir_pin = dir_port_->IDR & dir_pin_; //读取方向引脚 dir_pin 的状态。
    const int32_t dir = (-1 + 2 * dir_pin) * step_dir_active_; //根据方向计算正负(正为 +1,负为 -1)。
    controller_.input_pos_ += dir * config_.turns_per_step; //更新 input_pos_(控制目标位置)。
    controller_.input_pos_updated(); //通知控制器位置更新。
};

10.Axis::load_default_step_dir_pin_config

封装默认配置设置,用于初始化或从某些模板中加载配置项。

void Axis::load_default_step_dir_pin_config(
        const AxisHardwareConfig_t& hw_config, Config_t* config) {
    config->step_gpio_pin = hw_config.step_gpio_pin;
    config->dir_gpio_pin = hw_config.dir_gpio_pin;
}

11.Axis::load_default_can_id(const int& id, Config_t& config)

封装默认配置设置,用于初始化或从某些模板中加载配置项。

void Axis::load_default_can_id(const int& id, Config_t& config){
    config.can_node_id = id;
}

12.decode_step_dir_pins()

从配置中的 GPIO 引脚号获取实际的 GPIO_TypeDef* 和 pin mask,便于后续快速访问 GPIO 的电平状态或设置输出。

void Axis::decode_step_dir_pins() {
    step_port_ = get_gpio_port_by_pin(config_.step_gpio_pin);
    step_pin_ = get_gpio_pin_by_pin(config_.step_gpio_pin);
    dir_port_ = get_gpio_port_by_pin(config_.dir_gpio_pin);
    dir_pin_ = get_gpio_pin_by_pin(config_.dir_gpio_pin);
}
constexpr GPIO_TypeDef* get_gpio_port_by_pin(uint16_t GPIO_pin){
  switch(GPIO_pin){
    case 1: return GPIO_1_GPIO_Port; break;
    case 2: return GPIO_2_GPIO_Port; break;
    case 3: return GPIO_3_GPIO_Port; break;
    case 4: return GPIO_4_GPIO_Port; break;
#ifdef GPIO_5_GPIO_Port
    case 5: return GPIO_5_GPIO_Port; break;
#endif
#ifdef GPIO_6_GPIO_Port
    case 6: return GPIO_6_GPIO_Port; break;
#endif
#ifdef GPIO_7_GPIO_Port
    case 7: return GPIO_7_GPIO_Port; break;
#endif
#ifdef GPIO_8_GPIO_Port
    case 8: return GPIO_8_GPIO_Port; break;
#endif
    default: return GPIO_1_GPIO_Port;
  }
}

constexpr uint16_t get_gpio_pin_by_pin(uint16_t GPIO_pin){
  switch(GPIO_pin){
    case 1: return GPIO_1_Pin; break;
    case 2: return GPIO_2_Pin; break;
    case 3: return GPIO_3_Pin; break;
    case 4: return GPIO_4_Pin; break;
#ifdef GPIO_5_Pin
    case 5: return GPIO_5_Pin; break;
#endif
#ifdef GPIO_6_Pin
    case 6: return GPIO_6_Pin; break;
#endif
#ifdef GPIO_7_Pin
    case 7: return GPIO_7_Pin; break;
#endif
#ifdef GPIO_8_Pin
    case 8: return GPIO_8_Pin; break;
#endif
    default: return GPIO_1_Pin;
  }
}

13.set_step_dir_active(bool active)

这个函数用于启用或关闭对步进电机 Step/Dir 控制接口的响应。

void Axis::set_step_dir_active(bool active) {
    if (active) {
        // Set up the direction GPIO as input
        GPIO_InitTypeDef GPIO_InitStruct;
        GPIO_InitStruct.Pin = dir_pin_;
        GPIO_InitStruct.Mode = GPIO_MODE_INPUT;
        GPIO_InitStruct.Pull = GPIO_NOPULL;
        HAL_GPIO_Init(dir_port_, &GPIO_InitStruct);

        // Subscribe to rising edges of the step GPIO
        GPIO_subscribe(step_port_, step_pin_, GPIO_PULLDOWN, step_cb_wrapper, this);

        step_dir_active_ = true;
    } else {
        step_dir_active_ = false;

        // Unsubscribe from step GPIO
        GPIO_unsubscribe(step_port_, step_pin_);
    }
}

14.do_checks()

这段代码 Axis::do_checks() 是 ODrive 项目中用来进行轴状态自检的函数,确保系统各部分处于正常工作状态,否则就设置相应的 error_ 错误位。

bool Axis::do_checks() {
    if (!brake_resistor_armed) //检查刹车电阻(制动电阻)是否启用。
        error_ |= ERROR_BRAKE_RESISTOR_DISARMED;
    //检查当前轴状态。如果不是 IDLE 状态,但电机已经被“解除武装”(disarmed),这说明某些错误发生导致提前中止了控制流程,故设置错误位。
    if ((current_state_ != AXIS_STATE_IDLE) && (motor_.armed_state_ == Motor::ARMED_STATE_DISARMED))
        error_ |= ERROR_MOTOR_DISARMED;
    if (!(vbus_voltage >= odrv.config_.dc_bus_undervoltage_trip_level)) //检查母线电压是否过低
        error_ |= ERROR_DC_BUS_UNDER_VOLTAGE;
    if (!(vbus_voltage <= odrv.config_.dc_bus_overvoltage_trip_level)) //检查母线电压是否过高。
        error_ |= ERROR_DC_BUS_OVER_VOLTAGE;

    // 温度保护检查。遍历所有热敏电阻对象,检查是否有过热状态。
    for (ThermistorCurrentLimiter* thermistor : thermistors_) {
        thermistor->do_checks();
    }
    motor_.do_checks(); //电机模块的内部自检(比如过流、未初始化等),也会上传错误。
    // encoder_.do_checks();
    // sensorless_estimator_.do_checks();
    // controller_.do_checks();

    //限位开关检查:如果当前不是归零状态 AXIS_STATE_HOMING,却检测到限位开关被按下,说明可能撞机,立即设置错误停止电机。
    if (min_endstop_.config_.enabled && min_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
        error_ |= ERROR_MIN_ENDSTOP_PRESSED;
    } else if (max_endstop_.config_.enabled && max_endstop_.get_state() && !(current_state_ == AXIS_STATE_HOMING)) {
        error_ |= ERROR_MAX_ENDSTOP_PRESSED;
    }

    return check_for_errors();
}

15.do_updates()

bool Axis::do_updates() {
    for (ThermistorCurrentLimiter* thermistor : thermistors_) {
        thermistor->update(); //更新热敏电阻状态(主要检测温度过高)
    }
    encoder_.update(); //读取编码器位置
    sensorless_estimator_.update(); //无编码器估算器,计算速度或位置
    min_endstop_.update(); //检查限位开关状态
    max_endstop_.update();
    bool ret = check_for_errors(); //判断是否有任何 error_ 被设置
    odCAN->send_heartbeat(this); //向主控/上位机报告当前状态
    return ret;
}
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值