2024年电赛H题解析

fyphauhs

已于 2024-11-05 21:32:30 修改

阅读量4k

点赞数 58

文章标签：单片机 stm32 嵌入式硬件

于 2024-11-05 21:28:02 首次发布

本文链接：https://blog.csdn.net/fyphauhs/article/details/143527757

版权

一、硬件选择

这里不是打广告

(一)电机驱动

TB6612双路驱动模块带稳压版接口

(二)红外循迹

幻尔机器人4路循迹传感器（GPIO通信）

(三)小车车模以及电机

R1系列三轮小车+带铰链高度角度可调相机支架+mg310电机两个

(四)陶晶驰串口屏4.3英寸

(五)角度传感器

MPU6050模块串口6轴加速度计电子陀螺仪JY61

(六)MCU

嘉立创MSPM0G3507(咸鱼买二手)

二、硬件链接

(一)电机驱动

1.电机驱动

MCU的PA14连电机驱动的AIN1,PA13连电机驱动的AIN2，PA15连电机驱动的BIN1，PA16连电机驱动的BIN2。TB6612上的STAY直接置3.3v高电平。

MCU的PA21是左轮电机的pwm引脚连到TB6612的PWMA,PA22是右轮电机的pwm引脚连到TB6612的PWMB。

电机A是小车左边的电机，电机B是小车右边的电机

2.霍尔编码器

MCU的PB18连电机驱动的E1A,MCU的PB19连电机驱动的E1B。

3.红外循迹

红外循迹模块左2(最左边的红外管)连MCU的PA2,左1(中间靠左)连PA7,右1(中间靠右)连PA8,右2(最右边的红外管)连PA9。

4.蜂鸣器

蜂鸣器的IO口连接MCU的PB7。

5.串口

MCU的PA1连接到MPU6050的TX,PA0连接到MPU6050的RX(串口0，波特率：115200bps,注意：此串口协议为：RS485,不是常规的RS232)

MCU的PA18连接到串口屏的TX,PA17连接到串口屏的RX(串口1,波特率：115200bps)

6.其他

除此之外，还要把MCU的主频提高到80MHz,定时器每隔50ms中断，外部中断

三、软件代码编写思路

题目切换思路

如果接受的串口1的数据为 '1' ,则进入题目1程序
如果接受的串口1的数据为 '0' ,则继续发送有关题目1程序的小车参数，但让小车停止
如果接受的串口1的数据为 '2' 并且已完成mpu6050角度初始化,则进入题目2程序，这里的角度初始化完成状态为 ‘N’
如果接受的串口1的数据为 '3' ,则继续发送有关题目2程序的小车参数，但让小车停止
如果接受的串口1的数据为 '4' 并且已完成mpu6050角度初始化,则进入题目3程序这里的角度初始化完成状态为 ‘C’
如果接受的串口1的数据为 '5' ,则继续发送有关题目3程序的小车参数，但让小车停止
如果接受的串口1的数据为 '6', 小车行驶的圈数小于4圈，并且已完成mpu6050角度初始化,则进入题目3程序这里的角度初始化完成状态为 ‘D’
如果接受的串口1的数据为 '7' 或小车行驶的圈数大于等于4圈，则继续发送有关题目4程序的小车参数，但让小车停止

题目切换状态机图

题目一

题目一的思路很简单，就是使小车按0度行驶,当小车检测到有黑线时，则认为到达B点，题目1完成

void qustion1(void)
 {
     if(IT.Infraed_Tracking_State == 11 && car_state == 'A')//小车红外循迹为全白
     {
         PID_Control(18, 3 , 7 , Value[2] , 0 , yaw_old ,&PID_left.pwm,car_state);
         PID_Control(18, 3 , 7 , 0 , Value[2] , yaw_old ,&PID_right.pwm,car_state);
     }
     else if(IT.Infraed_Tracking_State != 11) //小车红外循迹不为全白
     {
         car_state = 'B';
         PID_left.pwm = PID_right.pwm = 0;
     }
     if(car_state == 'B')
     {
             if(Buzzer_count <=20)//蜂鸣器工作1s
             {
                 Buzzer_count++;
                 DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
             }
             else {
                 DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
             }
     }
     DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
     DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
 }

题目二

题目二，先让小车按0度行驶，检测到黑线就正常循迹。当小车状态为‘B’时，红外循迹模块检测为全白，判断偏航角是否大于170度或小于-170度，是的话，小车状态为‘C’,小车按+-180度行驶。当小车状态为‘C’时，红外循迹模块检测到黑线，则认为小车到达D点，再正常循迹。当小车状态为‘D’时，红外循迹模块检测为全白，判断偏航角是否处于[-10,10]区间，如是，则认为到达A点，题目二完成

void question2(void)
{
    if(IT.Infraed_Tracking_State == 11 && car_state == 'A')//红外循迹为全白
    {
        PID_Control(15, 1 , 5 , Value[2] , 0 , yaw_old ,&PID_left.pwm,car_state);
        PID_Control(15, 1 , 5 , 0 , Value[2] , yaw_old ,&PID_right.pwm,car_state);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        yaw_old=Value[2];
    }
    else if(IT.Infraed_Tracking_State !=11 && car_state == 'A')//红外循迹不为全白
    {
        car_state = 'B';
        Buzzer_count = 0;
        yaw_old=0;
        PID_left.pwm = PID_right.pwm=0;
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
    }
    else if(car_state == 'B')//小车进入红外循迹
    {
        Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);
        PID_left.pwm = 520 + (pwm_duty_difference * 40);
        PID_right.pwm = 520 - (pwm_duty_difference * 40);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        
        if(Buzzer_count <=20)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        if(IT.Infraed_Tracking_State == 11 && car_state == 'B' && (Value[2] <= -170 || Value[2] >= 170))//如果红外循迹为全白,小车偏航角大于170度小于-170，则认为小车进入C点 
        {
            car_state = 'C';
            Buzzer_count = 0;
            pwm_duty_difference = 0;
            PID_left.pwm = PID_right.pwm=0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }        
    }
    else if(car_state == 'C')
    {
        if(Buzzer_count <=25)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }

        if(Value[2] < -0)
        {
            PID_Yaw  = Value[2] + 180;
        }
        if(Value[2] >= 0)
        {
            PID_Yaw = -180 + Value[2];  
        }
        PID_Control(15, 2 , 0 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
        PID_Control(15, 2 , 0 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);

        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        yaw_old=PID_Yaw;
        if(IT.Infraed_Tracking_State !=11 && car_state == 'C')//红外循迹不为全白 ，则小车到达D点
        {
            car_state = 'D';
            Buzzer_count = 0;
            yaw_old=0;
            PID_left.pwm = PID_right.pwm=0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state == 'D')
    {
        if(Buzzer_count <=25)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);
        PID_left.pwm = 520 + (pwm_duty_difference * 40);
        PID_right.pwm = 520 - (pwm_duty_difference * 40);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);

        if(IT.Infraed_Tracking_State == 11 && car_state == 'D' && (Value[2] >= -15 && Value[2] <= 15))//红外循迹为全白，小车偏航角为		[-15,15],小车进入'E'状态
        {
            car_state = 'E';
            Buzzer_count = 0;
            pwm_duty_difference = 0;
            PID_left.pwm = PID_right.pwm = 0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state ==  'E')
    {
        if(Buzzer_count <=25)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
    }
}

题目三

小车在A点，原地向右转，当偏航角为[-30,-40]时，小车进入 ‘a’状态。

小车为 ‘a’状态，向-37度行驶，当小车循迹状态不为全白时，小车进入C点。
小车进入C点，开始红外循迹，当循迹状态为全白并且(偏航角大于170度或小于-170度) ，小车到达B点。
小车进入B点，原地向左转，当偏航角为[-130,-150]时，小车进入'b'状态。
小车进入'b'状态,当红外循迹状态不为全白，小车进入'D'点。
小车进入'D'点，当红外循迹状态为全白并且偏航角为[-10,10]，小车状态为 'E'，题目三完成。
题目三状态机图

void question3(void)
{
    if(car_state == 'A')//小车在A点，原地向右转，当偏航角为[-30,-40]时，小车进入 ‘a’状态
    {
        DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
        DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);

        PID_left.pwm = 450;
        PID_right.pwm = 400;
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);

        if(Value[2] >= -40.0f  && Value[2] <= -30.0f)
        {
            car_state = 'a';
            PID_left.pwm = PID_right.pwm = 0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
            DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
            DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
        }
    }
    else if(car_state == 'a')//小车为 ‘a’状态，向-37度行驶，当小车循迹状态不为全白时，小车进入C点
    {
        PID_Yaw = 37 + Value[2];  
        
        PID_Control(18, 3 , 2 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
        PID_Control(18, 3 , 2 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);

        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        yaw_old =  PID_Yaw;
        if(IT.Infraed_Tracking_State != 11)
        {
            car_state = 'C';
            Buzzer_count = 0;
            yaw_old =0;
            PID_left.pwm = PID_right.pwm = 0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state == 'C')//小车进入C点 开始红外循迹 ， 当循迹状态为全白并且(偏航角大于170度或小于-170度) ，小车到达B点
    {
        if(Buzzer_count <=10)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);
        PID_left.pwm = 520 + (pwm_duty_difference * 40);
        PID_right.pwm = 520 - (pwm_duty_difference * 40);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        
        if(IT.Infraed_Tracking_State == 11 && (Value[2] <= -170 || Value[2] >= 170))
        {
            car_state = 'B';
            Buzzer_count = 0;
            pwm_duty_difference = 0;
            PID_left.pwm = PID_right.pwm=0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state == 'B')//小车进入B点 原地向左转 ， 当偏航角为[-130,-150]时，小车进入'b'状态
    {
        if(Buzzer_count <=10)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }

        DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
        DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);

        PID_left.pwm = 450;
        PID_right.pwm = 400;
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);

        if(Value[2] >= -150.0f  && Value[2] <= -130.0f)
        {
            car_state = 'b';
            PID_left.pwm = PID_right.pwm = 0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
            DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);
            DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
        }
    }
    else if(car_state == 'b')//小车进入'b'状态,当红外循迹状态不为全白，小车进入'D'点
    {
        PID_Yaw = 144 + Value[2];  
        
        PID_Control(18, 3 , 2 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
        PID_Control(18, 3 , 2 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);
        yaw_old = PID_Yaw;
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);

        if(IT.Infraed_Tracking_State != 11)
        {
            car_state = 'D';
            Buzzer_count = 0;
            yaw_old = 0;
            PID_left.pwm = PID_right.pwm = 0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state == 'D')//小车进入'D'点 ,当红外循迹状态为全白并且偏航角为[-10,10]，小车状态为 'E'
    {
        if(Buzzer_count <=10)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }

        Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);
        PID_left.pwm = 520 + (pwm_duty_difference * 40);
        PID_right.pwm = 520 - (pwm_duty_difference * 40);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
        DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
        
        if(IT.Infraed_Tracking_State == 11 && Value[2] <= 10 && Value[2] >= -10)
        {
            car_state = 'E';
            Buzzer_count = 0;
            pwm_duty_difference = 0;
            PID_left.pwm = PID_right.pwm=0;
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
            DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
        }
    }
    else if(car_state == 'E')//蜂鸣器工作
    {
        if(Buzzer_count <=10)
        {
            Buzzer_count++;
            DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
        else {
            DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
        }
    }
}

题目四

若小车在状态‘E’到C点偏右，就加上Value_yaw_old1，若偏左，则减去Value_yaw_old1。

/*因为mpu6050有累积误差，在小车在每次一圈结束时记录最后的偏航角,让新的偏航角减去这个乘个系数的最后的偏航角 一般最后一题在这调参*/
     Value[2] = Value[2] + Value_yaw_old1 * 0.12f;

小车状态为 ‘A’ ,让原地向右转。如果小车偏航角为[-70,-80],则小车进入‘a’状态。

小车状态为 ‘a’ ,向-75度行驶。小车左轮的竖直位移的计数值大于1170，进入到 'E'状态。在此状态下，进行对小车的左轮编码器计数求竖直方向的位移

因为减小角度误差对竖直位移计算的影响，小车要按-75度行驶，竖直位移计算公式为：

左轮总竖直位移 = 左轮上一时刻的总竖直位移 + (左轮位移 - 上一刻左轮位移) * cos(偏航角)

cos(X)在+-90度附近的导数(-sin(X))较小，能减小角度误差对竖直位移计算的影响，故小车向-75度行驶。

 if(car_state == 'a')//小车状态为 ‘a’ ,向-75度行驶
 {
         PID_Yaw = 75 + Value[2];
 
         //进行小车竖直位移的测量
         E1.Left_Wheel_Vertical_displacement_Value = E1.Left_Wheel_Vertical_displacement_Value + (E1.Left_Wheel_Value - E1.Left_Wheel_Value_old) * cos(Value_yaw_old) ;
       
         PID_Control(20, 3 , 2 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
         PID_Control(20, 3 , 2 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);
 
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
         yaw_old =  PID_Yaw;
         Value_yaw_old = (-90 - Value[2] ) * (M_PI / 180.00f);//小车上一刻的偏航角
         
         E1.Left_Wheel_Value_old = E1.Left_Wheel_Value;//小车上一刻的左轮计数值
   }

小车进入到‘E’状态进行转向，如果偏航角为(-3,3) ,小车进入‘F’状态。

小车进入到‘F’状态向0度行驶，如果小车循迹状态不为全白,则小车到C点。

小车进入到‘C’状态，进行黑线循迹。如果小车循迹状态为全白并且(偏航角大于172 或小于-172),则认为小车到了B点。

小车进入到‘B’点，原地向左转，如果小车偏航角为[-100,-110],则小车为‘b’状态。

小车进入到‘b’状态，向-105度行驶，如果小车左轮竖直方向计数值大于1170,则小车进入到‘G’状态。向-105度行驶的原因与状态‘a’的小车向-75度行驶的原因相同，故不做解释。

小车进入到‘G’状态原地向右转向，如果小车偏航角大于-175度或小于175度，则进入到H状态。

小车进入到‘H’状态向+-180度行驶，如果小车循迹状态不为全白，则进入到D点。

小车进入到‘D’状态进行红外循迹，如果小车循迹状态为全白并且偏航角为[-10,10],小车进入到‘I’状态。

小车进入到‘I’状态等蜂鸣器工作0.5s之后，再跳转到‘A’状态，小车行驶圈数自加1。若小车行驶圈数大于等于4圈(初始圈数为0)，则题目4完成

题目四状态机图

void question4(void)
 {
     /*因为mpu6050有累积误差，在小车在每次一圈结束时记录最后的偏航角,让新的偏航角减去这个乘个系数的最后的偏航角 一般最后一题在这调参*/
     Value[2] = Value[2] + Value_yaw_old1 * 0.12f;
 
     if(car_state == 'A')//小车状态为 ‘A’ ,让原地向右转
     {
         DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
         DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
 
         PID_left.pwm = 550;
         PID_right.pwm = 450;
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
 
         if(Value[2]  >= -80.0f  && Value[2] <= -70.0f)//如果小车偏航角为[-70,-80],则小车进入‘a’状态 
         {
             car_state = 'a';
             PID_left.pwm = PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
            
         }
     }
     else if(car_state == 'a')//小车状态为 ‘a’ ,向-75度行驶
     {
         PID_Yaw = 75 + Value[2];
 
         //进行小车竖直位移的测量
         E1.Left_Wheel_Vertical_displacement_Value = E1.Left_Wheel_Vertical_displacement_Value + (E1.Left_Wheel_Value - E1.Left_Wheel_Value_old) * cos(Value_yaw_old) ;
       
 
         PID_Control(20, 3 , 2 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
         PID_Control(20, 3 , 2 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);
 
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
         yaw_old =  PID_Yaw;
         Value_yaw_old = (-90 - Value[2] ) * (M_PI / 180.00f);//小车上一刻的偏航角
         
         E1.Left_Wheel_Value_old = E1.Left_Wheel_Value;//小车上一刻的左轮计数值
 
         if(E1.Left_Wheel_Vertical_displacement_Value> 1170 ) //小车左轮的竖直位移的计数值大于1170，进入到 'E'状态
         {
             Buzzer_count = 0;
             yaw_old =0;
             PID_left.pwm = 0;
             PID_right.pwm = 0;
             E1.Left_Wheel_Vertical_displacement_Value =0; E1.Left_Wheel_Value_old = 0;
             E1.Left_Wheel_Value = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);
             car_state = 'E';
         }
         
     }
     else if(car_state == 'C')//小车进入到‘C’状态 进行黑线循迹
     {
         if(Buzzer_count <=10)
         {
             Buzzer_count++;
             DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
         else {
             DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
 
         Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);//根据小车循迹状态 给出小车的左右轮pwm差值
         PID_left.pwm = 470 + (pwm_duty_difference * 40);
         PID_right.pwm = 470 - (pwm_duty_difference * 40);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
         
         if(IT.Infraed_Tracking_State == 11 && (Value[2] <= -172 || Value[2] >= 172))//如果小车循迹状态为全白并且(偏航角大于172 或小于-172),则认为小车到了B点
         {
             car_state = 'B';
             Buzzer_count = 0;
             pwm_duty_difference = 0;
             PID_left.pwm = PID_right.pwm=0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         }
     }
     else if(car_state == 'E') //小车进入到‘E’状态 进行转向，如果偏航角为(-3,3) ,小车进入‘F’状态
     {
         PID_left.pwm = 550;
         PID_right.pwm = 450;
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         if(Value[2] > -3 && Value[2] < 3)
         {
             car_state = 'F';
             PID_left.pwm = 0;
             PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
             
         }
        
     }
     else if(car_state == 'F')//小车进入到‘F’状态 向0度行驶，如果小车循迹状态不为全白,则小车到C点
     {
         PID_Control(20, 1 , 8 , Value[2] , 0 , yaw_old ,&PID_left.pwm,car_state);
         PID_Control(20, 1 , 8 , 0 , Value[2] , yaw_old ,&PID_right.pwm,car_state);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         yaw_old = Value[2];
 
         if(IT.Infraed_Tracking_State != 11)
         {
             car_state = 'C';
             yaw_old =0;
             PID_left.pwm = PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         }
         
     }
     
     else if(car_state == 'B')//小车进入到‘B’点 原地向左转，如果小车偏航角为[-100,-110],则小车为‘b’状态
     {
         if(Buzzer_count <=5)
         {
             Buzzer_count++;
             DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
         else {
             DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
 
         DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
         DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);
 
         PID_left.pwm = 550;
         PID_right.pwm = 450;
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
 
         if(Value[2] >= -110.0f  && Value[2] <= -100.0f)
         {
             car_state = 'b';
             PID_left.pwm = PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN1_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E1_AIN2_PIN);
         }
     }
     else if(car_state == 'b')//小车进入到‘b’点 向-105度行驶，如果小车左轮竖直方向计数值大于1170,则小车进入到‘G’状态
     {
         PID_Yaw = 105 + Value[2] ;  
         //进行小车竖直位移的测量
         E1.Left_Wheel_Vertical_displacement_Value = E1.Left_Wheel_Vertical_displacement_Value + fabs((E1.Left_Wheel_Value - E1.Left_Wheel_Value_old) * cos(Value_yaw_old)) ;
 
 
         PID_Control(20, 3 , 2 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
         PID_Control(20, 3 , 2 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);
         
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
         yaw_old = PID_Yaw;
         Value_yaw_old = ( 90.0f + Value[2] - 180.0f) * (M_PI / 180.00f);
       
         E1.Left_Wheel_Value_old = E1.Left_Wheel_Value; 
 
         if(E1.Left_Wheel_Vertical_displacement_Value> 1170 )
         {
             Buzzer_count = 0;
             yaw_old =0;
             PID_left.pwm = 0;
             PID_right.pwm = 0;
             E1.Left_Wheel_Vertical_displacement_Value = 0;E1.Left_Wheel_Value_old = 0; 
             E1.Left_Wheel_Value =0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
             car_state = 'G';
         }
 
         
     }
     else if(car_state == 'G')//小车进入到‘G’状态 原地向右转向，如果小车偏航角大于-175度或小于175度，则进入到H状态
     {
         PID_left.pwm = 550;
         PID_right.pwm = 450;
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         if(Value[2] < -175 || Value[2] > 175)
         {
             car_state = 'H';
             PID_left.pwm = 0;
             PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
             
         }
     }
     else if(car_state == 'H')//小车进入到‘H’状态 向+-180度行驶，如果小车循迹状态不为全白，则进入到D点
     {
         if(Value[2] < -0)
         {
             PID_Yaw  = Value[2] + 180;
         }
         if(Value[2] >= 0)
         {
             PID_Yaw = -180 + Value[2];  
         }
         PID_Control(20, 3 , 8 , PID_Yaw , 0 , 0,&PID_left.pwm,car_state);
         PID_Control(20, 3 , 8 , 0 , PID_Yaw , 0 ,&PID_right.pwm,car_state);
 
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         yaw_old=PID_Yaw;
 
         if(IT.Infraed_Tracking_State != 11)
         {
             car_state = 'D';
             yaw_old =0;
             DL_GPIO_setPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN1_PIN);
             DL_GPIO_clearPins(GPIO_Motor_PORT,GPIO_Motor_PIN_E2_AIN2_PIN);
             PID_left.pwm = PID_right.pwm = 0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         }
     }
     else if(car_state == 'D')//小车进入到‘D’状态 进行红外循迹，如果小车循迹状态为全白并且偏航角为[-10,10],小车进入到‘I’状态
     {
         if(Buzzer_count <=10)
         {
             Buzzer_count++;
             DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
         else {
             DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
 
         Judge_pwm_duty_difference(IT.Infraed_Tracking_State,&pwm_duty_difference);
         PID_left.pwm = 470 + (pwm_duty_difference * 40);
         PID_right.pwm = 470 - (pwm_duty_difference * 40);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm , DL_TIMER_CC_0_INDEX);
         DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm , DL_TIMER_CC_1_INDEX);
         
         if(IT.Infraed_Tracking_State == 11 && Value[2]    <= 10  && Value[2]  >= -10 )
         {
             car_state = 'I';
             Buzzer_count = 0;
             pwm_duty_difference = 0;
             PID_left.pwm = PID_right.pwm=0;
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_left.pwm, DL_TIMER_CC_0_INDEX);
             DL_TimerG_setCaptureCompareValue(PWM_0_INST, PID_right.pwm, DL_TIMER_CC_1_INDEX);
         }
     }
     else if(car_state == 'I')//小车进入到‘I’状态 等蜂鸣器工作之后 再跳转到‘A’状态，小车行驶圈数自加1
     {
         Value_yaw_old1 = Value[2];
         if(Buzzer_count <=10)
         {
             Buzzer_count++;
             DL_GPIO_setPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
         }
         else {
             DL_GPIO_clearPins(GPIO_Buzzer_PORT,GPIO_Buzzer_PIN_0_PIN);
             
             car_state = 'A';
             laps_number++;
         }
     }   
 }

四、代码文件

通过百度网盘分享的文件：empty_LP…
链接:https://pan.baidu.com/s/1N5vi2J0HFzZ_0mV7JbqbyQ?pwd=98zd
提取码:98zd
复制这段内容打开「百度网盘APP 即可获取

五、总结

填补在电赛的遗憾，就重做24年电赛题，从2024年十月初到10月30日，耗时一个月完成（挤出课余时间，其实不是每天都写代码），除了MPU6050串口接收的部分代码是商家资料提供，其他均是本人所写。由于本人才疏学浅，代码和解析的不足挺多的，比如多余的变量，中断服务函数代码过多，状态机图有些状态转换没画出以及部分文字口语化等。作者已经大三了，留给我的大学生活时间已经不多，接下来我要学FPGA(因为我是集成专业的)和一些未来的打算。在此，特别鸣谢帮助我的同学和老师（特别鸣谢高老师）。最后希望读者都有充实的大学生活。与君共勉。

作者于2024年10月31日编写完成