718校内赛技术报告

此篇博客用于记录校内赛的技术细节和感悟（屎山预警！！！）

1.1 红外对管

排布见下图：

其中4 5作为循迹灯，其余全部用于路口判断。

1.1.1 循迹

利用左右循迹灯实现循迹，使用了PID算法，只使用了P和D，详细说明见2.1。

1.1.2 路口判断逻辑

利用1 2 3 0四个红外对管实现对路口的识别。

另外规定ADC的值>1900为完全踩上线，ADC的值小于lowx为完全没踩上线，ADC的值大于lowx+200（需要大些）为正在踩上线。

我们对路口的分类如下：

1.2 机械臂

构成：亚克力板，三个MG90S舵机，胶带，热熔胶，夹子，螺丝，螺母。

自由度：3

设计想法：为了便于能量块的夹取和定点投放

2. 软件

注：此部分主要内容及解释都在代码注释中

2.1 宏及全局变量

#include "stm32f10x.h"
#include "Delay.h"
#include "PWM.h"
#include "bsp_usart.h"
#include "AD.h"
#include "stm32f10x_it.h"
#include <stdio.h>

# define kp 0.90     

# define kd -80      

# define high1 2100
# define low1 1800
# define high2 2400
# define low2  2000
# define delaytime 200


// 转换通道个数
#define NOFCHANEL 12
#define FILTER_SIZE 5  

int16_t filtered_AD0[FILTER_SIZE] = {0};
int16_t filtered_AD1[FILTER_SIZE] = {0};
int16_t filtered_AD2[FILTER_SIZE] = {0};
int16_t filtered_AD3[FILTER_SIZE] = {0};
int16_t filtered_AD4[FILTER_SIZE] = {0};
int16_t filtered_AD5[FILTER_SIZE] = {0};
//用于储存ADC返回值

// 将 ADC1 转换的电压值通过 DMA 方式传到 SRAM
extern __IO uint16_t ADC_ConvertedValue[NOFCHANEL];

// 局部变量，用于保存转换计算后的电压值
float ADC_ConvertedValueLocal[NOFCHANEL];


int16_t et = 0;
int16_t det = 0;
int16_t temp = 0;
int16_t wucha =0;
//PID中变量，用于更新误差等数据

2.2 主函数及初始化

int main(void)
{   
    USART_Config();
    TIM1_CH4_PWM_Init();
    PWM_Init();
    AD_Init();
    wucha =AD_Value[4]-AD_Value[5];
    while(1)
    {
        if('7'==ucTemp)
        {
            while(1){
                pid();
                // 在特殊路口执行特殊操作
                if (isTIntersection()) {
                    handleTIntersection();
                } else if (isLeftTurn()) {
                    handleLeftTurn();
                } else if (isRightTurn()) {
                    handleRightTurn();
                } else if (isRoundabout()) {
                    handleRoundabout();
                }else if (isRound()) {
                    handleROUND();
                }
                if('6'==ucTemp	)
                {Motor_Run(0,0);
                 Motor_Run(1,0);
                 break;}
            }
        }

        if('5'==ucTemp)
        {while(1)
        {
            BlueTeeth();
            if('6'==ucTemp	)
            {break;}
        }
        }
        if('8'==ucTemp)
        {while(1)
        {
            pid();
            if('6'==ucTemp)
            {break;}
        }

        }


    }
}

2.3 PID

我们的小车只使用了P和D，因为我们实现闭环的不是加速度，所以不需要用积分项去逼近目标位置。P好理解，就是正比于误差的量；但P也不能太小，否则P太小的话转弯太慢，导致S弯绕不过去。D作为斜率可以通过抑制P的趋势来实现消抖的功能。

理论学习如下：

• 整体思路：ADC采样—>对ADC采样数据的数学处理—>对得到的数据进行PID算法运算—>赋值给ccr调整pwm波输出占空比驱动电机。循环此过程则可实现速度闭环实现直角弯道除外的赛道。
• 方案：两灯and三灯。但考虑到两灯操作更简单，对硬件兼容性更高。最终采用两灯方案。
• 理论理解：

尽可能的扩大两个参数的值域：ADC数学处理获得值、小车识别黑线范围。针对第一个参数，当此值变化范围很大时，可以获得更高的精度调控；针对第二个参数，当此值变化范围很大时，可以扩大小车的“视野”，即使已经偏线也可以回归正轨。

归根结底，重点在于数学处理。

• 实操理解：

@针对采样：

重要参数：单个采样范围及变化趋势，单个采样值。

单个采样范围及变化趋势：非常有限，几乎不触及黑线采样值变化很小，但一接近黑线立即成指数增长。

单个采样值：几百个体差异到约2300的原始值。但转化为电压值之后仅为 0~3.3，大量数据被压缩，损失了很多精度，最终决定采用原始值进行操作。

误差来源：环境红外线影响，红外对管个体差异，红外对管彼此距离，红外对管对地距离。

环境红外线影响：很大。非常影响红外对管采集数据。最终解决办法有两种，一是在每个红外对管四周缠上黑胶布，用以反射环境红外线并聚合红外对管发出的红外线；二是在代码中初始化过程中消除环境误差并不需要测得，但随着小车运动导致的位置改变，环境误差在变化，而我们只消除了开始的误差，我们选择使用动态消除误差。

红外对管个体差异：不同红外对管的峰值大致相同，但最低值各有千秋。有的一两百，有的却五六百。考虑到对ADC采样数据进行处理时，个体误差的存在会影响很多计算结果造成两个灯调节作用差别很大。故必须消除。最终我们采用个体采样值域大的红外对管，以输出更加灵敏的值，并在代码中在初始化过程中，连同环境误差一起消除。

红外对管彼此距离：主要影响两点，一是识别范围，二是数学处理时产生漏洞。对于第一点，两灯距离太小时，识别范围就太小，小车遇到稍抖一点的弯路时极易偏线，但两灯距离过大时，就相当于允许小车有一定程度的偏线，造成循迹不流畅。对于第二点，若两灯距离过小，此时可以忽略个体误差，但同时ADC数学处理获得值值域很小，调节精度或说范围很小，若两灯距离过大，两灯之间会留出一个处理值为0的空白区允许小车一定程度偏线，这是我们不希望看到的。

红外对管对地距离：主要影响红外对管的采样值域。对地距离过低时红外对管接收不到反射光，采样值很低，对地距离过高时很受环境光干扰，接收红外线很多，采样值很高，通过打印波形图来观察何对地距离红外对管的检测距离最大，以及采样值域最大。

@针对数学处理：

处理参数：识别范围，输出值。

识别范围：即小车的“视野”，能够允许偏线程度最大的情况下依然可以回归正轨。

输出值：作为偏差，进行PID运算后，改变ccr来改变电机占空比。

处理方法与消除误差：

缺少能够作为条件的判断，所以只有让两灯采样值相减，同时让两灯在同一环境下初始化，记录两灯差值，然后代入程序中来消除个体误差和环境误差，然后调用ADC数据处理函数。

@针对PID：

本次任务中采用位置式PID，且用到P与D参与运算。

采用PD系统调节原因：通过P进行主导线形控制，通过D来计算未来趋势抑制系统震荡，即通过PD系统获得更快的反应速度。符合循迹需求。

输入值：实时计算ADC数学处理获得值与目标值0的差值

输出值：除了初始占空比以外的用于控制电机的pwm

偏差：ADC数学处理获得值与目标值0的差值，用于P计算

二次偏差：前后两次偏差的差值，用于D计算

调参注意：P过小调节作用小，P过大造成过冲系统震荡。D过小调节作用小，D过大会放大系统趋势的影响，使系统震荡。

调参顺序：先设D=0，调P，从0逐渐增大，直到系统震荡。再调D使其逐渐增大，直到系统震荡。之后进行微调，直到系统稳定。

@针对电机控制：

调节方式：对两边电机附PID不同运算值，以达到使两侧不同性能电机具有相同效果的调节作用。

调节精度及限制：

考虑到可供调节的ccr范围为0~500( 有点小，占空比的相对调节精度小），但考虑到对循迹小车来讲500的调节范围够用，所以没改，但后来调车发现问题，发现有些曲率较大的弯转不过去，起初认为是P设的过小，但经过数学运算后发现问题是数值溢出，即能转过这个弯路的PID处理值没发挥出它的作用，因为溢出下限即占空比为0，溢出上限即占空比为100，所以最终采用【循迹电机反转函数】的使用，使溢出值得到充分利用。事实上增大ccr的可操作区间亦可。

初始值设置：一开始设置为50%占空比，因为考虑到想使PID调节具有对称性。事实效果很好，循迹很丝滑，但后来考虑到走直线速度较慢，故逐渐增大初始占空比，再具体进行调参。

详细解释见下方代码注释。

2.3.1 PID代码

2.3.1.1 PID基本原理

PID详解

• 比例增益（P）： 比例部分根据当前误差的大小调整输出。如果误差较大，比例增益会产生更大的输出变化，以更快地减小误差。然而，如果比例增益设置得太大，系统可能会变得不稳定。
• 积分时间（I）： 积分部分考虑了误差随时间的积累。它用于消除系统稳态误差，因为它会持续增加控制输出，直到误差为零。但如果积分时间设置得太大，可能导致系统的超调或振荡。
• 微分时间（D）： 微分部分考虑了误差变化的速度。它可以帮助系统抑制振荡，因为它对误差变化的速度进行响应，减小输出的变化速度。然而，如果微分时间设置得太大，可能会导致系统对噪声敏感。

PID算法基本原理

PID算法的执行流程是非常简单的，即利用反馈来检测偏差信号，并通过偏差信号来控制被控量。而控制器本身就是比例、积分、微分三个环节的加和。

根据上图我们考虑在某个特定的时刻t，此时输入量为rin(t)，输出量为rout(t)，于是偏差就可计算为err(t)=rin(t)-rout(t)。于是PID的基本控制规律就可以表示为如下公式：

其中Kp为比例带，TI为积分时间，TD为微分时间。

PID算法离散化

由于在计算机上应实现离散化问题，我们对比例，积分，微分特性做简单说明。

比例就是用来对系统的偏差进行反应，所以只要存在偏差，比例就会起作用。积分主要是用来消除静差，所谓静差就是指系统稳定后输入输出之间依然存在的差值，而积分就是通过偏差的累计来抵消系统的静差。而微分则是对偏差的变化趋势做出反应，根据偏差的变化趋势实现超前调节，提高反应速度。

在实现离散前，我们假设系统采样周期为T。假设我们检查第K个采样周期，很显然系统进行第K次采样。此时的偏差可以表示为err(K)=rin(K)-rout(K)，那么积分就可以表示为：err(K)+ err(K+1)+┈┈，而微分就可以表示为：(err(K)- err(K-1))/T。于是我们可以将第K次采样时，PID算法的离线形式表示为：

这就是所谓的PID算法离散描述公式。还有一个增量型PID算法，下面来推导一下。
上面的公式描述了第k哥采样周期的结果，那么前一时刻也就是k-1哥采样周期可表示为：

那么我们再来说第K个采样周期的增量，很显然就是U(k)-U(k-1)。于是我们用第k个采样周期公式减去第k-1个采样周期的公式，就得到了增量型PID算法的表示公式：

当然，增量型PID必须记得一点，就是在记住U(k)=U(k-1)+∆U(k)

PID控制器基本实现

完成了离散化后，我们就可以来实现它了。已经用离散化的数据公式表示出来后，再进型计算机编程已经不是问题了。接下来我们就使用C语言分别针对位置型公式和增量型公式来具体实现。

位置型PID简单实现

位置型PID的实现就是以前面的位置型公式为基础。这一节我们只是完成最简单的实现，也就是将前面的离散位置型PID公式的计算机语言化。
首先定义PID对象的结构体：

/*定义结构体和公用体*/

typedef struct

{

  float setpoint;       //设定值

  float proportiongain;     //比例系数

  float integralgain;      //积分系数

  float derivativegain;    //微分系数

  float lasterror;     //前一拍偏差

  float result; //输出值

  float integral;//积分值

}PID;

接下来实现PID控制器：

void PIDRegulation(PID *vPID, float processValue)

{

  float thisError;

  thisError=vPID->setpoint-processValue;

  vPID->integral+=thisError;

  vPID->result=vPID->proportiongain*thisError+vPID->integralgain*vPID->integral+vPID->derivativegain*(thisError-vPID->lasterror);

  vPID->lasterror=thisError;

}

这就实现了一个最简单的位置型PID控制器，当然没有考虑任何干扰条件，仅仅只是对数学公式的计算机语言化。

增量型PID简单实现

增量型PID的实现就是以前面的增量型公式为基础。这一节我们只是完成最简单的实现，也就是将前面的离散增量型PID公式的计算机语言化。

首先定义PID对象的结构体：

/*定义结构体和公用体*/

typedef struct

{

  float setpoint;       //设定值

  float proportiongain;     //比例系数

  float integralgain;      //积分系数

  float derivativegain;    //微分系数

  float lasterror;     //前一拍偏差

  float preerror;     //前两拍偏差

  float deadband;     //死区

  float result; //输出值

}PID;

位置型PID控制器的基本特点：

• 位置型PID控制的输出与整个过去的状态有关，用到了偏差的累加值，容易产生累积偏差。
• 位置型PID适用于执行机构不带积分部件的对象。
• 位置型的输出直接对应对象的输出，对系统的影响比较大。

增量型PID控制器的基本特点：

• 增量型PID算法不需要做累加，控制量增量的确定仅与最近几次偏差值有关，计算偏差的影响较小。
• 增量型PID算法得出的是控制量的增量，对系统的影响相对较小。
• 采用增量型PID算法易于实现手动到自动的无扰动切换。

2.3.1.2 最终代码实现

代码如下：

void pid(void) {
    // 获取滤波后的值
    int16_t filtered_AD4_value = getFilteredValue(filtered_AD4);
    int16_t filtered_AD5_value = getFilteredValue(filtered_AD5);

    et = filtered_AD4_value - filtered_AD5_value - wucha;
    det = et - temp;

    if (((kp) * et < 200) && ((kp) * et > -200)) {
        MotorControl(FRONT, 1000 + (kp)/2 * et - kd * det);
    }  else if (kp * et <= -50) {  
        MotorControl(LEFT, -100 - kp * et + kd * det);  
        MotorControl(FRONT, 1000);
    }else if (kp* et >= 50) {
        MotorControl(FRONT, 1000);
        MotorControl(RIGHT, kp * et - 100 - kd * det);
    }
    temp = et;//pid输出值暂存

    updateFilter(filtered_AD4, AD_Value[4]);
    updateFilter(filtered_AD5, AD_Value[5]);
    //更新滤波输入，减小误差
}

2.4 路口识别

在本节中，详细讲讲判断环岛及真假环岛的逻辑（即连续两次左/右T型路口，左手定则为右T型，右手定则为左T型），其余路口都较为简单，不多赘述。

以左手定则走右环岛为例，首先不论环岛的真假，先判断连续两个右T型路口。我们设置了标志位flag（详见2.2主函数），在每次识别到非右T型路口时将其置零；在判断到右T型路口时将其取反，然后在下面写个若flag为0就进环岛函数的if，这样就实现了判断连续两个右T型路口。（详见2.4.1，2.4.3）

接着就是环岛的走法及判断环岛的真假。

(1) 环岛走法的大致逻辑就是，首先在第二个右T型路口右拐进环岛，然后识别环岛内第一个路口为右拐（第一个while，右拐后跳出），然后识别第二个路口为右拐（第二个while，右拐后跳出），然后识别第三个路口为T字型路口（第三个while，右拐后跳出），并return0，出函数再把flag置0，防止其一直走环岛。

(2) 判断环岛真假的大致逻辑是，在上述依次识别三个路口过程中，只要有一个识别到的路口与我们设定的路口不一致，就立马结束整个环岛函数并return相应的值（比如第一个路口不一致了就返回1，第二个不一致就返回2，第三个不一致就返回3），然后在循迹函数里做相应的处理。这里的处理也好理解，先让小车转180度，然后用switch case语句，从case3开始往case1写，内容便是按原路返回，然后在最初始的路口右拐。

比较有趣的是，我们发现在左手定则的大前提下，如果在同一个路口连续使用偶数次右手定则，则完全不会破坏左手定则的逻辑，换句话说，同一路口的偶数次右手定则等价于1次左手定则。

当然，从假环岛出来后还得把flag置1，这样又能解决走右T型+右环岛时进不了环岛的问题。

其代码如下：

//判断是否为断头路
int isRound(void) {
    return (AD_Value[7] < 2000	);
}

//处理断头路的函数
void handleROUND(void) {
	  performAction(BACK_ACTION, 300);
    performAction(LEFT_ACTION, 1300);
	 performAction(LEFT_ACTION, 900);
}


// 判断是否为 T 字灯特殊路口
int isTIntersection(void) {
    return (AD_Value[2] > high1 && AD_Value[0] > high1 && AD_Value[1] < low1);
}

// 处理 T 字灯特殊路口的函数
void handleTIntersection(void) {
    
	  performAction(STOP_ACTION, 500);
	  //performAction(BACK_ACTION, 300);
    performAction(FRONT_ACTION, 500);
}

// 判断是否为左拐路口
int isLeftTurn(void) { 
	return (AD_Value[2] > high2 && AD_Value[0] < low2 );
}

// 判断是否为右拐路口
int isRightTurn(void) {
    return (AD_Value[2] < low1 && AD_Value[0] > high1);
}

// 处理左拐路口的函数
void handleLeftTurn(void) {
  performAction(STOP_ACTION, 500);  
	//performAction(BACK_ACTION, 300);
    performAction(LEFT_ACTION, 1400);
	 performAction(FRONT_ACTION, 500);
    pid();
}

// 处理右拐路口的函数
void handleRightTurn(void) {
    performAction(STOP_ACTION, 500);
	  //performAction(BACK_ACTION, 300);
    performAction(RIGHT_ACTION, 1500);
	  performAction(FRONT_ACTION, 500);
     pid();
}

// 判断是否为环岛
int isRoundabout(void) {
    return (AD_Value[2] > high1 && AD_Value[0] > high1 && AD_Value[0] > high1 && AD_Value[1] > high1 && AD_Value[4] > high1 && AD_Value[5] > high1);
}

// 处理进入环岛的函数
void handleRoundabout(void) {
    performAction(STOP_ACTION, 500);
    performAction(FRONT_ACTION, 500);
}

void  CROSS_JUDG(void)
{
		int a=0;
    if(AD_Value[1]>2000&&AD_Value[2]>2000)
     {a=1;}
		else if(AD_Value[1]>2000&&AD_Value[0]>2000)
			{a=2;}
    else if(AD_Value[1]>2000&&AD_Value[0]>2000&&AD_Value[2]>2000)
			{a=3;}
		switch(a)
			{ 
			case 0:
				      break;
			case 1:
				     MoveDirection(LEFT);
			       Delay_s(1);
			       break;
			case 2:
				     MoveDirection(RIGHT);
			       Delay_s(1);
			       break;
			case 3:
				     MoveDirection(RIGHT);
			       Delay_s(1);
			       break;
			}
}

2.5 蓝牙

蓝牙这块比较简单，只是一个简单的通信协议，其中给i赋的值都是ASCII码，然后在手机的按键里设置相应的字符即可。

void BlueTeeth(){
    if ('1' == ucTemp)//前进
    {
        MoveDirection(FRONT);
        //	ucTemp = '0';
        while (1)
        {
            if ('0' == ucTemp)//松开手机上的按键就让其停下
            {
                MoveDirection(STOP);
                ucTemp = '0';
                break;
            }
        }
    }
    if ('2' == ucTemp)//后退
    {
        MoveDirection(BACK);
        //ucTemp = '0';
        while (1)
        {
            if ('0' == ucTemp)//松开手机上的按键就让其停下
            {
                MoveDirection(STOP);
                ucTemp = '0';
                break;
            }
        }
    }
    if ('3' == ucTemp)//左转
    {
        MoveDirection(LEFT);
        //ucTemp = '0';
        while (1)
        {
            if ('0' == ucTemp)//松开手机上的按键就让其停下
            {
                MoveDirection(STOP);
                ucTemp = '0';
                break;
            }
        }
    }
    if ('4' == ucTemp)//右转
    {
        MoveDirection(RIGHT);
        //ucTemp = '0';
        while (1)
        {
            if ('0' == ucTemp)//松开手机上的按键就让其停下
            {
                MoveDirection(STOP);
                ucTemp = '0';
                break;
            }
        }
    }

}

2.6 电机控制

// 控制电机运行
void MotorControl(Direction direction, int speed) {
    switch (direction) {
        case FRONT:
            left_up();
            right_up();
            Motor_Run(0,speed);
            Motor_Run(1,speed);
            break;
        case BACK:
            left_down();
            right_down();
            Motor_Run(0,speed);
            Motor_Run(1,speed);
            break;
        case LEFT:
            left_down();
            right_up();
            Motor_Run(0,speed);
            Motor_Run(1,speed);
            break;
        case RIGHT:
            left_up();
            right_down();
            Motor_Run(0,speed);
            Motor_Run(1,speed);
            break;
        case STOP:
            left_up();
            right_up();
            Motor_Run(0,0);
            Motor_Run(1,0);
            break;
    }
}
//控制轮子正反转
void performAction(ActionType action, int duration) {
    switch (action) {
        case STOP_ACTION:
            MotorControl(STOP, 0);
            break;
        case FRONT_ACTION:
            MotorControl(FRONT, 1000);
            break;
        case BACK_ACTION:
            MotorControl(BACK, 1000);
            break;
        case LEFT_ACTION:
            MotorControl(LEFT, 1300);
            break;
        case RIGHT_ACTION:
            MotorControl(RIGHT, 1300);
            break;
    }

    Delay_ms(duration);
}
//控制轮子旋转90°
void MoveDirection(Direction direction) {
    switch (direction) {
        case FRONT:
            MotorControl(FRONT, 1000);
            break;
        case BACK:
            MotorControl(BACK, 1000);
            break;
        case LEFT:
            MotorControl(LEFT, 1000);
            break;
        case RIGHT:
            MotorControl(RIGHT, 1000);
            break;
        case STOP:
            MotorControl(STOP, 0);
            break;
    }
}
轮子的直接控制

这一部分的代码主要是电机四个轮子的直接控制封装，由于左右两侧的轮子分别可以看作统一步调，故可以调整电机参数后统一输出PWM，达到同步效果。（底层封装略）

2.7机械臂

#include <Servo.h>
#include "NeoSWSerial.h"
NeoSWSerial mySerial(12, 13);

#define SERVOS      (4)     // 机械臂需要的舵机个数

Servo arm_servos[SERVOS];   // 声明SERVOS个舵机
int servo_pins[SERVOS];         // 舵机要接入的主板IO口
int servo_cur_angle[SERVOS];    // 舵机当前的旋转角度
int servo_min_angle[SERVOS];    // 允许舵机旋转到达的最小值
int servo_max_angle[SERVOS];    // 允许舵机旋转到达的最大值
int servo_init_angle[SERVOS];   // 刚上电时，舵机的初始角度
int joystick_value[SERVOS];

#define JOYSTICK_MIN_THRESH     (300)   // 摇杆ADC值的最小阈值，用于判断摇杆向哪边推动了
#define JOYSTICK_MAX_THRESH     (700)   // 摇杆ADC值的最大阈值，用于判断摇杆向哪边推动了
#define CLAW_OPEN_ANGLE     (45)    // 爪子张开时的舵机角度
#define CLAW_CLOSE_ANGLE    (5)    // 爪子闭合时的舵机角度
#define CLAW_SERVO_INDEX    (SERVOS-1)  // 爪子舵机的序号，倒数第一个
#define LEARN_MAX_ACTIONS   (100)   // 学习模式最多可记录的动作个数

bool learning_mode = false;  // 是否进入学习模式？
bool repeat_mode = false;  // 是否重复刚才学习到的动作？(配合学习模式使用)

int learn_actions[LEARN_MAX_ACTIONS][SERVOS];   // 学习模式存储记录动作
int learn_action_count = 0;                     // 学习模式记录了多少动作？


#define MAXSPEED    (10)    // 舵机最大旋转速度
int demo_speed = 1;         // 1倍速、2倍速......


#define JOYSTICK_LEFT_BUTTON    (2)     // 左键用于学习模式
#define JOYSTICK_RIGHT_BUTTON   (4)     // 右键用于播放预设动作
#define JOYSTICK_LED            (3)     // 摇杆模块上面的LED灯，接入主板3号脚

bool has_btn_been_pressed = false;          // 记录上次的按键状态



void setup() {
  Serial.begin(115200);

  pinMode(JOYSTICK_LEFT_BUTTON, INPUT_PULLUP);      // 左键用于学习模式
  pinMode(JOYSTICK_RIGHT_BUTTON, INPUT_PULLUP);     // 右键用于播放预设动作

  pinMode(JOYSTICK_LED, OUTPUT);
  digitalWrite(JOYSTICK_LED, HIGH); // 亮灯

  Serial.println("System Running...");
  Serial.print(digitalRead(JOYSTICK_LEFT_BUTTON));      //读取并串口打印按键状态
  Serial.print(", ");
  Serial.println(digitalRead(JOYSTICK_RIGHT_BUTTON));   //读取并串口打印按键状态

  // Bluetooth Module UART default baudrate is 9600
  mySerial.begin(9600);
  delay(200);
  test_bluetooth_module();

  // 初始化工作
  initialization();

  if (0 == digitalRead(JOYSTICK_LEFT_BUTTON))
  {
    // 左键用于学习模式
    learning_mode = true;
    has_btn_been_pressed = true;  // 记录有按键按下了！
    learn_action_count = 0;
    delay(1000);
    digitalWrite(JOYSTICK_LED, LOW);    // 灭灯

    Serial.println("Enter learning_mode!!");    // 进入学习模式
  }
  else if (0 == digitalRead(JOYSTICK_RIGHT_BUTTON))
  {
    // 右键用于播放预设动作
    play_demo_mode = true;
    has_btn_been_pressed = true;  // 记录有按键按下了！
    Serial.println("Enter play_demo_mode!!");

    // 开始播放预设动作序列
    play_demo();
  }
  else
  {
    // 炫耀武力，爪子抓2次
    cut_cut();
    Serial.println("Enter Bluetooth/JoyStick Control Mode!!");    // 进入摇杆操控模式
  }
}

void loop() {
  // 手机APP 操控机械臂工作
  move_by_bluetooth();

  // 摇杆模块操控机械臂工作
  move_by_joystick_contrl();

  // 学习模式，需要和摇杆操控互相配合
  learning_actions();
}

void test_bluetooth_module()
{
  Serial.print("\nTest Bluetooth Module: start\n");
  char cmd[16] = "AT+NAME\r\n\r\n";
  int len = strlen(cmd);
  Serial.println(cmd);
  Serial.println(len);
  for (int i = 0; i < len + 1; i++)
  {
    mySerial.write(cmd[i]);
  }
  delay(200);
  for (int i = 0; i < 16; i++)
  {
    if (mySerial.available())
    {
      Serial.print(mySerial.read(), 0);
    }
  }
  Serial.print("\nTest Bluetooth Module: end\n");
}

void initialization() {
  servo_pins[0] = 11;           // pin 11 -- Servo base   底座舵机
  servo_min_angle[0] = 0;       // 允许舵机旋转到达的最小值
  servo_max_angle[0] = 180;     // 允许舵机旋转到达的最大值
  servo_init_angle[0] = 90;     // 刚上电时，舵机的初始角度

  servo_pins[1] = 10;           // 10 : Servo left   左臂舵机
  servo_min_angle[1] = 10;      // 允许舵机旋转到达的最小值
  servo_max_angle[1] = 140;     // 允许舵机旋转到达的最大值
  servo_init_angle[1] = 90;     // 刚上电时，舵机的初始角度

  servo_pins[2] = 9;            //  9 : Servo right  右臂舵机
  servo_min_angle[2] = 40;      // 允许舵机旋转到达的最小值
  servo_max_angle[2] = 170;     // 允许舵机旋转到达的最大值
  servo_init_angle[2] = 90;     // 刚上电时，舵机的初始角度

  servo_pins[3] = 5;                        //  5 : Servo claw   爪子舵机
  servo_min_angle[3] = CLAW_CLOSE_ANGLE;    // 允许舵机旋转到达的最小值
  servo_max_angle[3] = CLAW_OPEN_ANGLE;     // 允许舵机旋转到达的最大值
  servo_init_angle[3] = CLAW_OPEN_ANGLE;   // 刚上电时，舵机的初始角度

  // 学习模式清空所有动作
  for (int i = 0 ; i < LEARN_MAX_ACTIONS; i++) {
    for (int j = 0 ; j < SERVOS; j++) {
      learn_actions[i][j] = 0;
    }
  }

  // 给所有舵机设置初始角度
  init_servos();
}


void init_servos() {
  for (int i = 0; i < SERVOS; i++)
  {
    arm_servos[i].attach(servo_pins[i], 500, 2500);      // 把舵机关联到对应的PWM引脚上
    arm_servos[i].write(servo_init_angle[i]); // 写入舵机的初始角度
    joystick_value[i] = 0;                    // 摇杆ADC值初始化为0
  }
}


// 炫耀武力，爪子抓2次
void cut_cut() {
  delay(1000);
  for (int i = 0; i < 2; i++)
  {
    close_claw(true);
    delay(150);
    close_claw(false);
    delay(150);
  }
}

// 控制爪子开闭
void close_claw(bool close)
{
  if (close) {
    arm_servos[CLAW_SERVO_INDEX].write(CLAW_CLOSE_ANGLE);
  } else {
    arm_servos[CLAW_SERVO_INDEX].write(CLAW_OPEN_ANGLE);
  }
}

void move_by_joystick_contrl()
{
  bool joy_changed = false;

  for (int i = 0; i < SERVOS; i++)
  {
    // 读取摇杆ADC值
    // A0 --> joystick_value[0] --> 左摇杆左右推，控制底座舵机旋转；
    // A1 --> joystick_value[1] --> 左摇杆前后推，控制左臂舵机旋转；
    // A2 --> joystick_value[2] --> 右摇杆前后推，控制右臂舵机旋转；
    // A3 --> joystick_value[3] --> 右摇杆左右推，控制爪子舵机旋转；
    joystick_value[i] = analogRead(i);

#if 1
    // 串口打印摇杆ADC值
    Serial.print("A[");
    Serial.print(i);
    Serial.print("]=");
    Serial.print(joystick_value[i]);
    Serial.print(", ");
#endif

    // 读取舵机当前的旋转角度
    servo_cur_angle[i] = arm_servos[i].read();

    if (joystick_value[i] > JOYSTICK_MAX_THRESH)    // 摇杆ADC值超过最大阈值
    {
      joy_changed = true;    // 摇杆被推动过了！

      if (servo_cur_angle[i] > servo_min_angle[i])
      {
        // 如果舵机当前角度，大于，允许舵机旋转到达的最小值，则舵机角度降低1度
        --servo_cur_angle[i];
      }

      if (i == CLAW_SERVO_INDEX)
      {
        // 如果是爪子舵机，则直接闭合爪子
        servo_cur_angle[i] = CLAW_OPEN_ANGLE;
      }
    }
    else if (joystick_value[i] < JOYSTICK_MIN_THRESH)   // 摇杆ADC值小于最小阈值
    {
      joy_changed = true;   // 摇杆被推动过了！

      if (servo_cur_angle[i] < servo_max_angle[i])
      {
        // 如果舵机当前角度，小于，允许舵机旋转到达的最大值，则舵机角度增加1度
        ++servo_cur_angle[i];
      }

      if (i == CLAW_SERVO_INDEX)
      {
        // 如果是爪子舵机，则直接打开爪子
        servo_cur_angle[i] = CLAW_CLOSE_ANGLE;
      }
    }
  }
  Serial.println("");

  if (true == joy_changed)
  {
    // 只要摇杆被推动过了，就刷新一遍舵机角度: 将当前最新的舵机角度值，写入舵机
    for (int i = 0 ; i < SERVOS; i++)
    {
#if 1
      // 串口打印舵机角度
      Serial.print("servo[");
      Serial.print(i);
      Serial.print("]=");
      Serial.print(servo_cur_angle[i]);
      Serial.print(", ");
#endif
      arm_servos[i].write(servo_cur_angle[i]);
    }
    Serial.println("");
  }

  delay(20);
}

void move_by_bluetooth() {
  bool joy_changed = false;

  if (mySerial.available())
  {
    char inByte = mySerial.read();
    Serial.print(inByte);
    if ((inByte != (char)0xFF) && (inByte != (char)0x0A) && (inByte != (char)0x0D))
    {
      joy_changed = true;
      switch (inByte)
      {
        case 'a':
          {
            if (servo_cur_angle[0] < servo_max_angle[0])
            {
              // 如果舵机当前角度，小于，允许舵机旋转到达的最大值，则舵机角度增加1度
              ++servo_cur_angle[0];
            }
            break;
          }
        case 'b':
          {
            if (servo_cur_angle[0] > servo_min_angle[0])
            {
              // 如果舵机当前角度，大于，允许舵机旋转到达的最小值，则舵机角度降低1度
              --servo_cur_angle[0];
            }
            break;
          }
        case 'c':
          {
            if (servo_cur_angle[1] < servo_max_angle[1])
            {
              // 如果舵机当前角度，小于，允许舵机旋转到达的最大值，则舵机角度增加1度
              ++servo_cur_angle[1];
            }
            break;
          }
        case 'd':
          {
            if (servo_cur_angle[1] > servo_min_angle[1])
            {
              // 如果舵机当前角度，大于，允许舵机旋转到达的最小值，则舵机角度降低1度
              --servo_cur_angle[1];
            }
            break;
          }
        case 'e':
          {
            // 爪子舵机，直接闭合爪子
            servo_cur_angle[3] = CLAW_CLOSE_ANGLE;
            break;
          }
        case 'f':
          {            
            // 爪子舵机，直接打开爪子
            servo_cur_angle[3] = CLAW_OPEN_ANGLE;
            break;
          }
        case 'h':
          {
            if (servo_cur_angle[2] > servo_min_angle[2])
            {
              // 如果舵机当前角度，大于，允许舵机旋转到达的最小值，则舵机角度降低1度
              --servo_cur_angle[2];
            }
            break;
          }
        case 'g':
          {
            if (servo_cur_angle[2] < servo_max_angle[2])
            {
              // 如果舵机当前角度，小于，允许舵机旋转到达的最大值，则舵机角度增加1度
              ++servo_cur_angle[2];
            }
            break;
          }
        default:
          {
            joy_changed = false;
            break;
          }
      }
    }
    Serial.println("");
  }

  if (true == joy_changed)
  {
    // 只要手机APP摇杆被推动过了，就刷新一遍舵机角度: 将当前最新的舵机角度值，写入舵机
    for (int i = 0 ; i < SERVOS; i++)
    {
#if 1
      // 串口打印舵机角度
      Serial.print("servo[");
      Serial.print(i);
      Serial.print("]=");
      Serial.print(servo_cur_angle[i]);
      Serial.print(", ");
#endif
      arm_servos[i].write(servo_cur_angle[i]);
    }
    Serial.println("");
  }
}

/*
    move_from_to:
    目的是，让机械臂的2个动作之间的变化、运动、转移，更加平滑，
    不要让舵机瞬间完成角度切换，一方面增加舵机使用寿命，一方面机械臂工作更加自然。
*/
void move_from_to(int *action_from, int *action_to)
{
  int max_angle = 0;
  int max_steps = 0;
  float step_angle[SERVOS];

  // 调整机械臂的运动、转移速度
  adjust_speed();

  /*
    1. 找到角度变化最大的那个舵机，算出角度变化绝对值max_angle；
    2. 角度变化值max_angle ÷ 舵机旋转速度demo_speed，就是舵机要运行的步数max_steps；
    3. 根据总步数max_steps，计算出每个舵机单步转动的角度step_angle；
  */
  max_angle = max(max(abs(action_to[0] - action_from[0]), abs(action_to[1] - action_from[1])), abs(action_to[2] - action_from[2]));
  max_steps = max_angle / demo_speed;
  max_steps = max_steps < 1 ? 1 : max_steps;
  for (int i = 0; i < CLAW_SERVO_INDEX; i++)
  {
    step_angle[i] = float(action_to[i] - action_from[i]) / float(max_steps);
  }

  for (int j = 1; j <= max_steps; j++) // 步数j累加
  {
    for (int i = 0; i < CLAW_SERVO_INDEX; i++)
    {
      // 随着j慢慢增大，new_angle 会慢慢趋近于 action_to[i]，也就实现了运动平滑的效果
      int new_angle = action_from[i] + j * step_angle[i];

      if ((new_angle < servo_min_angle[i]) || (new_angle > servo_max_angle[i]))
      {
        // 舵机角度值超出范围，忽略
        continue;
      }

      arm_servos[i].write(new_angle);  // 将最新的角度值，写入舵机
    }
    delay(20);
  }

  // 爪子舵机的开闭动作，不需要平滑过渡，直接设置角度给舵机
  arm_servos[CLAW_SERVO_INDEX].write(action_to[CLAW_SERVO_INDEX]);
  delay(20);

}

// 播放预设动作序列
void play_demo()
{
  // 计算总共有多少个动作？
  int counts = sizeof(demo_actions) / (SERVOS * sizeof(int));

  Serial.print("demo_actions counts = ");
  Serial.println(counts);

  move_from_to(servo_init_angle, demo_actions[0]);

  while (1)
  {
    for (int i = 0; i < counts - 1; i++)
    {
      // 摇杆左右键，任何按键按下，结束播放预设动作序列
      if (is_btn_pressed())
      {
        learning_mode = false;
        repeat_mode = false;
        play_demo_mode = false;
        return;
      }

      // 机械臂从当前动作，运动、转移至下一个动作
      move_from_to(demo_actions[i], demo_actions[i + 1]);
    }
  }
}


// 调整机械臂的运动、转移速度
void adjust_speed()
{
  // 任何一个摇杆推动任何一个方向，都可以调整速度
  for (int i = 0; i < SERVOS; i++)
  {
    if (analogRead(i) > JOYSTICK_MAX_THRESH) demo_speed++; // 速度加

    if (analogRead(i) < JOYSTICK_MIN_THRESH) demo_speed--; // 速度减
  }
  demo_speed = demo_speed < 1 ? 1 : demo_speed;
  demo_speed = demo_speed > MAXSPEED ? MAXSPEED : demo_speed;
}

bool is_btn_pressed()
{

  if ((false == has_btn_been_pressed) && ((0 == digitalRead(JOYSTICK_RIGHT_BUTTON)) || (0 == digitalRead(JOYSTICK_LEFT_BUTTON))))
  {
    // 如果上次没有按键按下，而现在左键或者右键按下了，则认为现在确实有按键按下了
    has_btn_been_pressed = true;
    return true;
  }
  else if ((1 == digitalRead(JOYSTICK_RIGHT_BUTTON)) && (1 == digitalRead(JOYSTICK_LEFT_BUTTON)))
  {
    // 如果左右键都没有按下，则认为确实没有按键按下
    has_btn_been_pressed = false;
  }
  return false;
}

机械臂使用的是arduino生态，主要使用了arduinoUNO这个板子，同时也加装了蓝牙模块，代码封装函数和stm32略有不同。

2.8滤波器

void updateFilter(int16_t* buffer, int16_t newValue) {
    for (int i = FILTER_SIZE - 1; i > 0; i--) {
        buffer[i] = buffer[i - 1];
    }
    buffer[0] = newValue;
}

int16_t getFilteredValue(int16_t* buffer) {
    // 计算缓冲区内所有值的平均值
    int32_t sum = 0;
    for (int i = 0; i < FILTER_SIZE; i++) {
        sum += buffer[i];
    }
    return sum / FILTER_SIZE;
}

加入滤波器相关程序的考虑如下：

1.减少控制器的灵敏度： 滤波器可以减少PID控制器对高频噪声和干扰的敏感度。这有助于防止PID控制器对系统中不必要的快速变化作出响应，从而提高系统的稳定性。

2.滤波测量信号： 在PID控制中，测量信号可能受到噪声的影响。通过对测量信号应用滤波器，可以改善系统对实际状态的感知，提高控制性能。

3.避免控制器振荡： 滤波器有助于减小系统中可能引起控制器振荡的高频成分。这有助于防止控制器在某些条件下产生不稳定的振荡。

后来经过测试证明，我们的设计是有很大效果的，极大程度上降低了红外对管物种多样性的阴间影响

3. 总结及反思

3.1 软件方面

• 由于缺乏经验和想法，在最开始的直接控制上浪费了很多时间（and金钱），当代码完成后，可以发现很多封装其实是没有必要的，而且会降低循迹的反应速度和稳定性。
• 在封装控制函数的过程中出现了“各自为政的现象”，造成了大量的冗余。直到后来才发现有些参数几乎是可以复用的，完全可以通过定义少数的宏来节省大量空间，并增强代码的可读性，提高运行效率。
• 我们在最终的代码里尝试定义了一个类似于摩擦系数的宏，所有的参数可以根据这个宏计算来调节，理论上只要知道比赛场地的路况等信息就可以一键调节所有的速度、角度等参数，从而节省了大量的调参时间。但是在最终的结果中我们发现这种定义方式并不足以满足各种复杂情况，主要因素如下：

1.各个电机和红外对管的性能不同，统一调控会导致系统整体的兼容性和稳定性下降。

2.路况复杂的情况下，盲目地追求减少调参时间是不可取的。

因此，虽然宏的使用是一个很好的尝试，但在实践中，我们也意识到需要更为灵活和细致的参数调整，以适应不同的实际情况。这一过程是一个不断优化和润色代码的过程，通过反复实验和调整，逐步找到最优的参数配置。这也是在机器人控制领域中常见的挑战之一，需要不断学习和改进。

3.2 硬件方面

• 由于对新生板的使用不够熟悉，我们在参赛前最后一天先后损坏了数块板子的电源等模块，为正式比赛优化代码造成了很恶劣的影响。
• 由于对比赛规则的不熟悉，在选购机械臂时出现了严重失误，机械臂的抬升高度不足以支持能量块的投放，故在正式比赛时我们放弃了给机械臂通电，避免电能的无效浪费。
• 面对IO口稀少的情况，我们使用的焊接杜邦线的骚操作，我们承认这一行为给我们减少了很多麻烦，但是电流的减弱也曾一度让我们的电机面临驱动力不足等问题，好在可以通过更改PWM占空比缓解这一问题。

3.3 工程经验方面

• 工程经验的缺少使我们分工不够明确，效率不够高效。好在大家目标一致，最终较为圆满地完成了部分任务，离不开所有队员的倾力合作和集思广益。

4.感悟

• 肝了一个月，这是大上学以来第一次独立做出可使用的工程（虽然垃圾得一批），学到了很多以前自认为要大三甚至大四才能碰到的知识，像L298电驱，PWM波占空比的调节，PID控制算法，各种路口判断的设计，在现场实地修（乱）改参数等，碰到了各种各样逆天且玄学的问题，such as芯片的神奇短路，电源模块莫名暴毙，ADC接口的梦幻联动，（至今工位上还摆放着四块待救治的板子）。——特此衷心感谢硬件陈学长的力挽狂澜

• 与此同时明显感觉到C语言的能力得到了大幅度提升（尤其时造轮子的能力仿佛登峰造极），还练就了一身从屎里找吃的硬能力。

  本篇博客就当成一个里程碑吧，纪念这疯狂熬夜的一个月（以及未来将要熬的无数个月），同时感谢718的各位学长（不）厌其烦的讲解和帮助。

外链：https://www.yuque.com/quanzhi-ndfvt/azx1dc/udzvfrcvol51mwr4?singleDoc# 《2023校内赛技术报告》