Odrive 代码开发随笔（二）

代码版本为0.5.1

这一章主要查看odrive的轴控制程序。

从上一节最后启动的void Axis::start_thread() 来查看。

// @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, stack_size_ / sizeof(StackType_t));
    thread_id_ = osThreadCreate(osThread(thread_def), this);
    thread_id_valid_ = true;
}

从代码看到，这里面创建为了一个运行状态监测任务。接下来查看

static void run_state_machine_loop_wrapper(void* ctx) {
    reinterpret_cast<Axis*>(ctx)->run_state_machine_loop();
    reinterpret_cast<Axis*>(ctx)->thread_id_valid_ = false;
}

这里面主要调用void Axis::run_state_machine_loop()函数。也就是说这个函数才是轴控制主要函数；

1.首先是配置PWM输出

2.检测是否设置了上电运行的状态。

3.根据状态，调用对应函数

4.检测运行结果是否出错

这里用到了task_chain_[]；根据注释应该是待运行的电机状态列队。当前是状态是task_chain_[0]。

删减代码如下：

// Infinite loop that does calibration and enters main control loop as appropriate
void Axis::run_state_machine_loop() {

    // arm!
    motor_.arm();	//配置电机PWM
    for (;;) {
        // Load the task chain if a specific request is pending
        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;
					//........ 根据配置上电校准等
            task_chain_[pos++] = AXIS_STATE_UNDEFINED;  // TODO: bounds checking
            requested_state_ = AXIS_STATE_UNDEFINED;
            // Auto-clear any invalid state error
            error_ &= ~ERROR_INVALID_STATE;
        }
        // Note that current_state is a reference to task_chain_[0]
        // Run the specified state
        // Handlers should exit if requested_state != AXIS_STATE_UNDEFINED
		// 根据不同状态调用不同程序。
        bool status;
        switch (current_state_) {
			// 删除源代码中的一些状态只看闭环控制
            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;
        }
    }
}

全闭环里面用到的是bool Axis::run_closed_loop_control_loop() 这个函数。那么我们接下来查看这个函数。

依次调用了

1.controller_.select_encoder(controller_.config_.load_encoder_axis)。没细看主要功能。

2.设定值初始化为当前位置

3.controller_.input_pos_updated(); 这里面只有一句input_pos_updated_ = true;

4.删除之前的积分值。

5.set_step_dir_active(config_.enable_step_dir);

6.run_control_loop（）注意，run_control_loop中的所有估计器都在循环前缀中更新

新的版本中，至于那个版本开始迁移的没有细看。可能是0.5.1_RC?

bool Axis::run_closed_loop_control_loop() {
    start_closed_loop_control();
    set_step_dir_active(config_.enable_step_dir);

    while ((requested_state_ == AXIS_STATE_UNDEFINED) && motor_.is_armed_) {
        osDelay(1);
    }

    set_step_dir_active(config_.enable_step_dir && config_.step_dir_always_on);
    stop_closed_loop_control();

    return check_for_errors();
}

只是启动和停止闭环控制，真正的控制在void ODrive::control_loop_cb(uint32_t timestamp)；

void ODrive::control_loop_cb(uint32_t timestamp) 最新的代码已经把控制部分移到了这个函数下，这个函数在中断void ControlLoop_IRQHandler(void)中被调用，而这个中断其实是OTG_HS_IRQHandler中断，这个有点奇怪，暂时不管。