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

发表时间:2021-09-14 10:00:00 人气:141

之前发过几个帖子,大家可以参考,但经过多次尝试,整套系统升级了,优化了很多地方目录:

(1)程序定义部分

(2)程序初始化部分

(3)AB霍尔传感器对电机转速,正反的读取

(4)sbus接收机信号读取与处理

(5)电机驱动与控制

(6)PID调制

机甲大师代码详解:

一 定义部分

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


以上是引脚定义等部分

首先我们要包括读取?豷bus信号的头文件,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(俯仰通道有值且为正)这个程序段,这时候速度的方向其实就固定为+了.所以我们就发现这样的算法方向改变很生硬,不够灵活.

void macount()//转速加一
{
  rev1=digitalRead(Q1);
  in1++;
}

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

if(millis()>t)
  {
    Speed();
    t=millis()+30;
  }


于是有了这样的代码

那么Speed()函数呢?其实也很简单

void Speed()
{
  GetM1Speed();
  GetM2Speed();
  GetM3Speed();
  GetM4Speed();
}
void GetM1Speed()//刷转速
{
    if(rev1)
      M1S =in1;//M1S变成in1
    else
      M1S=-in1;
    in1 = 0;//输出速度结果后清零,记录下一秒的触发次数
}


如果rev1=1代表电机正转,那么转速为正,否则为负

四 ?仄魇荻寥

我这里采用了天地飞9的?睾蛃bus接收机,sbus是一种协议,它与uart类似,可以与单片机通过串口通信(Serial)

(别看这个点很简单,我花了很多时间研究这个东西)

有懂航模的朋友就要问了 为什么不直接读取pwm输出的接收机 实话告诉你,我一开始就是用的pwm接收机,因为它代码简单,读取方便

但是它有个致命缺点:会被外部中断干扰,而且读取一下要2ms,并且精度低

因为pwm接收机是直接插舵机的,相当于接收机先接受信号,再转换成pwm输出,单片机再计时读取,绕了个大圈

于是我们选择sbus接收机,虽然代码量大一些,复杂一些,但它耗时少,精度高

软件:我们首先要下载并安装futabasbus(虽然名字是futaba但其他品牌的sbus也可以用)的库和streaming库(sbus示例程序中要用到,但是不包含也问题不大)文末会有下载链接

第一步:在setup()中写这两句话,初始化sbus

注意! 这里我们发现有一个Serial3,为什么要用Serial3,为什么呢?因为Serial(针脚0,1)是用来与电脑通信的,如果占用它下载程序和串口监视器都会出问题,所以尽量用其他几个Serial

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


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

sbus.receive();


第三步:然后有这行代码,代表我们要把读取的?厥莘诺絩csig数组里面,17,18是两个数字通道(我也不知道有什么用)

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


硬件: 硬件部分会复杂一些,因为sbus的信号其实是反的,我们要手工做一个电平取反器

  如上图,使用S8050三极管反向信号

做好取反器后将输入连到接收机输出,取反器输出连接单片机的rx3(注意要连到rx3,初始化写的是哪个串口就连到哪个)


上图是反向前的sbus信号


经过反向的sbus信号

可以看到反向后噪波有点大,但经过测试不影响读取

五 电机控制

电机控制是一个比较重要的部分,这里我会介绍一种比较新颖,高级,顺滑的(划掉)控制策略,与一般的控制策略不同

常见的控制方法是判断?厥萑绻陀谀掣鲋稻秃笸,高于某个值就前进,但是这样并不顺滑,不适合用于机甲大师.  这种方法的好处不需要知道电机的转向,因为既然我已经分类前进后退了,就自然知道转向了

那么它既然不顺滑,为了追求完美,自然要找一种新的方法

我们想象一下,第一种控制方法是以?仄鞯氖莘掷,那么能不能以电机分类?

于是就想到了这样的方法,每个电机对应一个前进后退方向的值,左右平移方向的值,旋转的值,三个值由?仄鞯氖菟愠,叠加后输出给电机,这样电机转的会更加平滑!因为换向,变速不需要再经过一个?厥莸呐卸

转换成代码如下

if(!((rcsig[2]>L)&&(rcsig[2]<R)))
    {
      mafor=map(rcsig[2],340,1708,-60,60);
      mbfor=map(rcsig[2],340,1708,-60,60);
      mcfor=map(rcsig[2],340,1708,-60,60);
      mdfor=map(rcsig[2],340,1708,-60,60);
    }
    else
    {
      mafor=0;
      mbfor=0;
      mcfor=0;
      mdfor=0;
    }
    if(!((rcsig[1]>L)&&(rcsig[1]<R)))
    {
      mago=map(rcsig[1],340,1708,-60,60);
      mbgo=map(rcsig[1],340,1708,60,-60);
      mcgo=map(rcsig[1],340,1708,-60,60);
      mdgo=map(rcsig[1],340,1708,60,-60);
    }
    else
    {
      mago=0;
      mbgo=0;
      mcgo=0;
      mdgo=0;
    }
    if(!((rcsig[4]>L)&&(rcsig[4]<R)))
    {
      maturn=map(rcsig[4],340,1708,60,-60);
      mbturn=map(rcsig[4],340,1708,-60,60);
      mcturn=map(rcsig[4],340,1708,-60,60);
      mdturn=map(rcsig[4],340,1708,60,-60);
    }
    else
    {
      maturn=0;
      mbturn=0;
      mcturn=0;
      mdturn=0;
    }
    ref1=max(min(mafor+mago+maturn,60),-60);
    ref2=max(min(mbfor+mbgo+mbturn,60),-60);
    ref3=max(min(mcfor+mcgo+mcturn,60),-60);
    ref4=max(min(mdfor+mdgo+mdturn,60),-60);
    /*************计算4个电机的参考转速*******************/
    setspeed();
    moving();
    if((rcsig[1]>L)&&(rcsig[1]<R)&&(rcsig[2]>L)&&(rcsig[2]<R)&&(rcsig[4]>L)&&(rcsig[4]<R))
    stop_all();


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

void moving()//移动
{
  if(M1PWMOUT>0)
  {
      analogWrite(ma2,M1PWMOUT);
      digitalWrite(ma1,0);
  }
  else
  {
      analogWrite(ma1,abs(M1PWMOUT));
      digitalWrite(ma2,0);
  }
  if(M2PWMOUT>0)
  {
     analogWrite(mb1,M2PWMOUT);
     digitalWrite(mb2,0);
  }
  else
  {
     analogWrite(mb2,abs(M2PWMOUT));
     digitalWrite(mb1,0);
  }
  if(M3PWMOUT>0)
  {
     analogWrite(mc1,M3PWMOUT);
     digitalWrite(mc2,0);
  }
  else
  {
    analogWrite(mc2,abs(M3PWMOUT));
    digitalWrite(mc1,0);
  }
  if(M4PWMOUT>0)
  {
     analogWrite(md2,M4PWMOUT);
     digitalWrite(md1,0);
  }
  else
  {
     analogWrite(md1,abs(M4PWMOUT));
     digitalWrite(md2,0);
  }
  //Serial.println("moved");
}

复制代码


六 PID调制


看了上面的第五部分,有人肯定有疑问了,这个65是什么?PWMOUT又是怎么算出来的?

且慢,这都是PID的事

想让小车走的直可以一定程度上转化成让电机转速一样,那么现在速度知道了,我们自然想到PID控制

其实PID本身不难,而且arduino有pid库(文末下载)

第一步:初始化&定义

这一部分放在setup()中


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


上面这一段是定义PID需要一个参考转速,也就是电机我希望它达到这个转速,p,i,d这三个参数,M1S,M2S,M3S,M4S这四个实际速度,和pwmout,代表输出的pwm

而65是什么?其实是让电机达到最大转速读取它的转速,这就是一个速度的最大值了 注意,正反两方向的最大转速需要取小的那个---------------------------------------------------------------------------------------------------------------------------------------


此文关键字: pcb行业