通过电机编码器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[]={,,,,,,,,,};//左侧速度档位
int Right_Speed[]={,,,,,,,,,};//右侧速度档位

int Left_Speed_Hold=;//定义左侧速度变量
int Right_Speed_Hold=;//定义右侧速度变量

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

byte STOP=;
//=============================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=;                          //定义定时器中断标志，用于判断是否发生中断

long counter_val_right[] = {,}; //定义数组，用于存放外部中断计数值
byte CurCnt_right = ;         //定义当前计数器标志，用于判断当前正在计数的数组
byte Direction_right=;
int rightSpeed=;
float rightPWM=0.0;

long counter_val_left[] = {,}; //定义数组，用于存放外部中断计数值
byte CurCnt_left = ;         //定义当前计数器标志，用于判断当前正在计数的数组
byte Direction_left=;
int leftSpeed = ;
float  leftPWM=0.0;
unsigned long lastPrintTime=;
//========================End PID=========================

void setup() {

 Serial.begin();//初始化波特率为115200
 initWifi();
 initIO();
 setCache(,);

  attachInterrupt(,counter_left,RISING);
  attachInterrupt(,counter_right, RISING);//设置中断方式为上升沿

  Timer1.initialize();// 设置定时器中断时间，单位微秒，此处为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>)leftPWM=;
 if(leftPWM<)leftPWM=;

 if(rightPWM>)rightPWM=;
 if(rightPWM<)rightPWM=;

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

void speedControl(){

  if(flag==)   //判断是否发生定时器中断，即定时时间是否到达
   {
     flag=;   //清除定时器中断标志位
    if((CurCnt_left&0x01) == ) //当前使用的是偶数计数器，则上次频率值存放在第二个元素中
     {
         leftSpeed =counter_val_left[];  //读取数组第二个元素中的数值
         counter_val_left[]=;       //读完清除原来的数值，以便下次使用
     }
     else   //当前使用的是奇数计数器，则上次频率值存放在第一个元素中
     {
       leftSpeed =counter_val_left[];  //读取数组第二个元素中的数值
       counter_val_left[]=;  //读完清除原来的数值，以便下次使用
     }

     if((CurCnt_right&0x01) == ) //当前使用的是偶数计数器，则上次频率值存放在第二个元素中
     {
         rightSpeed =counter_val_right[];  //读取数组第二个元素中的数值
         counter_val_right[]=;       //读完清除原来的数值，以便下次使用
     }
     else   //当前使用的是奇数计数器，则上次频率值存放在第一个元素中
     {
       rightSpeed =counter_val_right[];  //读取数组第二个元素中的数值
       counter_val_right[]=;  //读完清除原来的数值，以便下次使用
     }

   if(!STOP)  setSpeed();

    if((millis()-lastPrintTime)>){
       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] +=;
  Direction_left=digitalRead(D_Left_PIN);
}

void counter_right()
{
   //通过当前计数器来实现对外部中断计数值存储的切换
    counter_val_right[CurCnt_right& 0x01] += ;   //发生一次中断则加1
    Direction_right=digitalRead(D_Right_PIN);
}

//定时器中断处理函数
void timerIsr()
{
  flag=;     //置位定时器中断标志位
  CurCnt_right++; //当前计数器的值加1，实现另一个计数值切换
  CurCnt_left++;
}

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

  delayAndRead();
  Serial2.println("AT+CIOBAUD=38400");
  delayAndRead();
  Serial2.println("AT+RST");
  delayAndRead();
  Serial2.begin();
  Serial2.println("AT+CIPMUX=1");
  delayAndRead();
  Serial2.println("AT+CIPSERVER=1");
  delayAndRead();
  Serial2.println("AT+CIPSTO=60");
  delayAndRead(); 

}

void fire(long fireDelay){
  if(Fireing==)return;
  digitalWrite(FIREPIN,HIGH);
  Fireing=;
  FireStopTime=millis() + fireDelay;
  Serial.println(FIREPIN);
  Serial.println("fireDelay" + (String)fireDelay);
  Serial.println(FireStopTime);
}
void checkStopFire(){
      //check stop fire
    if(Fireing== && (FireStopTime <=millis())){
       Fireing=;
       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();
  Serial2.println(rStr);
 }else{
    rStr="$FEE-" + cmd +":" +msg +"$E";
    Serial2.println("AT+CIPSEND=0," + (String)rStr.length());
    delay();
    Serial2.println(rStr);

 } 

}

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

  int bIndex=bIndexOf("$F",);
  if(bIndex>=){
    int eIndex=bIndexOf("$E",bIndex);
    if(eIndex>bIndex){
       //Extra Data
       int dataLen= eIndex - (bIndex +);
       byte data[dataLen];
       for(int i=;i<dataLen;i++){
         data[i]= RecCache[bIndex+ +i] ;

       }

       for(int w=(eIndex +);w<CacheIndex && w<;w++){
          RecCache[w-(eIndex + )]=RecCache[w];
       }
       CacheIndex = CacheIndex - (eIndex +);
       setCache(CacheIndex,);

       lastReceiveTime=millis();
       handleCmd(data,dataLen);

    }
  }

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

}

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

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

    }
   if(flag) return true; 

   return false;
}

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

   for(int i=start;i<(CacheIndex +  -fLen);i++){

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

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[];
    unsigned long mlis=millis();
    long2byte(mlis,bs);
    Serial.println(mlis);
    replyBytes(true,"HB",bs,);
}

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

  byte duration=data[];
  if(duration>) duration=;
  if(duration<=)duration=;

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

void controlCmd(byte data[], int len){
  byte bs[]={,};
  bs[]=data[];bs[]=data[];
  byte isMatch=;

  if(data[]==0x01 && data[]==0x01){
    //Forward
    isMatch=;
    MOTOR_GO_FORWARD;
  }else if(data[]==0x01 && data[]==0x02){
    //Back
     isMatch=;
     MOTOR_GO_BACK;
  }else if(data[]==0x01 && data[]==0x03){
    //Turn Left
     isMatch=;
     MOTOR_GO_LEFT;
  }else if(data[]==0x01 && data[]==0x04){
    //Turn Right
     isMatch=;
     MOTOR_GO_RIGHT;
  }else if(data[]==0x01 && data[]==0x05){
    //Stop
     isMatch=;
     MOTOR_GO_STOP;
     Serial.println("Stop");
  }else if(data[]==0x02 && data[]==0x01){
    //Left Speed
     isMatch=;
     byte ena=data[];
     if(ena>= && ena<=){
        Left_Speed_Hold=ena;
     }

  }else if(data[]==0x02 && data[]==0x02){
    //Right Speed
     isMatch=;
     byte enb=data[];
     if(enb>= && enb<=){
        Right_Speed_Hold=enb;
     }

  }
  if(isMatch==)replyBytes(true,"C",bs,); 

}

 void long2byte(unsigned long res,byte targets[] ) {
        targets[] = (byte) (res & 0xff);// 鏈�浣庝綅
        targets[] = (byte) ((res >> ) & 0xff);// 娆′綆浣�
        targets[] = (byte) ((res >> ) & 0xff);// 娆￠珮浣�
        targets[] = (byte) (res >> );// 鏈�楂樹綅,鏃犵鍙峰彸绉汇��  

    }  

unsigned long bytes2long(byte buf[]) {
        unsigned long firstByte = ;
        unsigned long secondByte = ;
        unsigned long thirdByte = ;
        unsigned long fourthByte = ;
        int index = ;
        firstByte = (0x000000FFU & ( buf[index+]));
        secondByte = (0x000000FFU & ( buf[index + ]));
        thirdByte = (0x000000FFU & ( buf[index + ]));
        fourthByte = (0x000000FFU & ( buf[index ]));
        index = index + ;
        return ((unsigned long) (firstByte <<  | secondByte <<  | thirdByte <<  | fourthByte)) & 0xFFFFFFFFUL;
    }