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;
    }
}

四、工程获取

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