简述 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);
}
这样就实现了简单的电机速度闭环控制。当然这只算是比较基础的闭环，若要实现复杂功能，还需进行大量优化。
代码
如下为本文闭环全部代码。
motor.c
/*******************************************************************************************
* 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);
}
motor.h
#ifndef _MOTOR_H
#define _MOTOR_H
extern struct MOTOR_PID motor_pid; // PID 结构体
extern struct ENCODER encoder; // 编码器结构体
extern struct MOTOR motor; // 电机结构体
void MotorInit(void); // 电机初始化
void MotorControl(void); // 电机控制
void EncoderInit(void); // 编码器初始化
float EncoderRead(void); // 编码器取脉冲值
void GetMotorSpeed(void); // 获取电机速度
float GetMotorDistance(void); // 获取电机距离
#endif
math.h
// PID 变量
struct MOTOR_PID {
float kp;
float ki;
float kd;
float err; // 偏差角度
float lastErr; // 上一个偏差
float beforeErr; // 上上个偏差
float addErr; // 误差累加
float finalVal; // 最终得出的值
};
// 电机
struct MOTOR {
float expectSpeed; // 电机期望速度
uint16 expectDutyRatio; // 电机期望占空比
float distance; // 电机运行距离
};
// 编码器
struct ENCODER {
float value; // 编码器的值
float finalValue; // 编码器最终值
float motorRounds; // 电机 5ms 转的圈数
float motorDistance; // 电机运行距离
float motorSpeed; // 电机速度
};
