arduino pid麦克纳姆轮小车程序详解

wzzzq

之前发过几个帖子,大家可以参考,但经过多次尝试,整套系统升级了,优化了很多地方目录:
(1)程序定义部分
(2)程序初始化部分
(3)AB霍尔传感器对电机转速,正反的读取
(4)sbus接收机信号读取与处理
(5)电机驱动与控制
(6)PID调制
机甲大师代码详解:
一 定义部分

1. #include <PID_v1.h>
   
2. #include <FutabaSBUS.h>
   
3. #include<Servo.h>
   
4. FutabaSBUS sbus;
   
5. Servo yaw;
   
6. Servo roll;
   
7. int yawval,rollval;
   
8. int rcsig[25];
   
9. const int ma1=9;//电机A接口9
   
10. const int ma2=2;//电机A接口2
    
11. const int mb1=3;//电机B接口1
    
12. const int mb2=4;//电机B接口2
    
13. const int mc1=5;//以此类推
    
14. const int mc2=6;
    
15. const int md1=7;
    
16. const int md2=8;//电机D接口2
    
17. //-----------------上面定义电机PWM信号输出口
    
18. const int ma_in=20;//电机A转速输入口
    
19. const int mb_in=19;//电机B转速输入口
    
20. const int mc_in=18;//以此类推
    
21. const int md_in=21;
    
22. const int Q1=28;//B相输入
    
23. const int Q2=26;
    
24. const int Q3=22;
    
25. const int Q4=24;
    
26. bool rev1,rev2,rev3,rev4;//方向
    
27. //-----------------上面是中断读取霍尔传感器转速端口
    
28. int mafor,mago,maturn;
    
29. int mbfor,mbgo,mbturn;
    
30. int mcfor,mcgo,mcturn;
    
31. int mdfor,mdgo,mdturn;
    
32. //-----------------4电机的旋转,平移,前后值
    
33. const int L=1015,R=1035;
    
34. double M1PWMOUT,M2PWMOUT,M3PWMOUT,M4PWMOUT;//M1输出pwm，M2输出pwm，以此类推
    
35. double ref1,ref2,ref3,ref4;//四个电机的参考转速
    
36. double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脉冲的个数，M1-4S是转换成转速（没有单位）后的值
    
37. double Kp=6, Ki=1.3, Kd=0.1;  //PID系数
    
38. unsigned long t;
    
39. PID M1PID(&M1S,&M1PWMOUT,&ref1,Kp, Ki, Kd, DIRECT); //定义PID类
    
40. PID M2PID(&M2S,&M2PWMOUT,&ref2,Kp, Ki, Kd, DIRECT);
    
41. PID M3PID(&M3S,&M3PWMOUT,&ref3,Kp, Ki, Kd, DIRECT);
    
42. PID M4PID(&M4S,&M4PWMOUT,&ref4,Kp, Ki, Kd, DIRECT); '

复制代码

以上是引脚定义等部分
首先我们要包括读取遥控sbus信号的头文件,pid的头文件,以及舵机的头文件
接着定义电机驱动板的引脚,一个电机在驱动板上对应两个pwm输入的引脚,所以对于M1有ma1,ma2两个引脚输入pwm信号,同理有mb1,mb2,mc1,mc2,md1,md2分别对应其他三个带电机的引脚

而每个电机有霍尔传感器,霍尔传感器有AB两相输出,我们用A相发生电平变化触发的外部中断判断速度,而用A相触发时判断B的高低,高就代表B在A前面,反之在A后面,从而判断正反转向
于是我们定义M1电机的A相输入为ma_in,M2为mb_in,注意这里的引脚需要是单片机上支持外部中断的引脚
而B相定义为Q1,Q2,Q3,Q4,电机转动的方向定义为rev1,rev2,rev3,rev4
int mafor,mago,maturn;
int mbfor,mbgo,mbturn;
int mcfor,mcgo,mcturn;
int mdfor,mdgo,mdturn;
代表4电机的旋转,平移,前后值
后面一坨是Pid的变量以及pid的类
这里采用arduino自带的PID
二 初始化部分
void setup()
{
   pinMode(ma1,OUTPUT);
   pinMode(ma2,OUTPUT);
   pinMode(mb1,OUTPUT);
   pinMode(mb2,OUTPUT);
   pinMode(mc1,OUTPUT);
   pinMode(mc2,OUTPUT);
   pinMode(md1,OUTPUT);
   pinMode(md2,OUTPUT);
   pinMode(33,OUTPUT);
   pinMode(Q1,INPUT);
   pinMode(Q2,INPUT);
   pinMode(Q3,INPUT);
   pinMode(Q4,INPUT);
   pinMode(35,OUTPUT);
   /************/
   Serial.begin(9600);
   attachInterrupt(digitalPinToInterrupt(ma_in), macount, FALLING);//触发信号必须是变化的，上升或下降皆可（外部中断读转速）
   attachInterrupt(digitalPinToInterrupt(mb_in), mbcount, FALLING);//触发信号必须是变化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(mc_in), mccount, FALLING);//触发信号必须是变化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(md_in), mdcount, FALLING);//触发信号必须是变化的，上升或下降皆可
   M1PID.SetMode(AUTOMATIC);//设置PID为自动模式
   M1PID.SetSampleTime(30);//设置PID采样频率为100ms
   M2PID.SetMode(AUTOMATIC);//设置PID为自动模式
   M2PID.SetSampleTime(30);//设置PID采样频率为100ms
   M3PID.SetMode(AUTOMATIC);//设置PID为自动模式
   M3PID.SetSampleTime(30);//设置PID采样频率为100ms
   M4PID.SetMode(AUTOMATIC);//设置PID为自动模式
   M4PID.SetSampleTime(30);//设置PID采样频率为100ms
   M1PID.SetOutputLimits(-255,255);//PID输出值设置为-255~255
   M2PID.SetOutputLimits(-255,255);
   M3PID.SetOutputLimits(-255,255);
   M4PID.SetOutputLimits(-255,255);
   yaw.attach(25);
   roll.attach(23);
   buzzer(100);
   delay(100);
   buzzer(100);
   delay(100);
   buzzer(200);
  sbus.begin(Serial3);
   sbus.attachDataReceived(dataReceived);
   t=millis();
}
定义外部中断,设置PID,sbus初始化
三 转速读取
首先,我们要写外部中断的触发要做的操作,将计数变量+1,并判断B相来赋值1,0给rev代表电机转向

这里肯定有人要问 :为什么需要读取转向,一般PID调制只需要速率不就行了吗 [以下几行看不懂可以不看,不影响后续阅读,别把心态看炸了]
\\\\其实是因为这里的速度需要是一个[矢量].因为我用的算法不是先判断遥控信号对应的运动应该是前进还是后退再控制电机,而是直接将遥控信号映射到三个运动方向(偏航,平移,前后),这样可以使运动更平滑,而且可以做出组合动作.但缺点就是控制电机的时候我不知道到底小车要做的是什么运动,所以可能会出现这种情况:一开始某电机需要以-20的转速旋转(即以20的速率反向旋转),后来这台电机又需要以+30的速度旋转(即以正向的30速率旋转),那么这时[PID算法]中的[误差值]怎么算?是|30-20|=10还是|30-(-20)|=50?实际上,误差值应该是50,但是如果这里的速率只是个标量的话就会算出来10,这样这台电机的调速就会出问题.从另一方面解释,如果采用了先通过遥控信号判断运动方向,那么读取的速度其实[方向]已经固定了,所以需要处理的只有[数值].比如遥控的信号是俯仰+200,那么我就会进入if(俯仰通道有值且为正)这个程序段,这时候速度的方向其实就固定为+了.所以我们就发现这样的算法方向改变很生硬,不够灵活.\\\

1. void macount()//转速加一
   
2. {
   
3.   rev1=digitalRead(Q1);
   
4.   in1++;
   
5. }

复制代码

然后我们有了总共的脉冲数,但我们要的是速度,也就是单位时间的脉冲数,于是我们就有了这样的思路:用一个t变量,t=系统时间,每过了30ms将in1赋值给M1S(速度变量),并将in1清零,用于下一个30ms的读取

1. if(millis()>t)
   
2.   {
   
3.     Speed();
   
4.     t=millis()+30;
   
5.   }

复制代码

于是有了这样的代码
那么Speed()函数呢?其实也很简单

1. void Speed()
   
2. {
   
3.   GetM1Speed();
   
4.   GetM2Speed();
   
5.   GetM3Speed();
   
6.   GetM4Speed();
   
7. }
   
8. void GetM1Speed()//刷转速
   
9. {
   
10.     if(rev1)
    
11.       M1S =in1;//M1S变成in1
    
12.     else
    
13.       M1S=-in1;
    
14.     in1 = 0;//输出速度结果后清零，记录下一秒的触发次数
    
15. }

复制代码

如果rev1=1代表电机正转,那么转速为正,否则为负
四 遥控器数据读取
我这里采用了天地飞9的遥控和sbus接收机,sbus是一种协议,它与uart类似,可以与单片机通过串口通信(Serial)
(别看这个点很简单,我花了很多时间研究这个东西)
有懂航模的朋友就要问了 为什么不直接读取pwm输出的接收机 实话告诉你,我一开始就是用的pwm接收机,因为它代码简单,读取方便
但是它有个致命缺点:会被外部中断干扰,而且读取一下要2ms,并且精度低
因为pwm接收机是直接插舵机的,相当于接收机先接受信号,再转换成pwm输出,单片机再计时读取,绕了个大圈
于是我们选择sbus接收机,虽然代码量大一些,复杂一些,但它耗时少,精度高
软件:我们首先要下载并安装futabasbus(虽然名字是futaba但其他品牌的sbus也可以用)的库和streaming库(sbus示例程序中要用到,但是不包含也问题不大)文末会有下载链接
第一步:在setup()中写这两句话,初始化sbus
注意! 这里我们发现有一个Serial3,为什么要用Serial3,为什么呢?因为Serial(针脚0,1)是用来与电脑通信的,如果占用它下载程序和串口监视器都会出问题,所以尽量用其他几个Serial

1. sbus.begin(Serial3);
   
2.    sbus.attachDataReceived(dataReceived);

复制代码

第二步:在loop()中写这句话,每次读取一下信号

1. sbus.receive();

复制代码

第三步:然后有这行代码,代表我们要把读取的遥控数据放到rcsig数组里面,17,18是两个数字通道(我也不知道有什么用)

1. void dataReceived(ChannelData channels) {
   
2.         // do something with the data
   
3.         for(int i=1;i<=16;i++)
   
4.         {
   
5.           rcsig[i]=channels.data[i-1];
   
6.         }
   
7.         rcsig[17]=channels.channels.channel17;
   
8.         rcsig[18]=channels.channels.channel18;
   
9. }

复制代码

硬件: 硬件部分会复杂一些,因为sbus的信号其实是反的,我们要手工做一个电平取反器
如上图，使用S8050三极管反向信号
做好取反器后将输入连到接收机输出,取反器输出连接单片机的rx3(注意要连到rx3,初始化写的是哪个串口就连到哪个)
上图是反向前的sbus信号

经过反向的sbus信号
可以看到反向后噪波有点大，但经过测试不影响读取
五 电机控制
电机控制是一个比较重要的部分,这里我会介绍一种比较新颖,高级,顺滑的(划掉)控制策略,与一般的控制策略不同
常见的控制方法是判断遥控数据如果低于某个值就后退,高于某个值就前进,但是这样并不顺滑,不适合用于机甲大师.  这种方法的好处不需要知道电机的转向,因为既然我已经分类前进后退了,就自然知道转向了
那么它既然不顺滑,为了追求完美,自然要找一种新的方法
我们想象一下,第一种控制方法是以遥控器的数据分类,那么能不能以电机分类?
于是就想到了这样的方法,每个电机对应一个前进后退方向的值,左右平移方向的值,旋转的值,三个值由遥控器的数据算出,叠加后输出给电机,这样电机转的会更加平滑!因为换向,变速不需要再经过一个遥控数据的判断
转换成代码如下

1. if(!((rcsig[2]>L)&&(rcsig[2]<R)))
   
2.     {
   
3.       mafor=map(rcsig[2],340,1708,-60,60);
   
4.       mbfor=map(rcsig[2],340,1708,-60,60);
   
5.       mcfor=map(rcsig[2],340,1708,-60,60);
   
6.       mdfor=map(rcsig[2],340,1708,-60,60);
   
7.     }
   
8.     else
   
9.     {
   
10.       mafor=0;
    
11.       mbfor=0;
    
12.       mcfor=0;
    
13.       mdfor=0;
    
14.     }
    
15.     if(!((rcsig[1]>L)&&(rcsig[1]<R)))
    
16.     {
    
17.       mago=map(rcsig[1],340,1708,-60,60);
    
18.       mbgo=map(rcsig[1],340,1708,60,-60);
    
19.       mcgo=map(rcsig[1],340,1708,-60,60);
    
20.       mdgo=map(rcsig[1],340,1708,60,-60);
    
21.     }
    
22.     else
    
23.     {
    
24.       mago=0;
    
25.       mbgo=0;
    
26.       mcgo=0;
    
27.       mdgo=0;
    
28.     }
    
29.     if(!((rcsig[4]>L)&&(rcsig[4]<R)))
    
30.     {
    
31.       maturn=map(rcsig[4],340,1708,60,-60);
    
32.       mbturn=map(rcsig[4],340,1708,-60,60);
    
33.       mcturn=map(rcsig[4],340,1708,-60,60);
    
34.       mdturn=map(rcsig[4],340,1708,60,-60);
    
35.     }
    
36.     else
    
37.     {
    
38.       maturn=0;
    
39.       mbturn=0;
    
40.       mcturn=0;
    
41.       mdturn=0;
    
42.     }
    
43.     ref1=max(min(mafor+mago+maturn,60),-60);
    
44.     ref2=max(min(mbfor+mbgo+mbturn,60),-60);
    
45.     ref3=max(min(mcfor+mcgo+mcturn,60),-60);
    
46.     ref4=max(min(mdfor+mdgo+mdturn,60),-60);
    
47.     /*************计算4个电机的参考转速*******************/
    
48.     setspeed();
    
49.     moving();
    
50.     if((rcsig[1]>L)&&(rcsig[1]<R)&&(rcsig[2]>L)&&(rcsig[2]<R)&&(rcsig[4]>L)&&(rcsig[4]<R))
    
51.     stop_all();

复制代码

注意这里的ref是有上限和下限的,需要判断一下然后moving()函数怎么写呢?其实就是把pwm输出给电机

1. void moving()//移动
   
2. {
   
3.   if(M1PWMOUT>0)
   
4.   {
   
5.       analogWrite(ma2,M1PWMOUT);
   
6.       digitalWrite(ma1,0);
   
7.   }
   
8.   else
   
9.   {
   
10.       analogWrite(ma1,abs(M1PWMOUT));
    
11.       digitalWrite(ma2,0);
    
12.   }
    
13.   if(M2PWMOUT>0)
    
14.   {
    
15.      analogWrite(mb1,M2PWMOUT);
    
16.      digitalWrite(mb2,0);
    
17.   }
    
18.   else
    
19.   {
    
20.      analogWrite(mb2,abs(M2PWMOUT));
    
21.      digitalWrite(mb1,0);
    
22.   }
    
23.   if(M3PWMOUT>0)
    
24.   {
    
25.      analogWrite(mc1,M3PWMOUT);
    
26.      digitalWrite(mc2,0);
    
27.   }
    
28.   else
    
29.   {
    
30.     analogWrite(mc2,abs(M3PWMOUT));
    
31.     digitalWrite(mc1,0);
    
32.   }
    
33.   if(M4PWMOUT>0)
    
34.   {
    
35.      analogWrite(md2,M4PWMOUT);
    
36.      digitalWrite(md1,0);
    
37.   }
    
38.   else
    
39.   {
    
40.      analogWrite(md1,abs(M4PWMOUT));
    
41.      digitalWrite(md2,0);
    
42.   }
    
43.   //Serial.println("moved");
    
44. }

复制代码

六 PID调制

看了上面的第五部分,有人肯定有疑问了,这个65是什么?PWMOUT又是怎么算出来的?
且慢,这都是PID的事
想让小车走的直可以一定程度上转化成让电机转速一样,那么现在速度知道了,我们自然想到PID控制
其实PID本身不难,而且arduino有pid库(文末下载)
第一步:初始化&定义
这一部分放在setup()中

1. M1PID.SetMode(AUTOMATIC);//设置PID为自动模式
   
2.    M1PID.SetSampleTime(30);//设置PID采样频率为100ms
   
3.    M2PID.SetMode(AUTOMATIC);//设置PID为自动模式
   
4.    M2PID.SetSampleTime(30);//设置PID采样频率为100ms
   
5.    M3PID.SetMode(AUTOMATIC);//设置PID为自动模式
   
6.    M3PID.SetSampleTime(30);//设置PID采样频率为100ms
   
7.    M4PID.SetMode(AUTOMATIC);//设置PID为自动模式
   
8.    M4PID.SetSampleTime(30);//设置PID采样频率为100ms
   
9.    M1PID.SetOutputLimits(-255,255);//PID输出值设置为-255~255
   
10.    M2PID.SetOutputLimits(-255,255);
    
11.    M3PID.SetOutputLimits(-255,255);
    
12.    M4PID.SetOutputLimits(-255,255);

复制代码

1. double M1PWMOUT,M2PWMOUT,M3PWMOUT,M4PWMOUT;//M1输出pwm，M2输出pwm，以此类推
   
2. double ref1,ref2,ref3,ref4;//四个电机的参考转速
   
3. double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脉冲的个数，M1-4S是转换成转速（没有单位）后的值
   
4. double Kp=6, Ki=1.3, Kd=0.1;  //PID系数
   
5. unsigned long t;
   
6. PID M1PID(&M1S,&M1PWMOUT,&ref1,Kp, Ki, Kd, DIRECT); //定义PID类
   
7. PID M2PID(&M2S,&M2PWMOUT,&ref2,Kp, Ki, Kd, DIRECT);
   
8. PID M3PID(&M3S,&M3PWMOUT,&ref3,Kp, Ki, Kd, DIRECT);
   
9. PID M4PID(&M4S,&M4PWMOUT,&ref4,Kp, Ki, Kd, DIRECT);

复制代码

上面这一段是定义PID需要一个参考转速,也就是电机我希望它达到这个转速,p,i,d这三个参数,M1S,M2S,M3S,M4S这四个实际速度,和pwmout,代表输出的pwm
而65是什么?其实是让电机达到最大转速读取它的转速,这就是一个速度的最大值了 注意,正反两方向的最大转速需要取小的那个---------------------------------------------------------------------------------------------------------------------------------------
全部程序:

游客，如果您要查看本帖隐藏内容请回复

---------------------------------------附件下载(库文件)------------------------------------------------------

游客，如果您要查看本帖隐藏内容请回复

wzzzq

BUG修复日志2021.2.1  电机M4开始抽风,后退时一直以最大转速转使用串口监视器调试,发现无论正反旋转,速度都为正
故障排除
1.软件
经检查没问题
2.电机
更换了一个电机,还是抽风
3.接线
把md_in(A相输入)改为Q4(B相输入),速度始终为0发现问题:M4B相断线
经过重新焊接,恢复正常

wzzzq

如果有什么问题尽量在社区回帖,我会查看,这样可以帮助更多人,比较复杂可以发邮箱2936385857@qq.com,微信我一般不在线

wzzzq

图传效果很好
一开始很模糊,但是拔掉5G频段的路由器就好多了
这款图传是带OSD的,有需要可以自己研究

wzzzq

https://www.bilibili.com/video/BV1hK4y1374Z

wzzzq

附上三张模式图

嘉哥呀呀呀

mega2560的板子可以吗

嘉哥呀呀呀

我手上只有乐迪t8fb的遥控，一样的吗

wzzzq

嘉哥呀呀呀 发表于 2020-8-2 16:31
mega2560的板子可以吗

就是mega2560,示意图只是示意一下,mega2560处理速度快,端口多,uno端口不够

wzzzq

嘉哥呀呀呀 发表于 2020-8-2 16:32
我手上只有乐迪t8fb的遥控，一样的吗

乐迪的也可以,只要有sbus接收机就可以
但是我记得乐迪没有sbus吧
后面我会再出一个wifi手机视频控制的教程,也可以等那个

wzzzq

嘉哥呀呀呀

wzzzq 发表于 2020-8-2 16:35
乐迪的也可以,只要有sbus接收机就可以
但是我记得乐迪没有sbus吧
后面我会再出一个wifi手机视频控制的教 ...

我这个有接收机，接线的时候就接sbus和正负三根线吗

嘉哥呀呀呀

嘉哥呀呀呀 发表于 2020-8-2 16:42
我这个有接收机，接线的时候就接sbus和正负三根线吗

我之前用这个遥控是八个通道口都接线，但是感觉有延迟，比蓝牙控制效果还差劲

wzzzq

嘉哥呀呀呀 发表于 2020-8-2 16:42
我这个有接收机，接线的时候就接sbus和正负三根线吗

还有个电平取反器,需要自己做,因为sbus的信号是反的,可以参照文中的一个链接
淘宝上不知道有没有,可以去百度一下,好像有人买过(如果有也发我个购买链接)