AB相输出相差90度，即当A相"正跳变"时如果B相是高电平那么是"正转"，反之是"反转"
图片:
![]()
正转
![]()
反转
![]()
#include #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 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 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); } 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); } 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= RecCache[bIndex+2 +i] ; } for(int w=(eIndex +2);w512){ CacheIndex=0; setCache(0,512); } }void setCache(int start,int endIndex){ for(int i=start;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==fBytes[0]) { byte flag=1; for(int j=1;j15) 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; }
|
|
|
|
2021-10-21 13:42:21
评论
举报
|
|
|
|
电子发烧友论坛
/6 
工商网监
湘ICP备2023018690号
4116
淘帖
2546
