简述 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);
}