工赛物流搬运车制作-记录
记录我这次参加第七届工赛的小车制作过程，本次制作得比较low，下次制作回头看看这次思路并加强加固。
1. 设计思路
本次设计小车的运动，用到了 陀螺仪保持车身+ 四路激光测距粗劣定位 + 灰度精确定位 详情请看b站国一的那位大佬；我也是借鉴；
2. 材料
F103Vet6 二维码模块 openmv 麦克纳姆轮 编码电机 四个灰度 四路激光2m测距 mpu6050 四个舵机
3. 先让轮子动起来
因为用的是编码电机，所以先了解什么编码电机的运作。本人能力有限所借鉴了下面这位大佬的文章进行修改成四个电机的PID控制速度。
电机的pwm：pwm
电机的测速：测速
电机的pid控制：PID
如果需要请自行到他的公众号下载
下面是我修改的代码
Get_Motor_Speed(&SpeedNowA,&SpeedNowB,&SpeedNowC,&SpeedNowD); //计算电机速度
pid_Task_A.speedNow = SpeedNowA;
pid_Task_B.speedNow = SpeedNowB;
pid_Task_C.speedNow = SpeedNowC;
pid_Task_D.speedNow = SpeedNowD;
//执行PID控制函数
Pid_A_Ctrl(&motorA);
Pid_B_Ctrl(&motorB);
Pid_C_Ctrl(&motorC);
Pid_D_Ctrl(&motorD);
/**************************************************************************
函数功能：计算左右轮速
入口参数：int *leftSpeed,int *rightSpeed
返回 值：无
//计算左右车轮线速度，正向速度为正值 ，反向速度为负值，速度为乘以1000之后的速度 mm/s
//一定时间内的编码器变化值*转化率（转化为直线上的距离m）*200s（5ms计算一次） 得到 m/s *1000转化为int数据
一圈的脉冲数：
左：1560
右：1040
轮子半径：0.03m
轮子周长：2*pi*r
一个脉冲的距离：0.000120830m
速度分辨率：0.0120m/s 12.0mm/s
**************************************************************************/
void Get_Motor_Speed(int *ASpeed,int *BSpeed,int *CSpeed,int *DSpeed)
{
static int WheelEncoderNowA = 0;
static int WheelEncoderNowB = 0;
static int WheelEncoderNowC = 0;
static int WheelEncoderNowD = 0;
static int WheelEncoderLastA = 0;
static int WheelEncoderLastB = 0;
static int WheelEncoderLastC = 0;
static int WheelEncoderLastD = 0;
//记录本次左右编码器数据
WheelEncoderNowA += getTIMx_DetaCnt(TIM2);
WheelEncoderNowB += getTIMx_DetaCnt(TIM3);
WheelEncoderNowC += getTIMx_DetaCnt(TIM4);
WheelEncoderNowD += getTIMx_DetaCnt(TIM5);
//5ms测速
*ASpeed = (WheelEncoderNowA - WheelEncoderLastA)*1000*200*0.00015103;
*BSpeed = (WheelEncoderNowB - WheelEncoderLastB)*1000*200*0.00015103;
*CSpeed = -(WheelEncoderNowC - WheelEncoderLastC)*1000*200*0.00015103;
*DSpeed = (WheelEncoderNowD - WheelEncoderLastD)*1000*200*0.00015103;
//记录上次编码器数据
WheelEncoderLastA = WheelEncoderNowA;
WheelEncoderLastB = WheelEncoderNowB;
WheelEncoderLastC = WheelEncoderNowC;
WheelEncoderLastD = WheelEncoderNowD;
}
/*******************************************************************************
* 函数名：Pid_Ctrl(int *leftMotor,int *rightMotor)
* 描述 ：Pid控制
*******************************************************************************/
void Pid_A_Ctrl(int *AMotor)
{
Pid_A_Which(&pid_Task_A);
*AMotor += pid_Task_A.Adjust;
}
void Pid_B_Ctrl(int *BMotor)
{
Pid_B_Which(&pid_Task_B);
*BMotor += pid_Task_B.Adjust;
}
void Pid_C_Ctrl(int *CMotor)
{
Pid_C_Which(&pid_Task_C);
*CMotor += pid_Task_C.Adjust;
}
void Pid_D_Ctrl(int *DMotor)
{
Pid_D_Which(&pid_Task_D);
*DMotor += pid_Task_D.Adjust;
}
差不多是这样子；总结当时我改时出现的问题
1.因为接线接错 后面的两个轮子总是不能转困扰了我一个星期，后面从新差杜邦线接对了就转了，那个恨啊！
2.PID没有调好车子打歪；
3.四个编码器 我用了 TIM2 3 4 5; TIM8 用来输出PWM了，TIM1给了舵机；没有装陀螺仪前一直用通用TIM6和7来调试；
4.下面进入陀螺仪的调试
陀螺仪也是根据上面那位大佬的文章进行改编
陀螺仪陀螺仪
用上陀螺仪后 就不用TIM6 和7 进行调试了 因为都是5ms的定时
void EXTI15_10_IRQHandler(void)
{
EXTI_ClearITPendingBit(EXTI_Line12); //===清除LINE12线路挂起位
getAngle(&yaw,&yaw_acc_error); //获取角度
yaw_acc_error += FIVE_MS_ERROR; //===yaw漂移误差累加
Get_Motor_Speed(&SpeedNowA,&SpeedNowB,&SpeedNowC,&SpeedNowD); //计算电机速度
pid_Task_A.speedNow = SpeedNowA;
pid_Task_B.speedNow = SpeedNowB;
pid_Task_C.speedNow = SpeedNowC;
pid_Task_D.speedNow = SpeedNowD;
//执行PID控制函数
Pid_A_Ctrl(&motorA);
Pid_B_Ctrl(&motorB);
Pid_C_Ctrl(&motorC);
Pid_D_Ctrl(&motorD);
}
得到的数值是 0到360° YAW 这个变量 一上机开始是0° 后面我改了数值让他开机是180°
在这个函数的最后一句 加上180就可以了
/**************************************************************************
函数功能：读取MPU6050内置DMP的姿态信息
入口参数：无
返回 值：无
**************************************************************************/
void Read_DMP(void)
{
unsigned long sensor_timestamp;
unsigned char more;
long quat[4];
dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors, &more);
if (sensors & INV_WXYZ_QUAT )
{
q0=quat[0] / q30;
q1=quat[1] / q30;
q2=quat[2] / q30;
q3=quat[3] / q30;
// Pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;
Roll= atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3; // roll
Yaw = atan2(2 * (q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3)*57.3+180; //yaw 这里加上180是为了上机后的数值为180°
}
}
正式因为这一句我没有注意导致我比赛开始后车子走着走着变歪了 我一开始加的是181°
现在得到的数值后就开始 进行对车身进行保持PID算法让他整场下来保持车身
下面是我用的是 位置式
void balance(int yaw)//roll(横滚角)（回复力）、角速度（阻尼力）
{
float Balance_Kp=20,Balance_Ki=6,Balance_Kd=1.5;//kp 10 ki 5 kd 0.8
float Bias=0,Last_Bias=0,Sun_Bias=0;
Bias=yaw-ZHONGZHI; //计算偏差
Sun_Bias+=Bias;
Pwm=Balance_Kp*Bias+Balance_Ki*Sun_Bias+Balance_Kd*(Bias-Last_Bias); //===计算平衡控制的电机PWM PD控制 kp是P系数 kd是D系数
if(Pwm>690)Pwm=690; //按比例 容忍输出整体的%9 也就是10度左右
if(Pwm<-690)Pwm=-690;
Last_Bias=Bias;
}
在陀螺仪中断中用到这个函数，把测量得到的 YAW放到这个函数中就能得到PWM调节量
电机虽然用的是增量式 但是陀螺仪用位置式也可以调节
把得到的PWM调节加入电机 就可以保持车身了
/***********************************************************************************
** 函数名称 ：void Pid_Which(struct pid_uint *pl, struct pid_uint *pr)
** 函数功能 ：pid选择函数
***********************************************************************************/
void Pid_A_Which(struct pid_uint *pA)
{
/**********************A轮速度pid*************************/
if(pA->En == 1)
{
pA->Adjust = -PID_common(pA->speedSet, pA->speedNow, pA)+Pwm;
}
else
{
pA->Adjust = 0;
reset_Uk(pA);
pA->En = 2;
}
}
void Pid_B_Which(struct pid_uint *pB)
{
/***********************B轮速度pid*************************/
if(pB->En == 1)
{
pB->Adjust = -PID_common(pB->speedSet, pB->speedNow, pB)-Pwm;
}
else
{
pB->Adjust = 0;
reset_Uk(pB);
pB->En = 2;
}
}
void Pid_C_Which(struct pid_uint *pC)
{
/***********************C轮速度pid*************************/
if(pC->En == 1)
{
pC->Adjust = -PID_common(pC->speedSet, pC->speedNow, pC)+Pwm;
}
else
{
pC->Adjust = 0;
reset_Uk(pC);
pC->En = 2;
}
}
void Pid_D_Which(struct pid_uint *pD)
{
/***********************D轮速度pid*************************/
if(pD->En == 1)
{
pD->Adjust = -PID_common(pD->speedSet, pD->speedNow, pD)-Pwm;
}
else
{
pD->Adjust = 0;
reset_Uk(pD);
pD->En = 2;
}
}
总结
以上就是保持车身的过程，其中遇到了很多问题还未解决
1.车身在行走过程中会慢慢变歪一点 可能是 我的PID未调节好和用的方法不对
2.也可能是中断中放了太多函数
3.在调试过程中 我遇到了几个自己粗心的问题 已经解决 就是 初始化函数一定要有逻辑 比如延时函数一定要放在最前面 不然有些初始化过程中需要用到延迟函数 ，但是延迟函数又没有开始初始化，程序会卡死。任何函数初始化一定要遵循逻辑，不然都运行不起来。
以上是车身保持，程序就不发出来了，学过32的都可以根据那位大佬的程序修改出来
下一篇激光测距
5490
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
