通過電機編碼器AB相輸出確定電機轉向

AB相輸出相差90度，即當A相"正跳變"時如果B相是高電平那么是"正轉"，反之是"反轉"

 

圖片:

正轉

反轉

#include <TimerOne.h>
#define D_Left_PIN 7  
#define D_Right_PIN 8
#define IN1 22
#define IN2 23
#define IN3 24
#define IN4 25
#define ENA 5
#define ENB 13
#define FIREPIN 9
#define Kp 0.5
#define Ki 0.5
#define Kd 0.0

#define MOTOR_GO_FORWARD  {STOP=0; digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);}                                    
#define MOTOR_GO_BACK      {STOP=0; digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);}    
#define MOTOR_GO_RIGHT      {STOP=0; digitalWrite(IN1,HIGH);digitalWrite(IN2,LOW);digitalWrite(IN3,LOW);digitalWrite(IN4,HIGH);}    
#define MOTOR_GO_LEFT      {STOP=0; digitalWrite(IN1,LOW);digitalWrite(IN2,HIGH);digitalWrite(IN3,HIGH);digitalWrite(IN4,LOW);}   
#define MOTOR_GO_STOP      {STOP=1; digitalWrite(IN1,LOW);digitalWrite(IN2,LOW);digitalWrite(IN3,LOW);digitalWrite(IN4,LOW);}    


int Left_Speed[10]={20,30,35,40,45,50,55,60,65,70};//左側速度檔位
int Right_Speed[10]={20,30,35,40,45,50,55,60,65,70};//右側速度檔位

int Left_Speed_Hold=5;//定義左側速度變量
int Right_Speed_Hold=5;//定義右側速度變量

byte Fireing=0;
long FireStopTime=0;
unsigned long lastSendTime=0;
unsigned long lastReceiveTime=0;
byte RecCache[512];
volatile int CacheIndex=0;

byte STOP=1;
//=============================PID Args===========================
float  left_LastError=0.0;          //  Error[-1]
float  left_SumError=0.0;           //  Sums of Errors
 
float  right_LastError=0.0;          //  Error[-1]
float  right_SumError=0.0;           //  Sums of Errors

int flag=0;                          //定義定時器中斷標志，用於判斷是否發生中斷

long counter_val_right[2] = {0,0}; //定義數組，用於存放外部中斷計數值
byte CurCnt_right = 0;         //定義當前計數器標志，用於判斷當前正在計數的數組
byte Direction_right=2;
int rightSpeed=0;
float rightPWM=0.0;

long counter_val_left[2] = {0,0}; //定義數組，用於存放外部中斷計數值
byte CurCnt_left = 0;         //定義當前計數器標志，用於判斷當前正在計數的數組
byte Direction_left=2;
int leftSpeed = 0; 
float  leftPWM=0.0; 
unsigned long lastPrintTime=0;
//========================End PID=========================


void setup() {

 Serial.begin(38400);//初始化波特率為115200
 initWifi();
 initIO();
 setCache(0,512);

  attachInterrupt(0,counter_left,RISING);
  attachInterrupt(1,counter_right, RISING);//設置中斷方式為上升沿
 
  Timer1.initialize(100000);// 設置定時器中斷時間，單位微秒，此處為1秒
  Timer1.attachInterrupt( timerIsr ); // 打開定時器中斷
  interrupts();  //打開外部中斷
}
void initIO(){
  pinMode(D_Left_PIN,INPUT);
  pinMode(D_Right_PIN,INPUT);
  pinMode(IN1,OUTPUT);
  pinMode(IN2,OUTPUT);
  pinMode(IN3,OUTPUT);
  pinMode(IN4,OUTPUT);
  pinMode(ENA,OUTPUT);
  pinMode(ENB,OUTPUT);
  pinMode(FIREPIN,OUTPUT);
  digitalWrite(IN1,LOW);
  digitalWrite(IN2,LOW);
  digitalWrite(IN3,LOW);
  digitalWrite(IN4,LOW);
  
  digitalWrite(FIREPIN,LOW);

}
void loop()
{
  handleTXR();
  checkStopFire();
  speedControl();
   
}



void setSpeed(){
  
  float leftP=0.0,rightP=0.0,leftD=0.0,rightD=0.0;
  
  //  比例常數 Proportional Const
  leftP=(Left_Speed[Left_Speed_Hold]- leftSpeed);
  rightP=(Right_Speed[Right_Speed_Hold]  - rightSpeed);
  
  //積分常數 Integral Const
  left_SumError +=leftP;
  right_SumError+=rightP;
  
  //微分常數 Derivative Const
//  leftD=leftP-left_LastError;
//  rightD=rightP-right_LastError;
//  left_LastError=leftD;
//  right_LastError=rightD;
// leftPWM=(leftP * Kp) + (left_SumError * Ki) +(leftD * Kd);
// rightPWM=((rightP) * Kp) + (right_SumError * Ki) +(rightD * Kd) ; 
 
 leftPWM=(leftP * Kp) + (left_SumError * Ki) ;
 rightPWM=((rightP) * Kp) + (right_SumError * Ki)  ; 

 if(leftPWM>255)leftPWM=255;
 if(leftPWM<0)leftPWM=0;
 
 if(rightPWM>255)rightPWM=255;
 if(rightPWM<0)rightPWM=0;

 analogWrite(ENA,rightPWM);
 analogWrite(ENB,leftPWM);
}

void speedControl(){

  if(flag==1)   //判斷是否發生定時器中斷，即定時時間是否到達
   {
     flag=0;   //清除定時器中斷標志位
    if((CurCnt_left&0x01) == 0) //當前使用的是偶數計數器，則上次頻率值存放在第二個元素中
     {
         leftSpeed =counter_val_left[1];  //讀取數組第二個元素中的數值
         counter_val_left[1]=0;       //讀完清除原來的數值，以便下次使用
     }
     else   //當前使用的是奇數計數器，則上次頻率值存放在第一個元素中
     {
       leftSpeed =counter_val_left[0];  //讀取數組第二個元素中的數值
       counter_val_left[0]=0;  //讀完清除原來的數值，以便下次使用
     }
     
     if((CurCnt_right&0x01) == 0) //當前使用的是偶數計數器，則上次頻率值存放在第二個元素中
     {
         rightSpeed =counter_val_right[1];  //讀取數組第二個元素中的數值
         counter_val_right[1]=0;       //讀完清除原來的數值，以便下次使用
     }
     else   //當前使用的是奇數計數器，則上次頻率值存放在第一個元素中
     {
       rightSpeed =counter_val_right[0];  //讀取數組第二個元素中的數值
       counter_val_right[0]=0;  //讀完清除原來的數值，以便下次使用
     }
     
     
   if(!STOP)  setSpeed();
    
    
    if((millis()-lastPrintTime)>500){
       Serial.print("L:");    //發送幀頭大寫S
       Serial.print( leftSpeed);  //發送頻率數據，並回車換行
       Serial.print(",");
       Serial.print(Direction_left);
       Serial.print(",R:");
       Serial.print(rightSpeed);
       Serial.print(",");
       Serial.print(Direction_right);
       Serial.println("");
        Serial.println(leftPWM);
        Serial.println(rightPWM);
       lastPrintTime=millis();
      }
     } 
}
//外部中斷處理函數

void counter_left(){
  counter_val_left[CurCnt_left & 0x01] +=1;
  Direction_left=digitalRead(D_Left_PIN);
}

void counter_right()
{
   //通過當前計數器來實現對外部中斷計數值存儲的切換
    counter_val_right[CurCnt_right& 0x01] += 1;   //發生一次中斷則加1
    Direction_right=digitalRead(D_Right_PIN);
}

//定時器中斷處理函數
void timerIsr()
{
  flag=1;     //置位定時器中斷標志位
  CurCnt_right++; //當前計數器的值加1，實現另一個計數值切換
  CurCnt_left++;
}

//===================End PID ===============
void  initWifi(){
   Serial2.begin(115200);

  delayAndRead(100);
  Serial2.println("AT+CIOBAUD=38400");
  delayAndRead(100);
  Serial2.println("AT+RST");
  delayAndRead(3000);
  Serial2.begin(38400);
  Serial2.println("AT+CIPMUX=1");
  delayAndRead(500);
  Serial2.println("AT+CIPSERVER=1");
  delayAndRead(200);
  Serial2.println("AT+CIPSTO=60");
  delayAndRead(300); 
  
}

void fire(long fireDelay){
  if(Fireing==1)return;
  digitalWrite(FIREPIN,HIGH);
  Fireing=1;
  FireStopTime=millis() + fireDelay;
  Serial.println(FIREPIN);
  Serial.println("fireDelay" + (String)fireDelay);
  Serial.println(FireStopTime);
}
void checkStopFire(){
      //check stop fire
    if(Fireing==1 && (FireStopTime <=millis())){
       Fireing=0;
       digitalWrite(FIREPIN,LOW);
    }
}

void reply(bool isOk,String cmd,String msg){
 String rStr="";
 if(isOk){
  rStr="$FOK-" + cmd +":" +msg +"$E";
  Serial2.println("AT+CIPSEND=0," + (String)rStr.length());  
  delay(10);
  Serial2.println(rStr);
 }else{
    rStr="$FEE-" + cmd +":" +msg +"$E";
    Serial2.println("AT+CIPSEND=0," + (String)rStr.length());  
    delay(10);
    Serial2.println(rStr);
    
 } 
  
}

void replyBytes(bool isOk,String cmd,byte msg[],int msgLen){
 String rStr="";
 int  sendLen=0;
 if(isOk){
  rStr="$FOK-" + cmd +":" ;
    sendLen= rStr.length() + msgLen +2; //2 is "$E"
  Serial2.println("AT+CIPSEND=0," + (String)sendLen);  
  delay(10);
  Serial2.print(rStr);
  for(int i=0;i<msgLen;i++){
    Serial2.write(msg[i]);
  }
  Serial2.println("$E");
 }else{
  rStr="$FEE-" + cmd +":" ;
  sendLen= rStr.length() + msgLen +2; //2 is "$E"
  Serial2.println("AT+CIPSEND=0," + (String)sendLen);  
  delay(10);
  Serial2.print(rStr);
  for(int i=0;i<msgLen;i++){
    Serial2.write(msg[i]);
  }
  Serial2.println("$E");
 } 
  
}

void cls(){
   while(Serial2.available()){
     Serial2.read();
  }
}

void delayAndRead(int waitTime){
   delay(waitTime);
   while(Serial2.available()){
     Serial.write(Serial2.read()); 
   }
}


void handleTXR(){

  
  while(Serial2.available()){
    byte c=(byte) Serial2.read();
    RecCache[CacheIndex++]=c;

  }
   if(CacheIndex>512){
      CacheIndex=0;
      setCache(0,512); 
      Serial.println("Cut");
    }
    
  int bIndex=bIndexOf("$F",0);
  if(bIndex>=0){
    int eIndex=bIndexOf("$E",bIndex);
    if(eIndex>bIndex){
       //Extra Data
       int dataLen= eIndex - (bIndex +2);
       byte data[dataLen];
       for(int i=0;i<dataLen;i++){
         data[i]= RecCache[bIndex+2 +i] ;

       }

       for(int w=(eIndex +2);w<CacheIndex && w<512;w++){
          RecCache[w-(eIndex + 2)]=RecCache[w]; 
       }
       CacheIndex = CacheIndex - (eIndex +2);
       setCache(CacheIndex,512);
       
       lastReceiveTime=millis();
       handleCmd(data,dataLen);

    }
  }
  
    if(CacheIndex>512){
      CacheIndex=0;
      setCache(0,512); 
    }
  
}

void setCache(int start,int endIndex){
  for(int i=start;i<endIndex;i++){
     RecCache[i]=0;
  }  
}
boolean bStartsWith(byte data[],int len, String fStr){
    int fLen=fStr.length() ;
   byte fBytes[fLen + 1];
   fStr.getBytes(fBytes,fLen+1);

   if(len<=0)return false;
   if(len<fLen)return false;
   byte flag=1;
   for(int j=0;j<fLen;j++){
       if(fBytes[j]!=data[j])
       {
                flag=0;
                break; 
        }
              
    }
   if(flag) return true; 
  
   return false;
}

int bIndexOf(String fStr,int start){
    int fLen=fStr.length() ;
   byte fBytes[fLen + 1];
   fStr.getBytes(fBytes,fLen+1);

   for(int i=start;i<(CacheIndex + 1 -fLen);i++){
  
       if(RecCache[i]==fBytes[0])
       {
         byte flag=1;
          for(int j=1;j<fLen;j++){
              if(fBytes[j]!=RecCache[i+j])
              {
                flag=0;
                break; 
              }
          }
          if(flag)return i;
       }
   }
   return -1;
}

void handleCmd(byte data[], int len){


  
 if(bStartsWith(data,len,"HB:")){
     hbCmd();
 }else if(bStartsWith(data,len,"F:")){
    fireCmd(data,len);
 }else if(bStartsWith(data,len,"C:")){
    controlCmd(data,len);
 }
}

void hbCmd(){
    byte bs[4];
    unsigned long mlis=millis();
    long2byte(mlis,bs);
    Serial.println(mlis);
    replyBytes(true,"HB",bs,4); 
}

void fireCmd(byte data[], int len){


  byte duration=data[2];
  if(duration>15) duration=15;
  if(duration<=0)duration=1;
  

  
  long longDuration= duration * 1000;
  fire(longDuration);
  reply(true,"F","");
}

void controlCmd(byte data[], int len){
  byte bs[2]={0,0};
  bs[0]=data[2];bs[1]=data[3];
  byte isMatch=0;
  
  if(data[2]==0x01 && data[3]==0x01){
    //Forward
    isMatch=1;
    MOTOR_GO_FORWARD;
  }else if(data[2]==0x01 && data[3]==0x02){
    //Back
     isMatch=1;
     MOTOR_GO_BACK;
  }else if(data[2]==0x01 && data[3]==0x03){
    //Turn Left
     isMatch=1;
     MOTOR_GO_LEFT;
  }else if(data[2]==0x01 && data[3]==0x04){
    //Turn Right
     isMatch=1;
     MOTOR_GO_RIGHT;
  }else if(data[2]==0x01 && data[3]==0x05){
    //Stop
     isMatch=1;
     MOTOR_GO_STOP;
     Serial.println("Stop");
  }else if(data[2]==0x02 && data[3]==0x01){
    //Left Speed
     isMatch=1;
     byte ena=data[4];
     if(ena>=0 && ena<=9){
        Left_Speed_Hold=ena; 
     }
     
  }else if(data[2]==0x02 && data[3]==0x02){
    //Right Speed
     isMatch=1;
     byte enb=data[4];
     if(enb>=0 && enb<=9){
        Right_Speed_Hold=enb;
     }
     
  }
  if(isMatch==1)replyBytes(true,"C",bs,2); 
  
}

 void long2byte(unsigned long res,byte targets[] ) {  
        targets[0] = (byte) (res & 0xff);// 鏈�浣庝綅  
        targets[1] = (byte) ((res >> 8) & 0xff);// 嬈′綆浣�  
        targets[2] = (byte) ((res >> 16) & 0xff);// 嬈￠珮浣�  
        targets[3] = (byte) (res >> 24);// 鏈�楂樹綅,鏃犵鍙峰彸縐匯��  
              
    }  
    
unsigned long bytes2long(byte buf[]) {  
        unsigned long firstByte = 0;  
        unsigned long secondByte = 0;  
        unsigned long thirdByte = 0;  
        unsigned long fourthByte = 0;  
        int index = 0;  
        firstByte = (0x000000FFU & ( buf[index+3]));  
        secondByte = (0x000000FFU & ( buf[index + 2]));  
        thirdByte = (0x000000FFU & ( buf[index + 1]));  
        fourthByte = (0x000000FFU & ( buf[index ]));  
        index = index + 4;  
        return ((unsigned long) (firstByte << 24 | secondByte << 16 | thirdByte << 8 | fourthByte)) & 0xFFFFFFFFUL;  
    }