2024年电赛H题自动行驶小车全代码思路讲解

电子扫地僧

已于 2024-08-09 18:39:01 修改

阅读量775

点赞数 11

文章标签： vscode c语言单片机 mcu

于 2024-08-09 18:32:45 首次发布

本文链接：https://blog.csdn.net/2301_79299887/article/details/141063042

版权

前言：

笔者大二，今年是第二次参加电赛，感慨万千。本来准备的是视觉模块寻迹（openmv），并没有考虑到通过硬件寻迹，在赛题刚刚公布的时候看到“寻迹”两字暗自窃喜，结果看到不能使用视觉模块的要求的时候，恍如晴天霹雳，视觉队友原地下班。因为我负责的是主控部分，而恰恰准备的是32部分，写的也是32部分的模块，题目出来要求用TI板卡的M0系列，导致写的这些模块也没用上。恰恰就我一人学了TI板卡的M0系列，于是乎“重担”就落在了我的身上。赛前传出许多消息，M0系列可能会出现在控制类中，于是为了以防万一，我还是去学了一下相关教学视频的，在此必须感谢b站up主@Torris-Yin，感谢他为TI板卡M0系列的生态环境做出的贡献，让我们能够快速熟悉不至于去官网啃一段段让人头晕目眩的英文。同时感谢b站up主@江协科技，我是学他的32系列教程入门的。并且还要感谢b站up主@Alice_西风和@左-岚，在本次代码中的许多有关“状态机”思维都是通过他的“蓝桥杯单片机系列”免费课程中学得理解并熟悉的，感谢四位的伟大的免费开源精神，正是因为体会到了他们的开源精神，同时也看到很多人贩卖本题有关代码，于是选择在此开源我的完赛代码。最后，还要感谢b站up主@上nm网课呢，在赛前向up主求得他移植的维特智能的jy系列协议解析代码，没想到刚好派上了用场（笔者所在赛区是没有严令禁止不得使用带有MCU的传感器的），此次开发软件为TI官方软件CCS Theia 1.4.1。话不多说，开始讲解，这是我第一次在csdn上发贴开源，如果有语言表达不清晰的，或者表述不够清楚的，还望各位见谅，欢迎大家友好讨论。

首先贴出题目要求：

一、题目描述
设计一个采用 TI MSPM0 系列 MCU 控制的自动行驶小车，能在指定路径上自动行驶，行驶场地示意如图所示。场地面积不小于 220cm×120cm。图中两个对称半圆弧线的半径为 40cm，弧线为黑色，线宽 1.8cm 左右，弧线的四个顶点分别定义为 A、B、C 和 D 点。建议场地采用白色哑光喷绘布制作。场地除两个半圆弧外，不得添加任何标记。

要求：

（1）将小车放在位置 A 点，小车能自动行驶到 B 点停车，停车时有声光提示。用时不大于 15 秒。

（2）将小车放在位置 A 点，小车能自动行驶到 B 点后，沿半弧线行驶到 C 点，再由 C 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车，每经过一个点，声光提示一次。完成一圈用时不大于 30 秒。

（3）将小车放在位置 A 点，小车能自动行驶到 C 点后，沿半弧线行驶到 B 点，再由 B 点自动行驶到 D 点，最后沿半弧线行驶到 A 点停车。每经过一个点，声光提示一次。完成一圈用时不大于 40 秒。

（4）按要求 3 的路径自动行驶 4 圈停车，用时越少越好。

二、重点
我们的基本思路是，使用 MSPM0G3507 控制小车，七路灰度巡线模块，陀螺仪控制方向。难点一个是跑直线，一个是转弯。我整体的pid算法只用到了姿态环+寻迹环，姿态环控制小车直行或走斜线，寻迹环控制小车在弯道寻迹。

（1）跑直线

我们使用的是由两轮和一个万向轮组成的三轮车，而由于MSPM0G3507仅配备了一个编码器，这使得之前使用视觉寻迹小车时的体验并不理想。因此，我们决定放弃使用编码器，改为直接通过控制PWM信号来驱动小车。我们给两个轮子提供相同的PWM信号，使其能够同时运动，但是大家都知道这只是理论上的同时运动，实际上还会因为地面上的摩擦力不同以及诸多原因，导致运动的时候不能够走直线，于是便编写了一个简单的转向控制环。当按下按键1时，我们将初始化yaw角为0，并将其作为目标值（实际运用中，我发现这个目标值还是要稍微改一下的，我代码中大概是1°左右，这是调参的时候根据具体情况做出的稍微调整的）。通过将实际测量的yaw角值作为实际值输入到PID控制器中，我们能够进行相应的PID运算，从而实现对小车平稳直线行驶的控制。

（2）转弯

为了实现转弯，我们采用了陀螺仪，通过陀螺仪反馈的角度来使小车转动到固定的角度。因此，这两个难点的最终解决方案都依赖于陀螺仪。陀螺仪的精度是否达标直接决定了是否能够成功完成这个任务。

三、陀螺仪的选择
我们首先考虑使用MPU6050，这是一款便宜且易于使用的陀螺仪，尤其因为我们在备赛时已经有移植好的MPU6050代码。然而，便宜的价格往往有其原因。MPU6050的yaw值存在严重的零飘问题，每秒钟偏差约为2°，这偏差的影响直接给这套题造成了不小的影响。

于是我们就考虑使用更加高精度的陀螺仪，而最恰好的就是，我们此前就已经备好了维特智能科技陀螺仪移植到MSPM0G3507的代码，这直接为我们解决了许多有关角度大大小小的麻烦，他们家的陀螺仪太好用了！！！！

四、思路
1、第一问：

第一问其实很简单，我有两个思路，思路一就是我们确定好点位，让小车在A点正对D点，给左右电机输出一定的相同pwm，然后写一个姿态环，控制小车直行（前文有论述），再使能一个10ms的定时器，定义一个多少多少秒（这个时间是你自己具体测的，在你给电机的PWM不变的情况下A点到D点所花的时间）的时间状态标志位，就比如定义一个名为“Timer_1000ms”的1s时间标志位吧，那就是当Timer_1000ms == 100的时候就到达1s了，此时写个if语句，当满足这个时间条件的时候关闭电机，如此思路一就完成。思路二呢，就是同样让小车在A点正对D点，给左右电机输出一定的相同pwm，然后写一个姿态环，控制小车直行（前文有论述），写一条语句判断七路巡线模块是否为低电平，当七路其中任意一路不为低电平的时候就代表着扫到黑线了（这个判断语句放在10ms的定时器令它不断运行判断），那么因为之前是对准了A到D小车直行的嘛，检测到黑线的时候也同时代表着到达B点，停止小车。

2、第二问：

这一题同样是两种思路，思路一也是定个特定时间让A到B然后就开始寻迹，到达C点又开始特定时间C到D然后开始寻迹再到A点，这跟第一问的思路一没啥太大的区别。唯一比较难的是寻迹，有关pid的我会在后面说。思路二也同第一题的思路二差不多，扫到黑线（到B）然后开始寻迹扫不到黑线（到C）代表要直行了然后直到扫到黑线（到D），然后寻迹然后扫不到黑线（到A）。

3、第三四问：

第三四问思路是相同的，第四问也仅仅只是在第三问的基础上拓展了一下而已，那么这里只讲第三问思路，第三问从最开始就让小车从A点正对C点，然后开始姿态环pid控制直行，然后到达C点开始寻迹，寻到B点时代表巡线模块扫不到黑线了，这个时候初始化yaw角为0再根据实际转角写一个姿态环，这个要转的yaw角度就是目标值，测量值就是10ms定时器中一直运行测量的yaw值（具体看代码吧），如此控制直行到D点，检测到黑线，然后开始寻迹，直至到达A点。

五、PID算法部分

1、姿态环：

姿态环我写的很简单的，就这样就解决了，最重要的就是调参部分的Kp设置了

2、寻迹环：

寻迹环当时看了CSDN很多大佬的思想，然后再加上问了chatGPT4O，然后实现的。

首先读取传感器的偏移位置：

// 读取传感器数据并计算偏移位置
int readSensors()
{
    int position = 0;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_1_PORT, GPIO_Xunji_xunji_PIN_1_PIN)!=0) position = -3;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_2_PORT, GPIO_Xunji_xunji_PIN_2_PIN)!=0) position = -2;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_3_PORT, GPIO_Xunji_xunji_PIN_3_PIN)!=0) position = -1;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_4_PORT, GPIO_Xunji_xunji_PIN_4_PIN)!=0) position = 0;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_5_PORT, GPIO_Xunji_xunji_PIN_5_PIN)!=0) position = 1;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_6_PORT, GPIO_Xunji_xunji_PIN_6_PIN)!=0) position = 2;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_7_PORT, GPIO_Xunji_xunji_PIN_7_PIN)!=0) position = 3;

    return position;
}

然后判断是否进入弯道（写一个布尔就行了）：

// 检测是否进入弯道
bool isInTurn(int position)
{
    return (position <= -2 || position >= 2);
}

然后开始寻迹部分：

void xunji(void)
{
    int position = readSensors();
    float error = target - position;

    // 检测是否在弯道
    bool inTurn = isInTurn(position);

    // 根据是否在弯道调整PID参数
    if (inTurn)
    {
        X_Kp = 900.0;
        X_Kd = 200.0;
    }
    else
    {
        X_Kp = 500.0;
        X_Kd = 0.0;
    }

    // 计算PID修正值
    float correction = computeIncrementalPID(error);
    // 设置速度，在弯道时降低速度
    if (inTurn)
    {
        // Set_Speed(0, 20 - correction); // 左轮
        // Set_Speed(1, 20 + correction); // 右轮
        Set_PWM(4900+correction,4900-correction);
    }
    else
    {
        // Set_Speed(0, 25 - correction); // 左轮
        // Set_Speed(1, 25 + correction); // 右轮
        Set_PWM(3000+correction,3000-correction);
    }
}

整个循迹pid部分如下：

#include "Xunji.h"
#include "Motor.h"
// PID参数
float X_Kp = 900.0;
float X_Ki = 0.0;
float X_Kd = 200.0;
// PID变量
float lastError = 0.0;
float integral = 0.0;
float lastDerivative = 0.0;
// 目标值
const float target = 0.0;

// 防止积分饱和的限值
const float integralMax = 7200.0;
const float integralMin = -7200.0;

// 增量PID计算
float computeIncrementalPID(float currentError)
{
    // 计算增量误差
    float derivative = currentError - lastError;

    // 积分部分的抗饱和处理
    integral += currentError;
    if (integral > integralMax) integral = integralMax;
    if (integral < integralMin) integral = integralMin;

    // 增量计算
    float output = X_Kp * currentError + X_Ki * integral + X_Kd * (derivative - lastDerivative);

    lastError = currentError;
    lastDerivative = derivative;

    return output;
}

// 读取传感器数据并计算偏移位置
int readSensors()
{
    int position = 0;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_1_PORT, GPIO_Xunji_xunji_PIN_1_PIN)!=0) position = -3;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_2_PORT, GPIO_Xunji_xunji_PIN_2_PIN)!=0) position = -2;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_3_PORT, GPIO_Xunji_xunji_PIN_3_PIN)!=0) position = -1;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_4_PORT, GPIO_Xunji_xunji_PIN_4_PIN)!=0) position = 0;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_5_PORT, GPIO_Xunji_xunji_PIN_5_PIN)!=0) position = 1;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_6_PORT, GPIO_Xunji_xunji_PIN_6_PIN)!=0) position = 2;
    if (DL_GPIO_readPins(GPIO_Xunji_xunji_PIN_7_PORT, GPIO_Xunji_xunji_PIN_7_PIN)!=0) position = 3;

    return position;
}

// 检测是否进入弯道
bool isInTurn(int position)
{
    return (position <= -2 || position >= 2);
}

void xunji(void)
{
    int position = readSensors();
    float error = target - position;

    // 检测是否在弯道
    bool inTurn = isInTurn(position);

    // 根据是否在弯道调整PID参数
    if (inTurn)
    {
        X_Kp = 900.0;
        X_Kd = 200.0;
    }
    else
    {
        X_Kp = 500.0;
        X_Kd = 0.0;
    }

    // 计算PID修正值
    float correction = computeIncrementalPID(error);
    // 设置速度，在弯道时降低速度
    if (inTurn)
    {
        // Set_Speed(0, 20 - correction); // 左轮
        // Set_Speed(1, 20 + correction); // 右轮
        Set_PWM(4900+correction,4900-correction);
    }
    else
    {
        // Set_Speed(0, 25 - correction); // 左轮
        // Set_Speed(1, 25 + correction); // 右轮
        Set_PWM(3000+correction,3000-correction);
    }
}

六、总结：

总的来说，这套题门槛低，但是能够全部完成下来的组确不见得有很多，有很多组在实验室调得很好，四问全跑完还能很快，但是到了比赛场地却各种问题不断，所以说运气有时候也是成功的一部分的，但是竞赛的魅力之一不就在于它的“不确定性”吗？我们组选择求稳，所以第四问的速度很慢，跑完大概八十秒左右，最终完赛。这次比赛我熬了好几天夜，每次都感觉要累倒了，我一步步建立框架最后完善代码，这使我很有成就感，很开心！希望大家都能获得不错的成绩！欢迎一起交流。最后再次感谢b站up主@Torris-Yin、@江协科技、@Alice_西风和@左-岚提供的无偿教学帮助！

下载链接：

通过百度网盘分享的文件：2024年电赛H题
链接：https://pan.baidu.com/s/1al7XURilPo9I9rWyHtVoSg
提取码：6715