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

(1)程序定义部分

(2)程序初始化部分

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

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

(5)电机驱动与控制

(6)PID调制

机甲大师代码详解:

一 定义部分

#include 
#include 
#include
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); '

以上是引脚定义等部分

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

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的遥控和sbus接收机,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();

第三步:然后有这行代码,代表我们要把读取的遥控数据放到rcsig数组里面,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]L)&&(rcsig[1]L)&&(rcsig[4]L)&&(rcsig[1]L)&&(rcsig[2]L)&&(rcsig[4]

注意这里的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是什么?其实是让电机达到最大转速读取它的转速,这就是一个速度的最大值了 注意,正反两方向的最大转速需要取小的那个---------------------------------------------------------------------------------------------------------------------------------------