欧美国产综合_久久精品国产久久男男 -婷婷久久亚洲综合国产综合区在线视频 |国产精品自在在线午夜福利

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

(1)程序定義部分

(2)程序初始化部分

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

(4)sbus接收機(jī)信號(hào)讀取與處理

(5)電機(jī)驅(qū)動(dòng)與控制

(6)PID調(diào)制

機(jī)甲大師代碼詳解:

一 定義部分

#include 
#include 
#include
FutabaSBUS sbus;
Servo yaw;
Servo roll;
int yawval,rollval;
int rcsig[25];
const int ma1=9;//電機(jī)A接口9
const int ma2=2;//電機(jī)A接口2
const int mb1=3;//電機(jī)B接口1
const int mb2=4;//電機(jī)B接口2
const int mc1=5;//以此類推
const int mc2=6;
const int md1=7;
const int md2=8;//電機(jī)D接口2
//-----------------上面定義電機(jī)PWM信號(hào)輸出口
const int ma_in=20;//電機(jī)A轉(zhuǎn)速輸入口
const int mb_in=19;//電機(jī)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電機(jī)的旋轉(zhuǎn),平移,前后值
const int L=1015,R=1035;
double M1PWMOUT,M2PWMOUT,M3PWMOUT,M4PWMOUT;//M1輸出pwm，M2輸出pwm，以此類推
double ref1,ref2,ref3,ref4;//四個(gè)電機(jī)的參考轉(zhuǎn)速
double in1,in2,in3,in4,M1S,M2S,M3S,M4S;//in1-4是脈沖的個(gè)數(shù)，M1-4S是轉(zhuǎn)換成轉(zhuǎn)速（沒(méi)有單位）后的值
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信號(hào)的頭文件,pid的頭文件,以及舵機(jī)的頭文件

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

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

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

而B(niǎo)相定義為Q1,Q2,Q3,Q4,電機(jī)轉(zhuǎn)動(dòng)的方向定義為rev1,rev2,rev3,rev4

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

代表4電機(jī)的旋轉(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ā)信號(hào)必須是變化的，上升或下降皆可（外部中斷讀轉(zhuǎn)速）
   attachInterrupt(digitalPinToInterrupt(mb_in), mbcount, FALLING);//觸發(fā)信號(hào)必須是變化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(mc_in), mccount, FALLING);//觸發(fā)信號(hào)必須是變化的，上升或下降皆可
   attachInterrupt(digitalPinToInterrupt(md_in), mdcount, FALLING);//觸發(fā)信號(hào)必須是變化的，上升或下降皆可
   M1PID.SetMode(AUTOMATIC);//設(shè)置PID為自動(dòng)模式
   M1PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M2PID.SetMode(AUTOMATIC);//設(shè)置PID為自動(dòng)模式
   M2PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M3PID.SetMode(AUTOMATIC);//設(shè)置PID為自動(dòng)模式
   M3PID.SetSampleTime(30);//設(shè)置PID采樣頻率為100ms
   M4PID.SetMode(AUTOMATIC);//設(shè)置PID為自動(dòng)模式
   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ā)要做的操作,將計(jì)數(shù)變量+1,并判斷B相來(lái)賦值1,0給rev代表電機(jī)轉(zhuǎn)向

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

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

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

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

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

于是有了這樣的代碼

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

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代表電機(jī)正轉(zhuǎn),那么轉(zhuǎn)速為正,否則為負(fù)

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

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

(別看這個(gè)點(diǎn)很簡(jiǎn)單,我花了很多時(shí)間研究這個(gè)東西)

有懂航模的朋友就要問(wèn)了 為什么不直接讀取pwm輸出的接收機(jī) 實(shí)話告訴你,我一開(kāi)始就是用的pwm接收機(jī),因?yàn)樗a簡(jiǎn)單,讀取方便

但是它有個(gè)致命缺點(diǎn):會(huì)被外部中斷干擾,而且讀取一下要2ms,并且精度低

因?yàn)閜wm接收機(jī)是直接插舵機(jī)的,相當(dāng)于接收機(jī)先接受信號(hào),再轉(zhuǎn)換成pwm輸出,單片機(jī)再計(jì)時(shí)讀取,繞了個(gè)大圈

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

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

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

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

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

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

sbus.receive();

第三步:然后有這行代碼,代表我們要把讀取的遙控?cái)?shù)據(jù)放到rcsig數(shù)組里面,17,18是兩個(gè)數(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;
}

硬件: 硬件部分會(huì)復(fù)雜一些,因?yàn)閟bus的信號(hào)其實(shí)是反的,我們要手工做一個(gè)電平取反器

  如上圖，使用S8050三極管反向信號(hào)

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

上圖是反向前的sbus信號(hào)

經(jīng)過(guò)反向的sbus信號(hào)

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

五 電機(jī)控制

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

常見(jiàn)的控制方法是判斷遙控?cái)?shù)據(jù)如果低于某個(gè)值就后退,高于某個(gè)值就前進(jìn),但是這樣并不順滑,不適合用于機(jī)甲大師.  這種方法的好處不需要知道電機(jī)的轉(zhuǎn)向,因?yàn)榧热晃乙呀?jīng)分類前進(jìn)后退了,就自然知道轉(zhuǎn)向了

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

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

于是就想到了這樣的方法,每個(gè)電機(jī)對(duì)應(yīng)一個(gè)前進(jìn)后退方向的值,左右平移方向的值,旋轉(zhuǎn)的值,三個(gè)值由遙控器的數(shù)據(jù)算出,疊加后輸出給電機(jī),這樣電機(jī)轉(zhuǎn)的會(huì)更加平滑!因?yàn)閾Q向,變速不需要再經(jīng)過(guò)一個(gè)遙控?cái)?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ù)怎么寫呢?其實(shí)就是把pwm輸出給電機(jī)
void moving()//移動(dòng)
{
  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)制

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

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