这是在电赛实验室里接触的第一个控制组小车项目，是22年的电赛C题
以下是赛题的任务部分，要求采用TI的MCU,双车跟随，且具有循迹功能，速度在0.3 ~ 1m/s内可调，能在指定路径上完成行驶操作
基于MSP432P401R的小车跟随系统
搭载TI公司MSP432P401R控制内核的小车跟随系统设计精良。系统利用5路数字灰度传感器实现小车循迹，高精度编码器的直流减速电机配合PID算法，实现精准控速。超声波模块HC-SR04用于距离控制，蓝牙模块HC-05实现通信。按下按键后，领头小车通过蓝牙指令驱动跟随小车，二者匀速行驶。灰度传感器识别黑线，超声波模块防碰撞并实时反馈，串级PID调控跟随小车速度，保持精准距离。领头小车检测到停止标志后停止电机并发送指令给跟随小车。在岔道口，灰度传感器进行路径选择，让两车竞速。提速后，发现“等停指示”标识时停下。
理论分析和计算
小车运控循迹算法
PID控制器，即比例-积分-微分控制器，通过比例-积分-微分对系统的综合控制作用，从而达到调节系统的目的。对于该小车，我们需要对PID的参数进行计算，以速度式的PID为例，MSP432最小系统板通过霍尔编码器，利用检测脉冲的方式来计算转速及位置，输出有关旋转轴运动的信息进一步转换为速度、距离、每分钟转速等信息。MCU对数据进行PID算法处理可以调节小车当前的速度。PID是比例、积分、微分的缩写。比例调节是按比例反应系统的偏差,系统一旦出现了偏差,比例调节立即产生调节作用用以减少偏差。比例作用大,可以加快调节,减少误差,但是过大的比例,使系统的稳定性下降,甚至造成系统的不稳定。积分调节是使系统消除稳态误差,提高无差度。因为有误差,积分调节就进行,直至无差,积分调节停止,积分调节输出一常值。积分作用的强弱取决与积分时间常数Ti,Ti越小,积分作用就越强。反之Ti大则积分作用弱,加入积分调节可使系统稳定性下降,动态响应变慢。积分作用常与另两种调节规律结合,组成PI调节器或PID调节器。几者结合从而保持小车运行的稳定。
小车通信模式
采用两个HC-05蓝牙模块，一个主机，一个从机，用单片机的串口UART和蓝牙通信
蓝牙模块的RXD接单片机的TXD，TXD接RXD,VCC接5V，GND接GND
小车距离控制分析
采用HC-SR04超声测距模块，超声波测距模块采用IO口TRIG触发测距，模块自动发送方波，自动检测信号返回。有信号返回，输出高电平，开启定时器计数
程序设计
模块化代码
#include <stdint.h>
#include <ti/devices/msp432p4xx/driverlib/driverlib.h>
#include <stdlib.h>
#include <stdbool.h>
#include <stdio.h>
#include <math.h>
#include "inv_mpu_dmp_motion_driver.h"
#include "inv_mpu.h"
#include "dmpmap.h"
#include "dmpKey.h"
#include "led.h"
#include "delay.h"
#include "sysinit.h"
#include "key.h"
#include "buzzer.h"
#include "usart.h"
#include "hongwai.h"
#include "blooth_esp.h"
#include "timA.h"
#include "timA0.h"
#include "CCD.h"
#include "interrupt.h"
#include "adc.h"
#include "timer.h"
#include "oled.h"
#include "mpu6050.h"
#include "IOI2C.h"
#include "DataScope_DP.h"
#include "show.h"
#include "HCSR04.h"小车实物
核心代码
void T32_INT1_IRQHandler(void)
{
MAP_Timer32_clearInterruptFlag(TIMER32_0_BASE); // 清楚中断标志
//等停延时
if(delay_Flag==1)
{
Set_Pwm(0,0);
Buzzer_on();
delay_ms(5000);
delay_Flag=0;
}
else
{
if(Flag_start==1)
{
//不停车
if(Flag_Stop==0)
{
//红外循线
hw_test();
Encoder_Update();
//十次累计精确结果
Enl += Encoder_Left; // 为精确，求10次平均
Enr += Encoder_Right; // 为精确，求10次平均
Encount++; // 计数变量
if(Encount == 10)
{
Encoder_Left = Enl; // 放大十倍后结果相加
Encoder_Right = Enr; // 放大十倍后结果相加
Enall=(Enl+Enr),Enl=0,Enr=0,Encount=0; // 复位，左右轮编码器相加
}
else
{
Encoder_Left=10 * Encoder_Left; // 放大十倍232
Encoder_Right=10 * Encoder_Right; // 放大十倍
}
Get_Velocity_Form_Encoder(Encoder_Left,Encoder_Right);
Velocity_Pwm=Velocity(Encoder_Left,Encoder_Right);
if(mission==1|mission==2)
{
//防止第一次误转,加速冲过去，走外圈
if(Flag_cross == 1)
{
Motor_A=Velocity_Pwm+50+1300; //电机赋PWM，50手动校准误差，转向PWM波
Motor_B=Velocity_Pwm-50+1300; //电机赋PWM，50手动校准误差，转向PWM波
Xianfu_Pwm(4500); //PWM限幅
Set_Pwm(Motor_A,Motor_B); //直接赋值
delay_ms(180);
Flag_cross=0; //清零
}
//正常走
else
{
Motor_A=Velocity_Pwm+Turn_pwm+50; //电机赋PWM，50手动校准误差，转向PWM波
Motor_B=Velocity_Pwm-Turn_pwm-50; //电机赋PWM，50手动校准误差，转向PWM波
Xianfu_Pwm(4500); //PWM限幅
}
}
else if(mission==4)
{
Motor_A=Velocity_Pwm+Turn_pwm+50; //电机赋PWM，50手动校准误差，转向PWM波
Motor_B=Velocity_Pwm-Turn_pwm-50; //电机赋PWM，50手动校准误差，转向PWM波
Xianfu_Pwm(4500); //PWM限幅
}
//任务三走圈循迹
else if(mission==3)
{
//第一次，第三次,加速冲过去
if(Flag_cross == 1)
{
Motor_A=Velocity_Pwm+50+1000; //电机赋PWM，50手动校准误差，转向PWM波,加速冲过去
Motor_B=Velocity_Pwm-50+1200; //电机赋PWM，50手动校准误差，转向PWM波
Xianfu_Pwm(4500); //PWM限幅
Set_Pwm(Motor_A,Motor_B); //直接赋值
delay_ms(180);
Flag_cross=0; //清零
}
//第二次识别到岔道口，走内圈
else if(Flag_cross == 2)
{
Motor_A=Velocity_Pwm+2100; //电机赋PWM，50手动校准误差，转向PWM波
Motor_B=0; //电机关断
Xianfu_Pwm(4500); //PWM限幅
Set_Pwm(Motor_A,Motor_B); //直接赋值
delay_ms(250);
Flag_cross=0; //清零
}
//正常走
else
{
Motor_A=Velocity_Pwm+Turn_pwm+50; //电机赋PWM，50手动校准误差，转向PWM波
Motor_B=Velocity_Pwm-Turn_pwm-50; //电机赋PWM，50手动校准误差，转向PWM波
Xianfu_Pwm(4500); //PWM限幅
}
}
Set_Pwm(Motor_A,Motor_B);
}
//停车
else
{
Set_Pwm(0,0);
Buzzer_on();
}
}
}
}
//串口发送电机速度函数
void USART_Velocity(void)
{
//以下代码用于调试PID
printf_uart0("Velocity_L: %d ",Encoder_Left); //当前编码器位置打印到串口
printf_uart0("Moto_L: %d ",Motor_A); //当前的PID控制器输出打印到串口
printf_uart0("Target_L: %d\r\n",Target_A); //当前的位置目标值打印到串口
printf_uart0("Velocity_R: %d ",Encoder_Right); //当前编码器位置打印到串口
printf_uart0("Moto_R: %d ",Motor_B); //当前的PID控制器输出打印到串口
printf_uart0("Target_R: %d\r\n",Target_B); //当前的位置目标值打印到串口
}
//OLED显示电机位置，并发送到上位机
void OLED_Position(void)
{
OLED_ShowNum(20,6,(int)(Position_Right/1000),1,8);
OLED_ShowNum(28,6,(int)Position_Right/100%10,1,8);
OLED_ShowNum(36,6,(int)Position_Right/10%100,1,8);
OLED_ShowNum(44,6,(int)Position_Right%1000,1,8);
OLED_ShowNum(20,8,(int)(Position_Left/1000),1,8);
OLED_ShowNum(28,8,(int)Position_Left/100%10,1,8);
OLED_ShowNum(36,8,(int)Position_Left/10%100,1,8);
OLED_ShowNum(44,8,(int)Position_Left%1000,1,8);
DataScope();
delay_ms(50);
}
//电机位置控制
void Position_Control(void)
{
//位置PID
Position_Right+=Encoder_Right;
Position_Left+=Encoder_Left;
Motor_A=Position_PID_L(Position_Left,Target_Position_L); //位置PID控制器
Motor_B=Position_PID_R(Position_Right,Target_Position_R); //位置PID控制器
Xianfu_Pwm(3300); //PWM限幅
Set_Pwm(Motor_A,Motor_B);
}
/**************************************************************************
函数功能：增量PI控制器
入口参数：编码器测量值，目标速度
返回 值：电机PWM
根据增量式离散PID公式
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此类推
pwm代表增量输出
在我们的速度控制闭环系统里面，只使用PI控制
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)
**************************************************************************/
int Incremental_PI_A (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
Pwm+=Velocity_KD*(Bias-Last_bias)+Velocity_KP*Bias; //增量式PI控制器
if(Pwm>4800)
Pwm=4800;
if(Pwm<-4800)
Pwm=-4800;
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
int Incremental_PI_B (int Encoder,int Target)
{
static int Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
Pwm+=Velocity_KD*(Bias-Last_bias)+Velocity_KP*Bias; //增量式PI控制器
if(Pwm>4800)
Pwm=4800;
if(Pwm<-4800)
Pwm=-4800;
Last_bias=Bias; //保存上一次偏差
return Pwm; //增量输出
}
/**************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回 值：电机PWM
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
**************************************************************************/
int Distance_PID (int position,int target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias = target-position; //计算偏差
Integral_bias += Bias; //求出偏差的积分
//最大积分偏差3000
if(Integral_bias > 20)
Integral_bias = 20;
if(Integral_bias < -20)
Integral_bias = -20;
Pwm = Distance_KP * Bias + Distance_KI * Integral_bias + Distance_KD * (Bias-Last_Bias);//位置式PID控制器
Last_Bias = Bias; //保存上一次偏差
return Pwm; //增量输出
}
/**************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回 值：电机PWM
根据位置式离散PID公式
pwm=Kp*e(k)+Ki*∑e(k)+Kd[e（k）-e(k-1)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差
∑e(k)代表e(k)以及之前的偏差的累积和;其中k为1,2,,k;
pwm代表输出
**************************************************************************/
int Position_PID_L (int position,int target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias = target-position; //计算偏差
Integral_bias += Bias; //求出偏差的积分
//最大积分偏差3000
if(Integral_bias > 2000)
Integral_bias = 2000;
if(Integral_bias < -2000)
Integral_bias = -2000;
Pwm = Position_KP * Bias + Position_KI * Integral_bias + Position_KD * (Bias-Last_Bias);//位置式PID控制器
Last_Bias = Bias; //保存上一次偏差
return Pwm; //增量输出
}
int Position_PID_R (int position,int target)
{
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias = target-position; //计算偏差
Integral_bias += Bias; //求出偏差的积分
//最大积分偏差3000
if(Integral_bias > 2000)
Integral_bias = 2000;
if(Integral_bias < -2000)
Integral_bias = -2000;
Pwm = Position_KP * Bias + Position_KI * Integral_bias + Position_KD * (Bias-Last_Bias);//位置式PID控制器
Last_Bias = Bias; //保存上一次偏差
return Pwm; //增量输出
}
//循迹程序
void DataProcessing(uint8_t *data_buf)
{
if(data_buf[4]==0x01)
{
//中间检测 直行
Turn_pwm = 0;
}
if(data_buf[3]==0x01)
{
//左2检测到 小幅度向左调整
if(Turn_pwm > 500)
{
Turn_pwm = 500;
}
else
{
Turn_pwm += 60;
}
}
if(data_buf[2]==0x01)
{
//左1检测到 大幅度向左调整
if(Turn_pwm > 400)
{
Turn_pwm = 400;
}
else
{
Turn_pwm += 80;
}
}
if(data_buf[5]==0x01)
{
//右2检测到 大幅度向右调
if(Turn_pwm < -500)
{
Turn_pwm = -500;
}
else
{
Turn_pwm -= 60;
}
}
if(data_buf[6]==0x01)
{
//右1检测到 小幅度向右调整
if(Turn_pwm < -400)
{
Turn_pwm = -400;
}
else
{
Turn_pwm -= 80;
}
}
}
//距离程序
void fordistance(void)
{
if(mission==2)
{
if(distance_hcsr04<220&distance_hcsr04>180)//18-22
{
}
else if(distance_hcsr04<180&distance_hcsr04>220)//14-18
{
if(Velocity_Car<18)
Velocity_Car=18;
else
Velocity_Car--;
}
else if(distance_hcsr04<260&distance_hcsr04>220)//22-26
{
if(Velocity_Car>35)
Velocity_Car=35;
else
Velocity_Car++;
}
else if(distance_hcsr04<140)//14
{
if(Velocity_Car<5)
Velocity_Car=5;
else
Velocity_Car=Velocity_Car-10;
}
}
else if(mission==1)
{
//1
if(distance_hcsr04<220&distance_hcsr04>180)//18-22
{
}
else if(distance_hcsr04<180&distance_hcsr04>220)//14-18
{
if(Velocity_Car<18)
Velocity_Car=18;
else
Velocity_Car--;
}
else if(distance_hcsr04<260&distance_hcsr04>220)//22-26
{
if(Velocity_Car>23)
Velocity_Car=23;
else
Velocity_Car++;
}
else if(distance_hcsr04<140)//14
{
if(Velocity_Car<15)
Velocity_Car=15;
else
Velocity_Car=Velocity_Car-10;
}
}
else if(mission==3)
{
//1
if(distance_hcsr04<220&distance_hcsr04>180)//18-22
{
}
else if(distance_hcsr04<180&distance_hcsr04>220)//14-18
{
if(Velocity_Car<18)
Velocity_Car=18;
else
Velocity_Car--;
}
else if(distance_hcsr04<260&distance_hcsr04>220)//22-26
{
if(Velocity_Car>23)
Velocity_Car=23;
else
Velocity_Car++;
}
else if(distance_hcsr04<140)//14
{
if(Velocity_Car<15)
Velocity_Car=15;
else
Velocity_Car=Velocity_Car-10;
}
}
else if( mission==4)
{
if(distance_hcsr04<220&distance_hcsr04>180)//18-22
{
}
else if(distance_hcsr04<180&distance_hcsr04>220)//14-18
{
if(Velocity_Car<18)
Velocity_Car=18;
else
Velocity_Car--;
}
else if(distance_hcsr04<260&distance_hcsr04>220)//22-26
{
if(Velocity_Car>35)
Velocity_Car=35;
else
Velocity_Car++;
}
else if(distance_hcsr04<140)//14
{
if(Velocity_Car<5)
Velocity_Car=5;
else
Velocity_Car=Velocity_Car-10;
}
}
}
/*************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回 值：电机PWM增量
**************************************************************************/
float Position_PID_1(float Position,float Target)
{ //增量输出
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Target-Position; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP_DuoJi*Bias/100+Position_KI_DuoJi*Integral_bias/100+Position_KD_DuoJi*(Bias-Last_Bias)/100; //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm;
}
/*************************************************************************
函数功能：位置式PID控制器
入口参数：编码器测量位置信息，目标位置
返回 值：电机PWM增量
**************************************************************************/
float Position_PID_2(float Position,float Target)
{ //增量输出
static float Bias,Pwm,Integral_bias,Last_Bias;
Bias=Target-Position; //计算偏差
Integral_bias+=Bias; //求出偏差的积分
Pwm=Position_KP_DuoJi*Bias/100+Position_KI_DuoJi*Integral_bias/100+Position_KD_DuoJi*(Bias-Last_Bias)/100; //位置式PID控制器
Last_Bias=Bias; //保存上一次偏差
return Pwm;
}
