devbox
小舞很执着
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

【2024年电赛H题自动行驶小车】省一学长手把手从零教你学MSPM0_jy61p的零漂

作者：小舞很执着 | 2024-08-29 02:10:12

赞

踩

jy61p的零漂

一、前前言

第十二届浙江省大学生电子设计竞赛终于落下帷幕了，名单已公示，几家欢喜几家愁？我觉得每一位经历过电赛的朋友都称得上英雄，我想我们所学到的并非是“省一等”或“成功参赛奖”一个头衔能囊括的，相信真正有价值的东西会在人生的道路上生根发芽。

我写这篇博客的最初目的，也只是想用代码的方式记录这段伟大的时光。

二、前言

2024年省赛题目如下：

不难看出，题目的难点只有两个：

1、如何熟练使用TI MSPM0系列的板子？

2、无黑线部分如何尽可能保持直行？

对于第一个问题，我先给出我的解决方法：

1、【2024电赛】TI MSPM0快速入门课

2、LP-MSPM03507学习资料汇总

第一个链接是B站UP主Torris-Yin的基础教学视频，我觉得讲的很好，故上附链接推荐给各位。

第二个链接是我自己整理的纯代码形式的G3507资料，仅供参考！

第二个问题，我的解决方法是购买高精度的陀螺仪模块——JY61P、MPU6050。（不推荐使用MPU6050，需要软件去零漂）

好了，难点都解决了，现在发现其实拿下省一也没难度吧！

三、整体框架

基本思路就是：有线巡线，无线靠巡航角。

接下来进入正题，我会尽量按顺序一一介绍各模块的配置与使用，然后将各模块整合一块。

源码下附全开源，可根据需求跳转查看相应部分。

功底不深，多多包涵！

四、驱动电机

1、PWM配置

PWM Period Count：自动重装载值，相当于CUBEMX中的ARR

PWM Frequency：    PWM输出频率，建议选择15k ~ 20kHz间

2、PWM输通道及占空比配置

Channel Update Mode：死区PWM宽度设置，此处不设置死区影响不大。

3、正反转IO口配置

此四口仅配置为普通输出IO口即可

4、代码填写

AIN1、2控制左马达的正反转：AIN1为高，AIN2为低，则为正转；反之相反。

BIN1、2同理。

  1. /* 全局宏定义 */
  2. #define AIN1_High       DL_GPIO_setPins(DIR_PORT, DIR_AIN1_PIN)
  3. #define AIN1_Low        DL_GPIO_clearPins(DIR_PORT, DIR_AIN1_PIN)
  4. #define AIN2_High       DL_GPIO_setPins(DIR_PORT, DIR_AIN2_PIN)
  5. #define AIN2_Low        DL_GPIO_clearPins(DIR_PORT, DIR_AIN2_PIN)
  6. #define BIN1_High       DL_GPIO_setPins(DIR_PORT, DIR_BIN1_PIN)
  7. #define BIN1_Low        DL_GPIO_clearPins(DIR_PORT, DIR_BIN1_PIN)
  8. #define BIN2_High       DL_GPIO_setPins(DIR_PORT, DIR_BIN2_PIN)
  9. #define BIN2_Low        DL_GPIO_clearPins(DIR_PORT, DIR_BIN2_PIN)
  10.  
  11.  
  12.  
  13. /*
  14.     * 初始化
  15. */
  16. void Init(void)
  17. {
  18.     /* 关闭所有输出 */
  19.     AIN1_Low;
  20.     AIN2_Low;
  21.     BIN1_Low;
  22.     BIN2_Low;
  23.  
  24.     /* 使能PWM波输出 */
  25.     DL_Timer_startCounter(PWM_0_INST);
  26. }

至此，驱动电机部分完成。

五、读取编码器数值

此部分包含两个部分：配置IO口读取编码器配置、配置定时器定时读取编码值（此时的编码值相当于小车速度值）

编码器Port配置：GPIO_EncoderA\B

1、编码器IO口初始化配置

2、配置IO口及中断

Internal Resistor：上下拉电阻配置，由于读取的编码器有效电平为高电平，故选择配置为下拉电阻

Input Filter：           输入电平滤波，降低误读率

其余三个IO口配置同理。

3、代码填写

此代码选择的是四分频的编码器度数方式，精度更高。

想对编码器模式了解更多？详见【STM32+HAL】直流电机PID控制

  1. /*
  2.     * 编码器读取函数
  3.     * 编码器A、B分别对应编码器A、B的引脚
  4.     * EncoderA_CNT、EncoderB_CNT    分别对应编码器A、B的计数值
  5.     * EncoderA_Port、EncoderB_Port  分别对应编码器A、B的端口
  6. */
  7. void Encodering(void)
  8. {
  9.     EncoderA_Port = DL_GPIO_getEnabledInterruptStatus(GPIO_EncoderA_PORT, GPIO_EncoderA_PIN_0_PIN | GPIO_EncoderA_PIN_1_PIN);
  10.     EncoderB_Port = DL_GPIO_getEnabledInterruptStatus(GPIO_EncoderB_PORT, GPIO_EncoderB_PIN_2_PIN | GPIO_EncoderB_PIN_3_PIN);
  11.     
  12.     /* 编码器A */
  13.     if((EncoderA_Port & GPIO_EncoderA_PIN_0_PIN) == GPIO_EncoderA_PIN_0_PIN)        //编码器A-Pin0
  14.     {
  15.         if(!DL_GPIO_readPins(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_1_PIN))   EncoderA_CNT--;
  16.         else                                                                EncoderA_CNT++;
  17.     }
  18.     else if((EncoderA_Port & GPIO_EncoderA_PIN_1_PIN) == GPIO_EncoderA_PIN_1_PIN)   //编码器A-Pin1
  19.     {
  20.         if(!DL_GPIO_readPins(GPIO_EncoderA_PORT,GPIO_EncoderA_PIN_0_PIN))   EncoderA_CNT++;
  21.         else                                                                EncoderA_CNT--;
  22.     }
  23.     DL_GPIO_clearInterruptStatus(GPIO_EncoderA_PORT, GPIO_EncoderA_PIN_0_PIN|GPIO_EncoderA_PIN_1_PIN);
  24.  
  25.  
  26.     /* 编码器B */
  27.     if((EncoderB_Port & GPIO_EncoderB_PIN_2_PIN) == GPIO_EncoderB_PIN_2_PIN)
  28.     {
  29.         if(!DL_GPIO_readPins(GPIO_EncoderB_PORT,GPIO_EncoderB_PIN_3_PIN))   EncoderB_CNT--;
  30.         else                                                                EncoderB_CNT++;
  31.     }
  32.     else if((EncoderB_Port & GPIO_EncoderB_PIN_3_PIN) == GPIO_EncoderB_PIN_3_PIN)
  33.     {
  34.         if(!DL_GPIO_readPins(GPIO_EncoderB_PORT,GPIO_EncoderB_PIN_2_PIN))   EncoderB_CNT++;
  35.         else                                                                EncoderB_CNT--;
  36.     }
  37.     DL_GPIO_clearInterruptStatus(GPIO_EncoderB_PORT, GPIO_EncoderB_PIN_2_PIN|GPIO_EncoderB_PIN_3_PIN);
  38. }
  39.  
  40.  
  41.  
  42.  
  43.  
  44. /**************************************************************************
  45. Function: Read encoder count per unit time
  46. Input   : TIMX：Timer
  47. Output  : none
  48. 函数功能：单位时间读取编码器计数
  49. 入口参数：TIMX：定时器
  50. 返回  值：速度值
  51. **************************************************************************/
  52. void Read_Encoder(void)
  53. {
  54.     EncoderA_VEL = EncoderA_CNT;
  55.     EncoderA_CNT = 0;
  56.     EncoderB_VEL = EncoderB_CNT;
  57.     EncoderB_CNT = 0;
  58. }

编码器定时器配置：TIMER_Encoder_Read

1、定时器初始化配置

2、配置定时器周期及中断

因配置为周期向下计数模式，故中断对应的选择Zero event，即定时器计数到0触发中断。

3、代码填写

TIMER_Encoder_Read_INST_IRQHandler：定时器中断服务函数，每隔20ms进入中断读取编码值，并对速度变量赋值。

  1. /*
  2.     * 编码器初始化
  3. */
  4. void Encoder_Init(void)
  5. {
  6.     /* 关闭所有输出 */
  7.     AIN1_Low;
  8.     AIN2_Low;
  9.     BIN1_Low;
  10.     BIN2_Low;
  11.  
  12.     /* 使能编码器定时器、GPIO中断 */
  13.     NVIC_EnableIRQ(GPIO_EncoderA_INT_IRQN);
  14.     NVIC_EnableIRQ(GPIO_EncoderB_INT_IRQN);
  15.     NVIC_EnableIRQ(TIMER_Encoder_Read_INST_INT_IRQN);
  16.     DL_Timer_startCounter(TIMER_Encoder_Read_INST);
  17.  
  18.     /* 使能PWM波输出 */
  19.     DL_Timer_startCounter(PWM_0_INST);
  20. }
  21.  
  22.  
  23.  
  24.  
  25. /*
  26.     * 编码器读取中断服务函数
  27.     * 读取编码器数值
  28. */
  29. void TIMER_Encoder_Read_INST_IRQHandler(void)
  30. {
  31.     switch (DL_TimerG_getPendingInterrupt(TIMER_Encoder_Read_INST)){
  32.         case DL_TIMER_IIDX_ZERO:
  33.             Read_Encoder();            //赋值编码器数值
  34.             // printf("%d,%d\r\n",EncoderA_VEL,EncoderB_VEL);
  35.             break;
  36.         default:
  37.             break;
  38.     }
  39. }

 至此，我们实现了读取马达的速度值。

六、八路灰度传感器模块

1、IO口配置

普通输入IO口配置即可。

因为灰度传感器输出电压一般大于3.3V，建议不要配置上下拉电阻。

2、全局宏定义

检测到黑线时，输出高电平，变量置1。

  1. #define P1          DL_GPIO_readPins(Sensor_P1_PORT,Sensor_P1_PIN)
  2. #define P2          DL_GPIO_readPins(Sensor_P2_PORT,Sensor_P2_PIN)
  3. #define P3          DL_GPIO_readPins(Sensor_P3_PORT,Sensor_P3_PIN)
  4. #define P4          DL_GPIO_readPins(Sensor_P4_PORT,Sensor_P4_PIN)
  5. #define P5          DL_GPIO_readPins(Sensor_P5_PORT,Sensor_P5_PIN)
  6. #define P6          DL_GPIO_readPins(Sensor_P6_PORT,Sensor_P6_PIN)
  7. #define P7          DL_GPIO_readPins(Sensor_P7_PORT,Sensor_P7_PIN)
  8. #define P8          DL_GPIO_readPins(Sensor_P8_PORT,Sensor_P8_PIN)

至此，完成八路灰度传感器基础配置完毕。

七、串口获取巡航角

 1、串口配置

根据JY61P数据使用手册，配置为普通串口中断配置即可。（模块波特率需通过上位机修改，这里不赘述）

若想进一步提高通讯效率，详见文章中的串口DMA传输LP-MSPM03507学习资料汇总

2、代码填写
  1. #include "GYRO.h"
  2.  
  3. // 定义接收变量
  4. uint8_t RollL, RollH, PitchL, PitchH, YawL, YawH, VL, VH, SUM;
  5. float Pitch,Roll,Yaw;
  6.  
  7. // 串口接收状态标识
  8. #define WAIT_HEADER1 0
  9. #define WAIT_HEADER2 1
  10. #define RECEIVE_DATA 2
  11.  
  12. uint8_t RxState = WAIT_HEADER1;
  13. uint8_t receivedData[9];
  14. uint8_t dataIndex = 0;
  15.  
  16.  
  17. //发送置偏航角置零命令
  18. void Serial_JY61P_Zero_Yaw(void)
  19. {
  20.     DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
  21. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
  22. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X69);
  23. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X88);
  24. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XB5);
  25. 	delay_ms(100);
  26. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
  27. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
  28. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X76);
  29. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
  30. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
  31. 	delay_ms(100);
  32. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XFF);
  33. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0XAA);
  34. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
  35. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
  36. 	DL_UART_Main_transmitDataBlocking(UART_JY61P_INST,0X00);
  37.     delay_ms(500);
  38. }
  39.  
  40.  
  41.  
  42. // 串口中断处理函数
  43. /*
  44.     * 串口接收数据，并解析出角度
  45.     * 接收到的数据格式为：0x55 0x53 0xXX 0xXX 0xXX 0xXX 0xXX 0xXX 0xXX 0xXX
  46.     * 其中0xXX为角度数据，单位为0.1度
  47.     * 角度数据为16位，高8位在前，低8位在后
  48.     * 角度数据为正数表示顺时针旋转，负数表示逆时针旋转
  49.     * 角度数据为0表示水平
  50.     * 角度数据为180表示垂直
  51.     * 角度数据为90表示垂直向上
  52.     * 角度数据为-90表示垂直向下
  53.     * 角度数据为360表示水平向右
  54.     * 角度数据为-360表示水平向左
  55. */
  56. void UART_JY61P_INST_IRQHandler(void) 
  57. {
  58.     uint8_t uartdata = DL_UART_Main_receiveData(UART_JY61P_INST);   // 接收一个uint8_t数据
  59.  
  60.     switch (RxState) {
  61.     case WAIT_HEADER1:
  62.         if (uartdata == 0x55) {
  63.             RxState = WAIT_HEADER2;
  64.         }
  65.         break;
  66.     case WAIT_HEADER2:
  67.         if (uartdata == 0x53) {
  68.             RxState = RECEIVE_DATA;
  69.             dataIndex = 0;
  70.         } else {
  71.             RxState = WAIT_HEADER1;                                 // 如果不是期望的第二个头，重置状态
  72.         }
  73.         break;
  74.     case RECEIVE_DATA:
  75.         receivedData[dataIndex++] = uartdata;
  76.         if (dataIndex == 9) {
  77.             // 数据接收完毕，分配给具体的变量
  78.             RollL = receivedData[0];
  79.             RollH = receivedData[1];
  80.             PitchL = receivedData[2];
  81.             PitchH = receivedData[3];
  82.             YawL = receivedData[4];
  83.             YawH = receivedData[5];
  84.             VL = receivedData[6];
  85.             VH = receivedData[7];
  86.             SUM = receivedData[8];
  87.  
  88.             // 校验SUM是否正确
  89.             uint8_t calculatedSum = 0x55 + 0x53 + RollH + RollL + PitchH + PitchL + YawH + YawL + VH + VL;
  90.             if (calculatedSum == SUM) {
  91.                 // 校验成功，可以进行后续处理
  92.                 if((float)(((uint16_t)RollH << 8) | RollL)/32768*180>180){
  93.                     Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180 - 360;
  94.                 }else{
  95.                     Roll = (float)(((uint16_t)RollH << 8) | RollL)/32768*180;
  96.                 }
  97.  
  98.                 if((float)(((uint16_t)PitchH << 8) | PitchL)/32768*180>180){
  99.                     Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180 - 360;
  100.                 }else{
  101.                     Pitch = (float)(((uint16_t)PitchH << 8) | PitchL)/32768*180;
  102.                 }
  103.  
  104.                 if((float)(((uint16_t)YawH << 8) | YawL)/32768*180 >180){
  105.                     Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180 - 360;
  106.                 }else{
  107.                     Yaw = (float)(((uint16_t)YawH << 8) | YawL)/32768*180;
  108.                 }
  109.                 
  110.             } else {
  111.                 // 校验失败，处理错误
  112.             }
  113.             RxState = WAIT_HEADER1;                                 // 重置状态以等待下一个数据包
  114.         }
  115.         break;
  116.     }
  117. }

至此，陀螺仪模块配置完毕。

八、剩余外设配置

1、按键配置

按键一端接IO口，另一端接地即可，建议串联一个1kΩ左右电阻

按键EN配置同理

2、声光提示IO口

题目中要求每经过一个点需声光报警，我们配置为普通输出IO口即可。

3、代码填写
  1. #define LED_High        DL_GPIO_setPins(LED_PORT, LED_PIN_LED_PIN)
  2. #define LED_Low         DL_GPIO_clearPins(LED_PORT, LED_PIN_LED_PIN)
  3.  
  4.  
  5. while(1)
  6. {    
  7.     /* 按键扫描 */
  8.     Key_Scan();
  9.  
  10.     /* LED闪烁 */
  11.     LED_Sound();
  12. }
  13.  
  14.  
  15.  
  16. /*
  17.     * 按键扫描函数
  18.     * 按键按下时，切换mode
  19.     * 按键按下时，切换flag_en确认使能
  20. */
  21. void Key_Scan(void)
  22. {
  23.     /* 模式切换按键 */
  24.     if(DL_GPIO_readPins(KEY_PORT, KEY_S2_PIN) == 0)
  25.     {
  26.         delay_ms(10);
  27.         if(DL_GPIO_readPins(KEY_PORT, KEY_S2_PIN) == 0)
  28.         {
  29.             while(!DL_GPIO_readPins(KEY_PORT, KEY_S2_PIN));
  30.             mode = (mode + 1) % 5;
  31.         }
  32.     }
  33.  
  34.     /* 使能按键 */
  35.     if(DL_GPIO_readPins(KEY_PORT, KEY_EN_PIN) == 0)
  36.     {
  37.         delay_ms(10);
  38.         if(DL_GPIO_readPins(KEY_PORT, KEY_EN_PIN) == 0)
  39.         {
  40.             while(!DL_GPIO_readPins(KEY_PORT, KEY_EN_PIN));
  41.             flag_en = 1 - flag_en;
  42.         }
  43.     }
  44. }
  45.  
  46.  
  47.  
  48. /*
  49.     * LED闪烁函数
  50. */
  51. void LED_Sound(void)
  52. {
  53.     if(flag_LED)
  54.     {
  55.         LED_High;
  56.         LED_CNT  = LED_CNT + 0.1;
  57.         if(LED_CNT >= 800)         //闪烁延时
  58.         {
  59.             LED_Low;
  60.             LED_CNT = 0;
  61.             flag_LED = 0;
  62.         }
  63.     }
  64. }

4、控制定时器TIMER_0

控制函数需要保证尽可能的不被打断，所以放在定时器里是一个很好的选择。

而放在while里的优点是执行周期短，故考虑两者可尽量降低定时器周期

1）设置定时器周期500us

2）开启中断

九、速度环、差速环实现

速度环

速度环的目标值为设定目标速度，测试参数时需先关闭差速环，仅测量速度环，并对目标速度Motor_Left赋初值，通过上位机查看当前速度与目标速度的数值曲线来调节PID参数。

  1. //速度环PID
  2. #define   Kp1   	40
  3. #define   Ki1     	0.13
  4. #define   Kd1  		0.1
  5.  
  6.  
  7. /*=================== 增量式PID控制设计 ===================*/
  8. //左A轮PID
  9. float PID_A(float Encoder,float Target)
  10. {
  11. 	static float Bias, Last_bias, Last2_bias, Pwm;
  12. 	Bias = Target - Encoder;
  13. 	Pwm += Kp1 * (Bias - Last_bias) + Ki1 * Bias + Kd1 * (Bias - 2 * Last_bias + Last2_bias);
  14.  
  15. 	Last_bias = Bias;
  16. 	Last2_bias = Last_bias;
  17. 	return Pwm;
  18. }
  19.  
  20.  
  21. //右B轮PID
  22. float PID_B(float Encoder,float Target)
  23. {
  24. 	static float Bias, Last_bias, Last2_bias, Pwm;
  25. 	Bias = Target-Encoder;
  26. 	Pwm += Kp1 * (Bias - Last_bias) + Ki1 * Bias + Kd1 * (Bias - 2 * Last_bias + Last2_bias);
  27.  
  28. 	Last_bias = Bias;
  29. 	Last2_bias = Last_bias;
  30. 	return Pwm;
  31. }

差速环

差速环的目标值为控制量的目标值，即陀螺仪的差速环GYRO_Control的目标值应始终为0（直行），输入为当前巡航角，输出为应偏移数值（bias）

巡线的差速环Incremental_Quantity略有不同，可以这么理解：当所有光电都返回为识别到白色，则两轮差速值为0；若离车体中心的两光电检测到黑线，则返回一个较小的偏差值；距离越远，返回的偏差值越大。

差速环的参数调节应在速度环之后。

  1. //陀螺仪PID
  2. #define   Kp3       1
  3. #define   Ki3       0
  4. #define   Kd3  	    0
  5.  
  6. /*
  7.     函数功能：角度PID
  8.     IN: 当前值，目标值
  9.     OUT: 输出值
  10. */
  11. float GYRO_Control(float now,float target)
  12. {
  13. 	static float Bias, Last_bias, Last2_bias, Pwm;
  14. 	Bias = target-now;
  15. 	Pwm += Kp3 * (Bias - Last_bias) + Ki3 * Bias + Kd3 * (Bias - 2 * Last_bias + Last2_bias);
  16.  
  17. 	Last_bias = Bias;
  18. 	Last2_bias = Last_bias;
  19. 	return Pwm;
  20. }
  21.  
  22.  
  23.  
  24. /*
  25.     函数：Incremental_Quantity
  26.     功能：普通直行巡线模式
  27.     参数：无
  28. */
  29. int Incremental_Quantity(void)
  30. {
  31.     int value = 0;
  32.     if(!P1) //检测到最右端
  33.         value += 12;
  34.     if(!P2) 
  35.         value += 9;
  36.     if(!P3)
  37.         value += 7;
  38.     if(!P4)
  39.         value += 3;
  40.     if(!P5)
  41.         value -= 3;
  42.     if(!P6)
  43.         value -= 7;
  44.     if(!P7)
  45.         value -= 9;
  46.     if(!P8)
  47.         value -= 12;
  48.     return value;
  49. }

速度环+差速环

为方便理解，这里暂时使用了部分伪代码，基本思想为：括号里为判断当前是否落在黑线上语句，若有一个在黑线上，则bias赋值为bias1，即巡线差速值；若全不在黑线上，bias赋值为bias2，即陀螺仪差速值。然后在中值速度Speed_Middle的基础上加减bias，自此实现了差速环控制。

  1. #define  Limit		3800			//PWM波限幅
  2. /*************************************/
  3. float Speed_Middle = 30;			                    //中值速度
  4. int Motor_Left, Motor_Right;		                    //左右马达占空比	
  5.  
  6.  
  7. /**************************************************************************
  8. Function: Control function
  9. Input   : none
  10. Output  : none
  11. 函数功能：控制小车巡线
  12. 入口参数：无
  13. 返回  值：无
  14. **************************************************************************/
  15. void Control(void)
  16. {
  17.     float TargetA, TargetB;                         //目标速度
  18.     float bias1, bias2, bias;                       //巡线、陀螺仪偏差
  19.  
  20.     bias1 = Incremental_Quantity();
  21.     bias2 = GYRO_Control(Yaw, 0);
  22.     bias = (是否检查到黑线) ？ bias1 : bias2;
  23.  
  24.     TargetA = Speed_Middle - bias;
  25.     TargetB = Speed_Middle + bias;
  26.  
  27.     Motor_Left  = (int)PWM_Limit(PID_A(EncoderA_VEL,TargetA), Limit, -Limit);    //占空比限幅
  28.     Motor_Right = (int)PWM_Limit(PID_B(EncoderB_VEL,TargetB), Limit, -Limit);
  29.     Set_Pwm(Motor_Left, Motor_Right);
  30. }
  31.  
  32.  
  33.  
  34.  
  35. /*
  36.     函数功能：设置PWM比较值
  37.     IN: 左马达占空比，右马达占空比
  38.     OUT: 无
  39. */
  40. void Set_Pwm(int Left, int Right)
  41. {
  42.     if(Left > 0)    {AIN1_High;AIN2_Low;}      //左正转
  43.     else            {AIN1_Low;AIN2_High;}      //左反转
  44.     DL_Timer_setCaptureCompareValue(PWM_0_INST, myabs(Left), DL_TIMER_CC_0_INDEX);
  45.  
  46.     if(Right > 0)   {BIN1_High;BIN2_Low;}      //右正转
  47.     else            {BIN1_Low;BIN2_High;}      //右反转
  48.     DL_Timer_setCaptureCompareValue(PWM_0_INST, myabs(Right), DL_TIMER_CC_1_INDEX);
  49. }

十、四种巡线模式

1、整体代码

由于此部分函数过于冗长，故先采用伪代码形式呈现供大家理解。代码虽长，逻辑简单。

基本思路为：通过按键切换巡线模式mode后，小车进入不同的if语句块，每个语句块放置各个模式的顺序执行代码。

  1. #include "Sensor.h"
  2.  
  3. /*
  4.     函数功能：路径控制总函数
  5.     参数：无
  6. */
  7. int Follow_Route(void)
  8. {
  9.     static int cnt = 0;
  10.     /* 模式一 */
  11.     if(mode == 1)
  12.     {
  13.         Follow_Route_Mode1();     //模式一时，仅需判断一次黑线
  14.         return 1;
  15.     }
  16.  
  17.     /* 模式二 */
  18.     else if(mode == 2)
  19.     {
  20.         Follow_Route_Mode2();    //模式二时，通过改变flag标志位实现四段路径的切换
  21.         return 2;
  22.     }
  23.  
  24.     /* 模式三 */
  25.     else if(mode == 3)
  26.     {
  27.         Follow_Route_Mode3(0);      //仅进行一次八字绕圈
  28.         return 3;
  29.     }
  30.  
  31.     /* 模式四 */
  32.     else if(mode == 4)
  33.     {
  34.         Follow_Route_Mode4();      //循环绕四圈
  35.         return 4;
  36.     }
  37.     return 0;
  38. }

2、全部代码

逻辑不难，大家自行理解吧！

  1. #include "Sensor.h"
  2.  
  3. #define Black_CNT1          30                               //mode = 1 黑线持续次数
  4. #define Black_CNT2          30                               //mode = 2 黑线持续次数
  5. #define Black_CNT3          50                               //mode = 3、4 黑线持续次数
  6.  
  7. #define White_CNT           1000                              //白线持续次数  
  8. #define White_CNT2          5000                              //mode = 2 白线持续次数
  9. #define White_CNT3          7000                              //mode = 3、4 白线持续次数
  10.  
  11.  
  12. extern volatile int flag;                                  //flag状态位：0-暂停；1-AB；2-BC；3-CD；4-DA；
  13. extern volatile bool flag_LED;
  14. extern volatile int mode;                                  //mode状态位：0-暂停；
  15.  
  16.  
  17.  
  18. /*
  19.     函数功能：路径控制总函数
  20.     参数：无
  21. */
  22. int Follow_Route(void)
  23. {
  24.     static int cnt = 0;
  25.     /* 模式一 */
  26.     if(mode == 1)
  27.     {
  28.         if(flag == 1)                                                    //AB段末尾
  29.         {
  30.             if(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8)        //任一点检测到黑线
  31.             {
  32.                 cnt++;
  33.                 // printf("%d\r\n", cnt);
  34.                 if(cnt > Black_CNT1)                                     //检测到黑线持续5次
  35.                 {
  36.                     flag_LED = 1;
  37.                     flag = 0;
  38.                     cnt = 0;
  39.                     return 0;
  40.                 }
  41.             }
  42.             else   cnt = 0;                                         //连续检测黑线判定
  43.         }   
  44.     }
  45.  
  46.     /* 模式二 */
  47.     else if(mode == 2)
  48.     {
  49.         /* B点判定 */
  50.         if(flag == 1)
  51.         {
  52.             if(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8)        //任一点检测到黑线
  53.             {
  54.                 cnt++;
  55.                 if(cnt > Black_CNT2)             //检测到黑线持续5次
  56.                 {
  57.                     flag_LED = 1;
  58.                     flag = 2;               //进入状态二
  59.                     cnt = 0;
  60.                     return 2;
  61.                 }
  62.             }
  63.             else   cnt = 0;
  64.         }
  65.         /* C点判定 */
  66.         else if(flag == 2)
  67.         {
  68.             if(!(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8))                  //所有点检测到白线
  69.             {
  70.                 cnt++;
  71.                 if(cnt > White_CNT2)               //检测到白线持续White_CNT2次
  72.                 {
  73.                     flag_LED = 1;
  74.                     flag = 3;
  75.                     cnt = 0;
  76.                     return 3;
  77.                 }
  78.             }
  79.             else   cnt = 0;
  80.         }
  81.         /* D点判定 */
  82.         else if(flag == 3)
  83.         {
  84.             if(P1 || P2 || P3 || P4)        //任一点检测到黑线
  85.             {
  86.                 cnt++;
  87.                 if(cnt > Black_CNT2)                //检测到黑线持续Black_CNT2次
  88.                 {
  89.                     flag_LED = 1;
  90.                     flag = 4;
  91.                     cnt = 0;
  92.                     return 4;
  93.                 }
  94.             }
  95.             else   cnt = 0;
  96.         }
  97.         /* A点（终点）判定 */
  98.         else if(flag == 4)
  99.         {
  100.             if(!(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8))                  //所有点检测到白线
  101.             {
  102.                 cnt++;
  103.                 if(cnt > White_CNT4 )                  //检测到白线持续White_CNT4次
  104.                 {
  105.                     flag_LED = 1;
  106.                     flag = 0;           //一圈结束
  107.                     cnt = 0;
  108.                     return 0;
  109.                 }
  110.             }
  111.             else   cnt = 0;
  112.         }
  113.     }
  114.  
  115.     /* 模式三 */
  116.     else if(mode == 3)
  117.     {
  118.         Follow_Route_Mode3(0);      //仅进行一次绕圈
  119.         return 3;
  120.     }
  121.  
  122.     /* 模式四 */
  123.     else if(mode == 4)
  124.     {
  125.         Follow_Route_Mode4();      //循环绕四圈
  126.         return 4;
  127.     }
  128.     return 0;
  129. }
  130.  
  131.  
  132.  
  133.  
  134. /*
  135.     函数功能：模式三路径控制
  136.     参数：  stop：循环停止标志位
  137.     stop - 1：循环继续
  138.     stop - 0：循环一次
  139. */
  140. int Follow_Route_Mode3(int stop)
  141. {
  142.     static int cnt3 = 0;
  143.     /* C点判定 */
  144.     if(flag == 1)
  145.     {
  146.         if(P5 || P6 || P7 || P8)        //车体左侧任一点检测到黑线
  147.         {
  148.             cnt3++;
  149.             if(cnt3 > Black_CNT3)               //检测到黑线持续Black_CNT3次
  150.             {
  151.                 flag_LED = 1;
  152.                 flag = 2;               //进入下一状态
  153.                 cnt3 = 0;
  154.                 return 2;
  155.             }
  156.         }
  157.         else   cnt3 = 0;
  158.     }
  159.  
  160.     /* B点判定 */
  161.     else if(flag == 2)
  162.     {
  163.         if(!(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8))              //所有点检测到白线
  164.         {
  165.             cnt3++;
  166.             if(cnt3 > White_CNT3)                             //检测到白线持续White_CNT3次
  167.             {
  168.                 flag_LED = 1;
  169.                 flag = 3;
  170.                 cnt3 = 0;
  171.                 return 3;
  172.             }
  173.         }
  174.     }
  175.  
  176.     /* D点判定 */
  177.     else if(flag == 3)
  178.     {
  179.         if(P1 || P2 || P3 || P4)        //车体右侧任一点检测到黑线
  180.         {
  181.             cnt3++;
  182.             if(cnt3 > Black_CNT3)                      //检测到黑线持续Black_CNT3次
  183.             {
  184.                 flag_LED = 1;
  185.                 flag = 4;
  186.                 cnt3 = 0;
  187.                 return 4;
  188.             }
  189.         }
  190.     }
  191.  
  192.     /* A点判定 */
  193.     else if(flag == 4)
  194.     {
  195.         if(!(P1 || P2 || P3 || P4 || P5 || P6 || P7 || P8))             //所有点检测到白线
  196.         {
  197.             cnt3++;
  198.             if(cnt3 > White_CNT3)                      //检测到白线持续White_CNT3次
  199.             {
  200.                 flag_LED = 1;
  201.                 flag = stop;                     //停止
  202.                 cnt3 = 0;
  203.                 return 0;
  204.             }
  205.         }
  206.     }
  207.     return 0;
  208. }
  209.  
  210.  
  211.  
  212.  
  213.  
  214. /*
  215.     函数功能：模式四路径控制
  216. */
  217. int Follow_Route_Mode4(void)
  218. {
  219.     Follow_Route_Mode3(1);  //循环三圈不停止
  220.     Follow_Route_Mode3(1);
  221.     Follow_Route_Mode3(1);
  222.     Follow_Route_Mode3(0);      //停止
  223.     return 0;
  224. }

十一、模块整合

核心控制代码放在main.c文件中，关键函数接口主要从Sensor.c、Control.c两个文件中引出。

main.c通过按键扫描来选择开启哪个模式

Sensor.c包含各模式的巡线流程代码

Control.c包含各模式速度环及差速环的PID函数代码

1、各个全局标志位代码的含义与赋值方式
  1. /* 
  2. 按键一：改变mode值以切换当前模式
  3. 按键二：使能flag_en以确认当前模式
  4. flag ： 状态机思想，每结束一段路径通过软件切换当前模式数
  5. */
  6.  
  7.  
  8. volatile int flag = 1;                                  //flag状态位：0-暂停；1-AB段；2-BC段；3-CD段；4-DA段；
  9. volatile int mode = 0;                                  //mode状态位：0-暂停；1-模式一... ...；
  10. volatile int flag_en = 0;                               //flag_en状态位：0-不使能；1-使能

2、初始化配置

用惯了HAL库代码书写习惯，，相信大家能明白我的意思！

  1.     SYSCFG_DL_init();
  2.   /* USER CODE BEGIN 2 */
  3.  
  4. /* GYRO初始化 */
  5. Serial_JY61P_Zero_Yaw();
  6. NVIC_EnableIRQ(UART_JY61P_INST_INT_IRQN);
  7. delay_ms(500);
  8.  
  9. /* 编码器及PWM初始化 */
  10. Encoder_Init();
  11.  
  12. /* 使能控制定时器 */
  13. NVIC_EnableIRQ(TIMER_0_INST_INT_IRQN);
  14. DL_Timer_startCounter(TIMER_0_INST);
  15.  
  16.   /* USER CODE END 2 */
  17.

3、while(1)填写

CPU不断扫描按键状态，当开启使能时开启巡线函数。

  1.     while (1) {
  2.  
  3.     /* 按键扫描 */
  4.     Key_Scan();
  5.  
  6.     if(flag_en)         //使能控制开启
  7.     {
  8.         Follow_Route(); //路径控制总函数函数
  9.         /* LED闪烁 */
  10.         LED_Sound();
  11.     }
  12. }
4、定时器控制中断

每隔500us进入控制中断，且只在使能情况下开启控制函数。

  1. /*
  2.     * 定时器0中断服务函数
  3. */
  4. void TIMER_0_INST_IRQHandler(void)
  5. {
  6.     switch (DL_TimerA_getPendingInterrupt(TIMER_0_INST)) {
  7.     case DL_TIMERA_IIDX_ZERO:
  8.         //flag位判断
  9.         if(flag == 0 || flag_en == 0 || mode == 0)      Set_Pwm(0,0);
  10.         else if(flag_en)                                Control();
  11.     break;
  12.     default:
  13.     break;
  14.     }
  15. }

十二、源码提供

夸克网盘：提取码：miXa

百度网盘：提取码：6666

Gitee：Automatic_Driving

CSDN：Automatic-Driving

十三、结语

本人能力有限，代码未必是最优解，若有可改进之处望在评论区留言。

若想看看其他方案，欢迎前往我好友的博客【2024电赛H题自动行使小车】 省一获奖方案整理汇

如有小伙伴想交流学习心得，欢迎加入群聊751950234，群内不定期更新代码，以及提供本人博客所有源码

声明：本文内容由网友自发贡献，转载请注明出处：【wpsshop】
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号