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 Errorsfloat right_LastError=0.0; // Error[-1] float right_SumError=0.0; // Sums of Errorsint 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 ConstleftP=(Left_Speed[Left_Speed_Hold]- leftSpeed);rightP=(Right_Speed[Right_Speed_Hold] - rightSpeed);//積分常數 Integral Constleft_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:"); //發送幀頭大寫SSerial.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; //發生一次中斷則加1Direction_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 fireif(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 Dataint 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){//ForwardisMatch=1;MOTOR_GO_FORWARD;}else if(data[2]==0x01 && data[3]==0x02){//BackisMatch=1;MOTOR_GO_BACK;}else if(data[2]==0x01 && data[3]==0x03){//Turn LeftisMatch=1;MOTOR_GO_LEFT;}else if(data[2]==0x01 && data[3]==0x04){//Turn RightisMatch=1;MOTOR_GO_RIGHT;}else if(data[2]==0x01 && data[3]==0x05){//StopisMatch=1;MOTOR_GO_STOP;Serial.println("Stop");}else if(data[2]==0x02 && data[3]==0x01){//Left SpeedisMatch=1;byte ena=data[4];if(ena>=0 && ena<=9){Left_Speed_Hold=ena; }}else if(data[2]==0x02 && data[3]==0x02){//Right SpeedisMatch=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; } View Code

?

轉載于:https://www.cnblogs.com/wdfrog/p/5144008.html

總結

以上是生活随笔為你收集整理的通过电机编码器AB相输出确定电机转向的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。