簡述 K60 直流電機閉環控制

僅用於備份本人所寫筆記，如有錯誤或不完善之處還請包含。轉載請注明出處！

程序使用逐飛科技 K60 庫 V2.0.3

電機速度閉環控制，簡單來說就是將電機的輸出量（速度）通過編碼器反饋到控制端（單片機），然后對該輸出量和設定的輸入量進行比對，接着經過 PID 運算，將運算結果輸入給電機的過程。

電機速度閉環一個顯而易見的好處就是，可以讓小車在不同的路段動態調整小車的速度，如果參數調整的好，可以讓小車極大的提升對賽道的適應性能，減少小車跑完一圈所需的時間。

閉環控制思路比較簡單，首先需要獲取編碼器的值，每 4ms 取一次值（可以設定其他時間），然后將取到的值進行相應的計算，得出電機當前速度。

代碼示例：

const float unit_pulse = 1783.0;  // 小車輪胎旋轉一周（360°）的脈沖
const float motor_radius = 0.063;  // 電機輪胎半徑（米）

// 編碼器初始化
void EncoderInit(void) {
    ftm_quad_init(ftm2);  // 初始化 ftm2 為正交解碼

    port_init_NoAlt(B18, PULLUP);  // 上拉
    port_init_NoAlt(B19, PULLUP);  // 上拉
}

// 讀取編碼器值
float EncoderRead(void) {
    float data = 0;

    data = ftm_quad_get(ftm2);  // 獲取編碼器的脈沖值
    ftm_quad_clean(ftm2);       // 清除正交解碼的脈沖值

    return data;
}

// 計算電機當前速度
// 輸入值為運行該程序的間隔時間
float GetMotorSpeed(uint16 run_time_ms) {
    float motor_speed = 0;

    encoder.finalValue +=
        0.25f * (encoder.value - encoder.finalValue);  // 一階低通濾波
    encoder.motorRounds =
        (encoder.finalValue * run_time_ms) / unit_pulse;  // 電機每 1s 轉的圈數
    encoder.motorDistance =
        encoder.motorRounds * (2 * PI * motor_radius);  // 電機運行長度 m
    motor_speed = encoder.motorDistance / 1;            // 電機速度 m/s

    return motor_speed;
}

計算出速度后，把該變量賦值給 PID 計算，算出需要輸出給電機的占空比。PID 計算公式可以根據實際情況稍作修改優化。

代碼示例：

// 電機 pid 程序
void MotorPid(void) {
    static float err, last_err, expect_pwm;

    // 請根據實際情況修改 KP、KI 參數大小
    motor_pid.kp = 0.1;
    motor_pid.ki = 0.2;

    err = motor.expectSpeed - encoder.motorSpeed;
    expect_pwm += motor_pid.kp * (err - last_err) + motor_pid.ki * err;
    last_err = err;

    // 電機占空比限幅
    // 請根據實際情況修改最大和最小 PWM 參數值
    if (expect_pwm > 260) expect_pwm = 260;
    if (expect_pwm <= 0) expect_pwm = 0;

    // 將值轉換為正整數類型
    motor.expectDutyRatio = (uint16)(expect_pwm);
}

PID 返回期望占空比賦值給電機控制函數：

// 電機控制程序
void MotorControl(void) {
    MotorPid();
    MotorPWM(0, motor.expectDutyRatio);
}

這樣就實現了簡單的電機速度閉環控制。當然這只算是比較基礎的閉環，若要實現復雜功能，還需進行大量優化。

如下為本文閉環全部代碼：

/*******************************************************************************************
 * COPYRIGHT NOTICE
 * Copyright (c) 2019, ZhangXiao
 * All rights reserved.
 *
 * 本文件僅供參考交流，未經允許不得用於商業用途！
 *
 * File           : motor.c
 * Author         : ZhangXiao
 * Blog           : www.cnblogs.com/zhangxiaochn
 * Version        : v1.0.1
 * Date           : 2019-08-06
 * Software       : IAR 7.70.1
 * Description    : 電機相關程序以及 PID、編碼器程序等
 * note           : None
********************************************************************************************/

#include "motor.h"
#include "headfile.h"

const float unit_pulse = 1783.0;  // 小車輪胎旋轉一周（360°）的脈沖
const float motor_radius = 0.063;  // 電機輪胎半徑（米）

// 電機 pid 程序結構體參數
struct MOTOR_PID motor_pid = {0.1, 0.2, 0, 0, 0, 0, 0, 0};

// 編碼器結構體
struct ENCODER encoder = {0, 0, 0, 0, 0};

// 電機結構體
struct MOTOR motor = {0, 0, 0};

// 電機初始化
// FTM1_CH0_PIN 端口為 A12
// FTM1_CH1_PIN 端口為 A13
// 電機初始化頻率默認為 10k，請根據實際情況更改頻率大小
void MotorInit(void) {
    ftm_pwm_init(ftm1, ftm_ch0, 10 * 1000, 0);
    ftm_pwm_init(ftm1, ftm_ch1, 10 * 1000, 0);
}

// 電機 PWM 控制
void MotorPWM(uint16 pwm_1, uint16 pwm_2) {
    ftm_pwm_duty(ftm1, ftm_ch0, pwm_1);  // A12 端口
    ftm_pwm_duty(ftm1, ftm_ch1, pwm_2);  // A13 端口
}

// 編碼器初始化
void EncoderInit(void) {
    ftm_quad_init(ftm2);  // 初始化 ftm2 為正交解碼

    port_init_NoAlt(B18, PULLUP);  // 上拉
    port_init_NoAlt(B19, PULLUP);  // 上拉
}

// 讀取編碼器值
float EncoderRead(void) {
    float data = 0;

    data = ftm_quad_get(ftm2);  // 獲取編碼器的脈沖值
    ftm_quad_clean(ftm2);       // 清除正交解碼的脈沖值

    return data;
}

// 計算電機當前速度
// 輸入值為運行該程序的間隔時間
float GetMotorSpeed(uint16 run_time_ms) {
    float motor_speed = 0;

    encoder.finalValue +=
        0.25f * (encoder.value - encoder.finalValue);  // 一階低通濾波
    encoder.motorRounds =
        (encoder.finalValue * run_time_ms) / unit_pulse;  // 電機每 1s 轉的圈數
    encoder.motorDistance =
        encoder.motorRounds * (2 * PI * motor_radius);  // 電機運行長度 m
    motor_speed = encoder.motorDistance / 1;            // 電機速度 m/s

    return motor_speed;
}

// 計算電機運行距離
// 輸入值為運行該程序的間隔時間
float GetMotorDistance(uint16 run_time_ms) {
    return encoder.motorDistance / run_time_ms;
}

// 編碼器相關參數計算
// 輸入值為運行該程序的間隔時間
// 程序建議每 4ms 運行一次，即 Encoder_Calculate(4);
void EncoderCalculate(uint16 run_time_ms) {
    uint16 run_time = 1000 / run_time_ms;  // 每秒鍾運行此程序次數

    encoder.value = EncoderRead();                 // 讀取編碼器的值
    encoder.motorSpeed = GetMotorSpeed(run_time);  // 計算電機當前速度
    motor.distance += GetMotorDistance(run_time);  // 獲取電機距離
}

// 電機 pid 程序
void MotorPid(void) {
    static float err, last_err, expect_pwm;

    // 請根據實際情況修改 KP、KI 參數大小
    motor_pid.kp = 0.1;
    motor_pid.ki = 0.2;

    err = motor.expectSpeed - encoder.motorSpeed;
    expect_pwm += motor_pid.kp * (err - last_err) + motor_pid.ki * err;
    last_err = err;

    // 電機占空比限幅
    // 請根據實際情況修改最大和最小 PWM 參數值
    if (expect_pwm > 260) expect_pwm = 260;
    if (expect_pwm <= 0) expect_pwm = 0;

    // 將值轉換為正整數類型
    motor.expectDutyRatio = (uint16)(expect_pwm);
}

// 電機控制程序
void MotorControl(void) {
    MotorPid();
    MotorPWM(0, motor.expectDutyRatio);
}