MSP430F5529循迹小车 2022电赛 C题
编辑时间2022/8/21
方案
选用材料：主控MSP430F5529、直流减速电机（带光电编码器）、TB6612电机驱动、超声波测距、灰度传感器、无线透传、OLED屏显示。
使用灰度传感器巡线，超声波检测前后车距作为位置环反馈，通过位置环调节前后车距离。
关键词：MSP430 循迹 无线串口透传通信 PID算法 陀螺仪
一、题目
二、控制方案
题目难点在于小车路过“y”路口时的判断，以及小车停车时需保证20cm的间距。我们最初的想法是通过陀螺仪直线矫正通过“y”路口，实际测试发现小车直线稳定性良好，但是陀螺仪数据反馈会有跳变，经过PID直线矫正后可能会导致小车偏离赛道。经过讨论后我们决定采用检测灰度传感器数字量判断小车位置，当小车行驶“y”路口时关闭靠左一路灰度值的传入，通过路口后重新开启，该方案实际测试没有发现较大误差。
题目中均要求一车停止后二车需立即停止并保证20cm间距，所以控制方案尤为重要，我们的想法是二车使用位置环-速度环串级PID控制，这样在一车保证速度的同时，二车会紧跟一车，在一车停止后，二车会立即停止。
问题一分析
问题一较为简单，一车二车设置一样的速度后跑一圈停止即可.
问题二分析
问题二要求二车在E路径上跑两圈后追上 一车，该问中需使用位置环-速度环控制二车，一车速度调到问题二要求的速度即可。
问题三分析
问题三要求小车行驶一圈检测“等停指示”后两辆车需同时停止，与第一问差别不大，唯一区别就是速度，在保证速度的同时又要考虑行车循迹的稳定性才是关键。
问题四分析
问题四需要对小车进行路径规划，一车二车之间需要将自身的编码器里程发送接收，通过里程判断是否需要加减速。
电赛准备的TB6612电机驱动板，画了两板，可直接连接电机。模块自带16-5V降压
链接; TB6612FNG编码电机控制 - 嘉立创EDA开源硬件平台 (oshwhub.com)
三、代码讲解
1、小车PID代码讲解
void PID_Position_Calc( PID *pp, float CurrentPoint, float NextPoint ) //位置环
{
pp->Error = NextPoint - CurrentPoint;
pp->SumError += pp->Error;
pp->DError = pp->Error - pp->LastError;
pp->output = pp->Proportion * pp->Error +
abs_limit(pp->Integral * pp->SumError, pp->Integralmax ) +
pp->Derivative * pp->DError ;
if(pp->output > pp->outputmax ) pp->output = pp->outputmax;
if(pp->output < - pp->outputmax ) pp->output = -pp->outputmax;
// pp->PrevError = pp->LastError;
pp->LastError = pp->Error;
}
void PID_Incremental_Calc( PID *pp, float CurrentPoint, float NextPoint ) //速度环
{
pp->Error = NextPoint - CurrentPoint;
pp->SumError += pp->Error;
pp->DError = pp->Error - pp->LastError;
pp->output += pp->Proportion * ( pp->Error - pp->LastError )+
abs_limit(pp->Integral * pp->Error, pp->Integralmax ) +
pp->Derivative * ( pp->Error + pp->PrevError - 2*pp->LastError);
if(pp->output > pp->outputmax ) pp->output = pp->outputmax;
if(pp->output < - pp->outputmax ) pp->output = -pp->outputmax;
pp->PrevError = pp->LastError;
pp->LastError = pp->Error;
}
void PIDInit(PID *pp, float Kp , float Ki , float Kd , float outputmax, float Integralmax) //PID初始化
{
pp->Integralmax = Integralmax;
pp->outputmax = outputmax;
pp->Proportion = Kp;
pp->Integral = Ki;
pp->Derivative = Kd;
pp->DError = pp->Error = pp->output = pp->LastError = pp->PrevError = 0;
} void navigation_execute(float angle_z,int16_t left_sp,int16_t right_sp)//正反馈 陀螺仪直线矫正
{
// uint8_t angle_dev = 2;
// if(mpu_angle>(angle_z-3))
// moto_set_speed(gpt_cnt.set_speed_a+angle_dev,gpt_cnt.set_speed_b-angle_dev);
// else if(mpu_angle<(angle_z+3))
// moto_set_speed(gpt_cnt.set_speed_a-angle_dev,gpt_cnt.set_speed_b+angle_dev);
// else
// moto_set_speed(gpt_cnt.set_speed_a,gpt_cnt.set_speed_b);
PID_Position_Calc(&ANGLE,Mpu_angle.angle_z+180.0f,angle_z/1.0f);//直线矫正
gpt_cnt.set_speed_a=left_sp - ANGLE.output;
gpt_cnt.set_speed_b=right_sp + ANGLE.output;
}
void car_follow(float now_distance,float next_distance)//小车位置-速度环 next_distance = 20
{
PID_Position_Calc(&distance,now_distance/1.0f,next_distance/1.0f);//位置环计算行驶距离误差
PID_Incremental_Calc(&MOTOR_Spid[0], gpt_cnt.speed_a, distance.output);//速度环输出
PID_Incremental_Calc(&MOTOR_Spid[1], gpt_cnt.speed_b, distance.output);
moto_set_speed(MOTOR_Spid[0].output, MOTOR_Spid[1].output);//速度环输出项发送电机
}
void car_remove(float now_speed[2],float set_speed[2])//小车速度环
{
PID_Incremental_Calc(&MOTOR_Spid[0],now_speed[0], set_speed[0]);//速度环输出
PID_Incremental_Calc(&MOTOR_Spid[1],now_speed[1], set_speed[1]);
moto_set_speed(MOTOR_Spid[0].output, -MOTOR_Spid[1].output);//速度环输出项发送电机
}
二、路径规划
1.编码器获取
#if defined(__TI_COMPILER_VERSION__) || defined(__IAR_SYSTEMS_ICC__)
#pragma vector=PORT2_VECTOR
__interrupt
#elif defined(__GNUC__)
__attribute__((interrupt(PORT2_VECTOR)))
#endif
void Port_2 (void)//编码器采用外部中断获取
{
if(GPIO_getInterruptStatus(GPIO_PORT_P2,GPIO_PIN4) == GPIO_PIN4)
{
if(GPIO_getInputPinValue(GPIO_PORT_P2,GPIO_PIN4) == GPIO_INPUT_PIN_HIGH)//上升沿
{
if(GPIO_getInputPinValue(GPIO_PORT_P2,GPIO_PIN0) == GPIO_INPUT_PIN_LOW)//正转
{
gpt_cnt.count_a --;
}
else//反转
{
gpt_cnt.count_a ++;
}
}
GPIO_clearInterrupt(GPIO_PORT_P2,GPIO_PIN4);
}
}2.小车任务三
void car_control_fort3(void) //任务三
{
run_distance = ((gpt_cnt.count_a+gpt_cnt.count_b)/2.0f)/Encoder_cnt*2.0f*3.14f*3.25f;//行驶距离 = 小车行驶总脉冲 / 1圈的脉冲值 * 2 * pi * 轮子半径
switch(run_dis)
{
case 0:
// navigation_execute(179.0f,set_sp,set_sp);//直线矫正
sensor_checking(30,0,0);//开启循迹
if(run_distance>=stright_dis+15.0f)//通过第一路口
{
gpt_cnt.set_speed_a = gpt_cnt.set_speed_b = 0;
run_dis = 1;
}
break;
case 1:
car_follow(rec_distance-run_distance,200);//位置环控制小车
sensor_checking(set_sp,min_sp,max_sp);//开启循迹
if(run_distance >= cross_third)//判断位置到达第三路口
{
if(sensor_reading[1]&&sensor_reading[2]&&sensor_reading[3])//检测停止标志
{
DL20_AgreementDirectives(0x05,USCI_A1_BASE);//发送停车指令
gpt_cnt.set_speed_a = gpt_cnt.set_speed_b = 0;
if(systick_delay.count_1s != systick_delay.last_count_1s)//1ms
{
if(++delay_tim>=6)//1ms*6
{
DL20_AgreementDirectives(0x06,USCI_A1_BASE);//发送中途启动指令
DL20_AgreementDirectives(0x06,USCI_A1_BASE);//发送中途启动指令
run_dis = 2;
}
systick_delay.last_count_1s = systick_delay.count_1s;
}
}
else
{
delay_tim = 0;
}
}
break;
case 2:
sensor_checking(30,min_sp,max_sp);//开启循迹
if(run_distance >= 580.0f)//一圈总距离584.8cm
{
if((sensor_reading[1]&&sensor_reading[2])||(sensor_reading[2]&&sensor_reading[3]))
{
gpt_cnt.set_speed_a = 0;
gpt_cnt.set_speed_b = 0;
DL20_AgreementDirectives(0x02,USCI_A1_BASE);//发送停车指令
beep_on;
run_flag = 0;
}
}
break;
}
}
四、工程获取
代码及设计报告还在整理中，开学后会公布