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,floatCurrentPoint,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 outputmax )pp->output = -pp->outputmax;//pp->PrevError = pp->LastError;pp->LastError = pp->Error;}void PID_Incremental_Calc( PID *pp,floatCurrentPoint,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 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)))#endifvoid 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;}}

四、工程获取

代码及设计报告还在整理中，开学后会公布