这次电赛题目选的简单了,还规定不能使用到摄像头,这让我之前学习的Opencv 4与树莓派无用武之地了,但我当时对于三子棋题目饶有兴趣,但架不住队友想稳奖,只能选择这个H题了......
之后我还想抽空将这个E题三子棋题目做做,写篇博客实现一下......
这篇文就将一些过程中 遇到的问题 与 可能的解决法 讲一下.....
目录
MSPM0G3507库函数问题与修改:
端口状态读取函数问题:
串口接收函数存在强制类型转换:
汇编文件定义的数据处理栈过小:
直流电机干扰磁场:
题目思路:
PID角度惯导:
PID角度惯导视频:
第一题:
第一题视频:
第二题:
第二题视频:
第三题:
第三题视频:
主函数与状态安排:
网上查阅资料贴出:
MSPM0G3507库函数问题与修改:
端口状态读取函数问题:
在尝试通过读取引脚高低电平来获取八路寻迹的端口状态时,发现它的引脚读取函数设计有些奇特,返回值不是类似于STM32的那种0和1,
我们发现 DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_15) ,低电平就返回0,而高电平返回的不是1,而是引脚的编号......
串口接收函数存在强制类型转换:
在使用串口时发现 STATIC INLINE uint8 t DL UART receiveData(UART Regs *uart)
存在强制类型转换的现象:
这会将十六进制发送过来的数据强制转换为0~255的十进制数据,这是个多此一举的冗余步骤
随后去掉了这个强制类型转换:
汇编文件定义的数据处理栈过小:
在进行串口与数据接收时,我写了一个函数用于解算数据包以及计算角度,没想到在进行通信测试时,串口一直没有执行printf返回给上位机任何数据,后来才发现是在解算步骤卡住了:
以下是我解算JY901B陀螺仪 数据包定义的数据变量与函数,该函数在串口中断中调用:
数据变量:
最终结算好的数据存在了float Angle_R[3];中
#include <string.h> uint8_t RX_BUFFER[10]; typedef struct { float angle[3]; }Angle; struct SAngle//角度 { short Angle[3]; short T; }; struct SAngle stcAngle; float Angle_R[3]; void JY901_Process(uint8_t RxData);
函数:
void JY901_Process(uint8_t RxData) { static uint8_t state=0; static uint8_t bufsize=0; switch(state) { case 0: //搜索到帧头 if(RxData==0x55) { state=1;} else state=0; break; case 1: //搜索到角度帧 if(RxData==0x53) { state=2;} else state=0; //否则重新搜素 break; case 2: RX_BUFFER[bufsize]=RxData; bufsize++; if(bufsize==9) { state=3; bufsize=0; } break; case 3: memcpy(&stcAngle,&RX_BUFFER[0],8); for(uint8_t j = 0; j < 3; j++) { Angle_R[j] = (float)stcAngle.Angle[j]/32768*180; } state = 0; break; } }
随后为了解决这个问题,将汇编文件的栈大小进行了修改:
修改后:
直流电机干扰磁场:
中途发现陀螺仪存在左右飘逸的现象,起初怀疑是直流电机磁场对它有影响,就将直流电机的外壳接地了,结果发现影响更大了,就不在接地了.....
题目思路:
PID角度惯导:
这是将电机速度闭环之后,再对角度闭环的结果:可以使小车保持在一个角度:
//用于初始化pid参数的函数 void PID_Init(PID *pid, float p, float i, float d, float maxI, float maxOut) { pid->kp = p; pid->ki = i; pid->kd = d; pid->maxIntegral = maxI; pid->maxOutput = maxOut; } //进行一次pid计算 //参数为(pid结构体,目标值,反馈值),计算结果放在pid结构体的output成员中 void PID_Calc(PID *pid, float reference, float feedback) { //更新数据 pid->lastError = pid->error; //将旧error存起来 pid->error = reference - feedback; //计算新error //计算微分 float dout = (pid->error - pid->lastError) * pid->kd; //计算比例 float pout = pid->error * pid->kp; //计算积分 pid->integral += pid->error * pid->ki; //积分限幅 if(pid->integral > pid->maxIntegral) pid->integral = pid->maxIntegral; else if(pid->integral < -pid->maxIntegral) pid->integral = -pid->maxIntegral; //计算输出 pid->output = pout+dout + pid->integral; //输出限幅 if(pid->output > pid->maxOutput) pid->output = pid->maxOutput; else if(pid->output < -pid->maxOutput) pid->output = -pid->maxOutput; } //首先定义PID结构体用于存放一个PID的数据 typedef struct { float kp, ki, kd; //三个系数 float error, lastError; //误差、上次误差 float integral, maxIntegral; //积分、积分限幅 float output, maxOutput; //输出、输出限幅 }PID; PID mypid = {0}; //创建一个PID结构体变量 PID_Init(&mypid, 1.5, 0, 3, 80, 100); //初始化PID参数
角度PID计算惯导部分:
char imu2(float angle) { char flag = 0; float chazhi_angle; chazhi_angle = fabs(Angle_R[2] - angle); targetValue=angle; float feedbackValue = Angle_R[2]; //这里获取到被控对象的反馈值 PID_Calc(&mypid, targetValue, feedbackValue); //进行PID计算,结果在output成员变量设定执行器输出大小(mypid.output); SPEEED[0]=mypid.output; SPEEED[1]=mypid.output*(-1); SPEEED[2]=mypid.output; SPEEED[3]=mypid.output*(-1); I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 if(chazhi_angle<3) {return 1;} return 0; }
PID角度惯导视频:
PID角度惯导视频
第一题:
就将小车四个电机闭环,保持同速即可,使用状态机的思维去写的,写一个状态就测试一次小车的具体运行程序结果,看是否需要添加或修改其余传感器逻辑......
这里我有个空闲状态,一旦切换到空闲状态,小车就会对0度进行角度惯导......
//任务1
void TASK_1(void)
{
GET_FTT_PORT_status();
if(DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_16)==0)
{beep(1);led(1); delay_ms(10); beep(0);led(0);status1 =1;status3=99;}
//选择状态进入
switch(status1)
{
case 0:target_angle_F=1;target_angle=0;system_idie_state(); break; //系统空闲状态:
case 1: //无脑直线
//GET_FTT_PORT_status();
target_angle_F=0;
target_angle=0;
SPEEED[0]=45*(-1);
SPEEED[1]=45;
SPEEED[2]=45;
SPEEED[3]=45*(-1);
I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动
//任意检测到线
if(L1==1 || L2==1 || L3==1|| L4==1|| L5==1|| L6==1|| L7==1|| L8==1)
{
SPEEED[0]=0;
SPEEED[1]=0;
SPEEED[2]=0;
SPEEED[3]=0;
I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动
beep(1);led(1);
delay_ms(15);
beep(0);led(0);
status1=0;
}
break;
}
}
第一题视频:
2024电赛自动行驶小车(H题)_第一问
第二题:
这题的A->B部分就是先靠PID直线,然后巡线,然后想办法通过陀螺仪反馈使小车转到差不多180度的位置,陀螺仪因为读取方式不同,其实转到180度正还是不容易的......
然后我定的是-178度位置,为了防止它缓慢偏出线,我又增加了179的惯导在直行的逻辑中,(但这样简单粗暴的添加使得我的小车挺抖......)
//任务2 不许删动 //放置时 先正放再 向左边斜放 void TASK_2(void) { GET_FTT_PORT_status(); //获得巡线返回 if(DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_15)==0){ beep(1);led(1); delay_ms(10); beep(0);led(0); status2=1;status1=99;status3=99;} switch (status2) { case 0:system_idie_state(); break; case 1://A->B 无脑直线 target_angle_F=0; //imu2(0); //惯导直行 SPEEED[0]=45*(-1); SPEEED[1]=45; SPEEED[2]=45; SPEEED[3]=45*(-1); I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 //任意检测到线 if(L1==1 || L2==1 || L3==1|| L4==1|| L5==1|| L6==1|| L7==1|| L8==1) { beep(1);led(1); delay_ms(15); beep(0);led(0); status2=2;status=1; } break; case 2://B->C 巡线 FTT_judge_status();//判定巡线系统状态 target_angle_F=0; // //先清零系统预设方向 status_Direction=0; //八路寻迹判断系统状态,一般会传给status_Direction数值 FTT_judge_status(); //选择状态进入 switch(status) { case 0:system_idie_state(); break; //系统空闲状态: case 1:system_follow_straight();break; //系统直线寻迹状态(可带小弧度偏转) case 2:system_Turn_left(); break; //小车左转调整状态 case 3:system_Turn_right(); break; //小车右转调整状态 //case 4:system_stop(); break; //停车 } //已经到C了 if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 &&(fabs(Angle_R[2]+180)<=15)) { beep(1);led(1); delay_ms(15); beep(0);led(0); //imu2(-60); //偏正 //targetValue=-44; target_angle_F=0; while(imu2_my(-175)==0) status2=3;status=0; } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0) { //imu2(-120); //惯导偏正(向右) system_Turn_right(); } break; case 3://C->D 无脑执行 imu2_my(-177); //惯导直行 175 SPEEED[0]=45*(-1); SPEEED[1]=45; SPEEED[2]=45; SPEEED[3]=45*(-1); I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 //任意检测到线 if(L1==1 || L2==1 || L3==1|| L4==1|| L5==1|| L6==1|| L7==1|| L8==1) { beep(1);led(1); delay_ms(15); beep(0);led(0); status2=4;status=1; } break; case 4://D->A // //先清零系统预设方向 status_Direction=0; //八路寻迹判断系统状态,一般会传给status_Direction数值 FTT_judge_status(); //选择状态进入 switch(status) { case 0:system_idie_state(); break; //系统空闲状态: case 1:system_follow_straight();break; //系统直线寻迹状态(可带小弧度偏转) case 2:system_Turn_left(); break; //小车左转调整状态 case 3:system_Turn_right(); break; //小车右转调整状态 //case 4:system_stop(); break; //停车 } //已经到A了 if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 &&(fabs(Angle_R[2])<=22)) { SPEEED[0]=0; SPEEED[1]=0; SPEEED[2]=0; SPEEED[3]=0; I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 beep(1);led(1); delay_ms(15); beep(0);led(0); target_angle_F=0; target_angle=0; while(imu2_my(0)==0) //偏正 status=0; status2=9;status1=0; } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]>0) { system_Turn_right();//惯导偏正(向右) } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]<0) { //imu2(10); //惯导偏正(向右) system_Turn_right(); } break; } }
第二题视频:
2024电赛自动行驶小车(H题)_第二问
第三题:
第二题会做,这题其实也没问题...
//任务 3 void TASK_3(void) { if(DL_GPIO_readPins(GPIOA, DL_GPIO_PIN_14)==0) {beep(1);led(1); delay_ms(10); beep(0);led(0); status3 =1;status2=9;status1=9;} switch(status3) { case 0:system_idie_state(); break; case 1://A->C 惯导后直线行驶 target_angle=-33; //target_angle_F=0; while(imu2(-33)==0) status3=2; break; case 2: //imu2(-37); //targetValue=-10; target_angle=-33; SPEEED[0]=55*(-1); SPEEED[1]=55; SPEEED[2]=55; SPEEED[3]=55*(-1); I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 //任意检测到线 if(L1==1 || L2==1 || L3==1|| L4==1|| L5==1|| L6==1|| L7==1|| L8==1) { beep(1);led(1); delay_ms(15); beep(0);led(0); target_angle=0; status3=3;status=1;status2=99;status1=99; } break; case 3://C-> B // //先清零系统预设方向 status_Direction=0; //八路寻迹判断系统状态,一般会传给status_Direction数值 FTT_judge_status(); target_angle_F=0; //选择状态进入 switch(status) { case 0:system_idie_state(); break; //系统空闲状态: case 1:system_follow_straight();break; //系统直线寻迹状态(可带小弧度偏转) case 2:system_Turn_left(); break; //小车左转调整状态 case 3:system_Turn_right(); break; //小车右转调整状态 //case 4:system_stop(); break; //停车 } //已经到B了 if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 &&(fabs(Angle_R[2])>=156)) { beep(1);led(1); delay_ms(15); beep(0);led(0); //while(imu2(-148)==0) //偏正 status=0; status3=4;status2=99;status1=99; } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]>0) { system_Turn_right(); //惯导偏正(向右) } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]<0) { //imu2(10); //惯导偏正(向右) system_Turn_left(); } break; case 4://调整好角度 target_angle_F=0; while(imu2_my(-163)==0) //target_angle=-63; //while(imu2(-63)==0) status3=5; status2=99; status1=99; break; case 5:// B->D target_angle_F=0; imu2_my(-138); system_Turn_left(); SPEEED[0]=55*(-1); SPEEED[1]=55; SPEEED[2]=55; SPEEED[3]=55*(-1); I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 //任意检测到线 if(L1==1 || L2==1 || L3==1|| L4==1|| L5==1|| L6==1|| L7==1|| L8==1) { beep(1);led(1); delay_ms(15); beep(0);led(0); status3=6;status=1; } case 6 ://D->A // //先清零系统预设方向 status_Direction=0; target_angle_F=0; //八路寻迹判断系统状态,一般会传给status_Direction数值 FTT_judge_status(); //选择状态进入 switch(status) { case 0:system_idie_state(); break; //系统空闲状态: case 1:system_follow_straight();break; //系统直线寻迹状态(可带小弧度偏转) case 2:system_Turn_left(); break; //小车左转调整状态 case 3:system_Turn_right(); break; //小车右转调整状态 //case 4:system_stop(); break; //停车 } //已经到A了 if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 &&(fabs(Angle_R[2])<=22)) { SPEEED[0]=0; SPEEED[1]=0; SPEEED[2]=0; SPEEED[3]=0; I2C_Write_Len(MOTOR_FIXED_SPEED_ADDR,SPEEED,4); //控制电机转动 beep(1);led(1); delay_ms(15); beep(0);led(0); //target_angle_F=0; target_angle_F=1;target_angle=0; while(imu2(0)==0) //偏正 status=0; status3=7; status2=9; status1=0; } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]>0) { system_Turn_right();//惯导偏正(向右) } //有可能是巡线时遇到调整角度黑线在夹缝 else if(L1==0&&L2==0&&L3==0&&L4==0&&L5==0&&L6==0&&L7==0&&L8==0 && Angle_R[2]<0) { //imu2(10); //惯导偏正(向右) system_Turn_right(); } break; } }
第三题视频:
这里就直接放第四问的视频了...第四问就是将第三问重复执行4次......
2024电赛自动行驶小车(H题)_第四问
主函数与状态安排:
主函数中有些任务状态变量statusx的初始值并不是0,这是为了防止进入其余状态时执行它的状态0操作(小车惯导0度 )的行为,并且,进入其余状态时,也应合理安排这些状态变量,使其不要互相串搞.....
网上查阅资料贴出:
PID超详细教程——PID原理+串级PID+C代码+在线仿真调参-CSDN博客