青青国产在线播放_av国产乱码一区二区三_久久亚洲精品系列网站_亚洲一区综合色_影院国产精品首页

之前發(fā)過幾個帖子,大家可以參考,但經(jīng)過多次嘗試,整套系統(tǒng)升級了,優(yōu)化了很多地方目錄:

(1)程序定義部分

(2)程序初始化部分

(3)AB霍爾傳感器對電機轉(zhuǎn)速,正反的讀取

(4)sbus接收機信號讀取與處理

(5)電機驅(qū)動與控制

(6)PID調(diào)制

機甲大師代碼詳解:

一 定義部分

#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轉(zhuǎn)速輸入口
const int mb_in=19;//電機B轉(zhuǎn)速輸入口
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;//方向
//-----------------上面是中斷讀取霍爾傳感器轉(zhuǎn)速端口
int mafor,mago,maturn;
int mbfor,mbgo,mbturn;
int mcfor,mcgo,mcturn;
int mdfor,mdgo,mdturn;
//-----------------4電機的旋轉(zhuǎn),平移,前后值
const int L=1015,R=1035;
double M1PWMOUT,M2PWMOUT,M3PWMOUT,M4PWMOUT;//M1輸出pwm，M2輸出pwm，以此類推
double ref1,ref2,ref3,ref4;//四個電機的參考轉(zhuǎn)速
double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脈沖的個數(shù)，M1-4S是轉(zhuǎn)換成轉(zhuǎn)速（沒有單位）后的值
double Kp=6, Ki=1.3, Kd=0.1;  //PID系數(shù)
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的頭文件,以及舵機的頭文件

接著定義電機驅(qū)動板的引腳,一個電機在驅(qū)動板上對應(yīng)兩個pwm輸入的引腳,所以對于M1有ma1,ma2兩個引腳輸入pwm信號,同理有mb1,mb2,mc1,mc2,md1,md2分別對應(yīng)其他三個帶電機的引腳

而每個電機有霍爾傳感器,霍爾傳感器有AB兩相輸出,我們用A相發(fā)生電平變化觸發(fā)的外部中斷判斷速度,而用A相觸發(fā)時判斷B的高低,高就代表B在A前面,反之在A后面,從而判斷正反轉(zhuǎn)向

于是我們定義M1電機的A相輸入為ma_in,M2為mb_in,注意這里的引腳需要是單片機上支持外部中斷的引腳

而B相定義為Q1,Q2,Q3,Q4,電機轉(zhuǎn)動的方向定義為rev1,rev2,rev3,rev4

int mafor,mago,maturn;
int mbfor,mbgo,mbturn;
int mcfor,mcgo,mcturn;
int mdfor,mdgo,mdturn;

代表4電機的旋轉(zhuǎn),平移,前后值

后面一坨是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);//觸發(fā)信號必須是變化的，上升或下降皆可（外部中斷讀轉(zhuǎn)速）
   attachInterrupt(digitalPinToInterrupt(mb_in), mbcount, FALLING);//觸發(fā)信號必須是變化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(mc_in), mccount, FALLING);//觸發(fā)信號必須是變化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(md_in), mdcount, FALLING);//觸發(fā)信號必須是變化的，上升或下降皆可
   M1PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M1PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M2PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M2PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M3PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M3PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M4PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M4PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M1PID.SetOutputLimits(-255,255);//PID輸出值設(shè)置為-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();
}

定義外部中斷,設(shè)置PID,sbus初始化

三 轉(zhuǎn)速讀取

首先,我們要寫外部中斷的觸發(fā)要做的操作,將計數(shù)變量+1,并判斷B相來賦值1,0給rev代表電機轉(zhuǎn)向

這里肯定有人要問 :為什么需要讀取轉(zhuǎn)向,一般PID調(diào)制只需要速率不就行了嗎 [以下幾行看不懂可以不看,不影響后續(xù)閱讀,別把心態(tài)看炸了]

其實是因為這里的速度需要是一個[矢量].因為我用的算法不是先判斷遙控信號對應(yīng)的運動應(yīng)該是前進還是后退再控制電機,而是直接將遙控信號映射到三個運動方向(偏航,平移,前后),這樣可以使運動更平滑,而且可以做出組合動作.但缺點就是控制電機的時候我不知道到底小車要做的是什么運動,所以可能會出現(xiàn)這種情況:一開始某電機需要以-20的轉(zhuǎn)速旋轉(zhuǎn)(即以20的速率反向旋轉(zhuǎn)),后來這臺電機又需要以+30的速度旋轉(zhuǎn)(即以正向的30速率旋轉(zhuǎn)),那么這時[PID算法]中的[誤差值]怎么算?是|30-20|=10還是|30-(-20)|=50?實際上,誤差值應(yīng)該是50,但是如果這里的速率只是個標(biāo)量的話就會算出來10,這樣這臺電機的調(diào)速就會出問題.從另一方面解釋,如果采用了先通過遙控信號判斷運動方向,那么讀取的速度其實[方向]已經(jīng)固定了,所以需要處理的只有[數(shù)值].比如遙控的信號是俯仰+200,那么我就會進入if(俯仰通道有值且為正)這個程序段,這時候速度的方向其實就固定為+了.所以我們就發(fā)現(xiàn)這樣的算法方向改變很生硬,不夠靈活.

void macount()//轉(zhuǎn)速加一
{
  rev1=digitalRead(Q1);
  in1++;
}

然后我們有了總共的脈沖數(shù),但我們要的是速度,也就是單位時間的脈沖數(shù),于是我們就有了這樣的思路:用一個t變量,t=系統(tǒng)時間,每過了30ms將in1賦值給M1S(速度變量),并將in1清零,用于下一個30ms的讀取

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

于是有了這樣的代碼

那么Speed()函數(shù)呢?其實也很簡單

void Speed()
{
  GetM1Speed();
  GetM2Speed();
  GetM3Speed();
  GetM4Speed();
}
void GetM1Speed()//刷轉(zhuǎn)速
{
    if(rev1)
      M1S =in1;//M1S變成in1
    else
      M1S=-in1;
    in1 = 0;//輸出速度結(jié)果后清零，記錄下一秒的觸發(fā)次數(shù)
}

如果rev1=1代表電機正轉(zhuǎn),那么轉(zhuǎn)速為正,否則為負

四 遙控器數(shù)據(jù)讀取

我這里采用了天地飛9的遙控和sbus接收機,sbus是一種協(xié)議,它與uart類似,可以與單片機通過串口通信(Serial)

(別看這個點很簡單,我花了很多時間研究這個東西)

有懂航模的朋友就要問了 為什么不直接讀取pwm輸出的接收機 實話告訴你,我一開始就是用的pwm接收機,因為它代碼簡單,讀取方便

但是它有個致命缺點:會被外部中斷干擾,而且讀取一下要2ms,并且精度低

因為pwm接收機是直接插舵機的,相當(dāng)于接收機先接受信號,再轉(zhuǎn)換成pwm輸出,單片機再計時讀取,繞了個大圈

于是我們選擇sbus接收機,雖然代碼量大一些,復(fù)雜一些,但它耗時少,精度高

軟件:我們首先要下載并安裝futabasbus(雖然名字是futaba但其他品牌的sbus也可以用)的庫和streaming庫(sbus示例程序中要用到,但是不包含也問題不大)文末會有下載鏈接

第一步:在setup()中寫這兩句話,初始化sbus

注意! 這里我們發(fā)現(xiàn)有一個Serial3,為什么要用Serial3,為什么呢?因為Serial(針腳0,1)是用來與電腦通信的,如果占用它下載程序和串口監(jiān)視器都會出問題,所以盡量用其他幾個Serial

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

第二步:在loop()中寫這句話,每次讀取一下信號

sbus.receive();

第三步:然后有這行代碼,代表我們要把讀取的遙控數(shù)據(jù)放到rcsig數(shù)組里面,17,18是兩個數(shù)字通道(我也不知道有什么用)

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;
}

硬件: 硬件部分會復(fù)雜一些,因為sbus的信號其實是反的,我們要手工做一個電平取反器

  如上圖，使用S8050三極管反向信號

做好取反器后將輸入連到接收機輸出,取反器輸出連接單片機的rx3(注意要連到rx3,初始化寫的是哪個串口就連到哪個)

上圖是反向前的sbus信號

經(jīng)過反向的sbus信號

可以看到反向后噪波有點大，但經(jīng)過測試不影響讀取

五 電機控制

電機控制是一個比較重要的部分,這里我會介紹一種比較新穎,高級,順滑的(劃掉)控制策略,與一般的控制策略不同

常見的控制方法是判斷遙控數(shù)據(jù)如果低于某個值就后退,高于某個值就前進,但是這樣并不順滑,不適合用于機甲大師.  這種方法的好處不需要知道電機的轉(zhuǎn)向,因為既然我已經(jīng)分類前進后退了,就自然知道轉(zhuǎn)向了

那么它既然不順滑,為了追求完美,自然要找一種新的方法

我們想象一下,第一種控制方法是以遙控器的數(shù)據(jù)分類,那么能不能以電機分類?

于是就想到了這樣的方法,每個電機對應(yīng)一個前進后退方向的值,左右平移方向的值,旋轉(zhuǎn)的值,三個值由遙控器的數(shù)據(jù)算出,疊加后輸出給電機,這樣電機轉(zhuǎn)的會更加平滑!因為換向,變速不需要再經(jīng)過一個遙控數(shù)據(jù)的判斷

轉(zhuǎn)換成代碼如下

if(!((rcsig[2]>L)&&(rcsig[2]L)&&(rcsig[1]L)&&(rcsig[4]L)&&(rcsig[1]L)&&(rcsig[2]L)&&(rcsig[4]

注意這里的ref是有上限和下限的,需要判斷一下然后moving()函數(shù)怎么寫呢?其實就是把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");
}
復(fù)制代碼

六 PID調(diào)制

看了上面的第五部分,有人肯定有疑問了,這個65是什么?PWMOUT又是怎么算出來的?
且慢,這都是PID的事
想讓小車走的直可以一定程度上轉(zhuǎn)化成讓電機轉(zhuǎn)速一樣,那么現(xiàn)在速度知道了,我們自然想到PID控制
其實PID本身不難,而且arduino有pid庫(文末下載)
第一步:初始化&定義
這一部分放在setup()中

M1PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M1PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M2PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M2PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M3PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M3PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M4PID.SetMode(AUTOMATIC);//設(shè)置PID為自動模式
   M4PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M1PID.SetOutputLimits(-255,255);//PID輸出值設(shè)置為-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;//四個電機的參考轉(zhuǎn)速
double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脈沖的個數(shù)，M1-4S是轉(zhuǎn)換成轉(zhuǎn)速（沒有單位）后的值
double Kp=6, Ki=1.3, Kd=0.1;  //PID系數(shù)
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需要一個參考轉(zhuǎn)速,也就是電機我希望它達到這個轉(zhuǎn)速,p,i,d這三個參數(shù),M1S,M2S,M3S,M4S這四個實際速度,和pwmout,代表輸出的pwm
而65是什么?其實是讓電機達到最大轉(zhuǎn)速讀取它的轉(zhuǎn)速,這就是一個速度的最大值了 注意,正反兩方向的最大轉(zhuǎn)速需要取小的那個---------------------------------------------------------------------------------------------------------------------------------------