下位机代码
#include <avr/wdt.h> #include <SoftwareSerial.h> #include <EEPROM.h> //设置系统启动指示灯 #define LED 13 #define ENA 5 #define ENB 6 #define INPUT2 7 #define INPUT1 8 #define INPUT3 12 #define INPUT4 13 #define FIREPIN 9 #define MOTOR_GO_FORWARD {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);} #define MOTOR_GO_BACK {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);} #define MOTOR_GO_RIGHT {digitalWrite(INPUT1,HIGH);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,HIGH);} #define MOTOR_GO_LEFT {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,HIGH);digitalWrite(INPUT3,HIGH);digitalWrite(INPUT4,LOW);} #define MOTOR_GO_STOP {digitalWrite(INPUT1,LOW);digitalWrite(INPUT2,LOW);digitalWrite(INPUT3,LOW);digitalWrite(INPUT4,LOW);} SoftwareSerial mySerial(10, 11); // RX, TX int Left_Speed[10]={10,122,138,154,170,186,203,218,234,255};//左侧速度档位 int Right_Speed[10]={10,122,138,154,170,186,203,218,234,255};//右侧速度档位 int Left_Speed_Hold=8;//定义左侧速度变量 int Right_Speed_Hold=8;//定义右侧速度变量 byte Fireing=0; long FireStopTime=0; unsigned long lastSendTime=0; unsigned long lastReceiveTime=0; byte RecCache[512]; volatile int CacheIndex=0; void setup() { // put your setup code here, to run once: Serial.begin(19200); initWifi(); initIO(); setCache(0,512); } void loop() { // put your main code here, to run repeatedly: handleTXR(); checkStopFire(); } void initWifi(){ mySerial.begin(115200); delayAndRead(100); mySerial.println("AT+CIOBAUD=19200"); delayAndRead(100); mySerial.println("AT+RST"); delayAndRead(3000); mySerial.begin(19200); mySerial.println("AT+CIPMUX=1"); delayAndRead(500); mySerial.println("AT+CIPSERVER=1"); delayAndRead(200); mySerial.println("AT+CIPSTO=60"); delayAndRead(300); } void initIO() { pinMode(LED,OUTPUT); pinMode(ENA,OUTPUT); pinMode(ENB,OUTPUT); pinMode(INPUT1,OUTPUT); pinMode(INPUT2,OUTPUT); pinMode(INPUT3,OUTPUT); pinMode(INPUT4,OUTPUT); pinMode(FIREPIN,OUTPUT); digitalWrite(FIREPIN,LOW); for(int i=0;i<10;i++) { digitalWrite(LED,LOW); delay(500); digitalWrite(LED,HIGH); delay(500); } digitalWrite(LED,LOW); //half speed analogWrite(ENA,Left_Speed[Left_Speed_Hold]); analogWrite(ENB,Right_Speed[Right_Speed_Hold]); } 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"; mySerial.println("AT+CIPSEND=0," + (String)rStr.length()); delay(10); mySerial.println(rStr); }else{ rStr="$FEE-" + cmd +":" +msg +"$E"; mySerial.println("AT+CIPSEND=0," + (String)rStr.length()); delay(10); mySerial.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" mySerial.println("AT+CIPSEND=0," + (String)sendLen); delay(10); mySerial.print(rStr); for(int i=0;i<msgLen;i++){ mySerial.write(msg[i]); } mySerial.println("$E"); }else{ rStr="$FEE-" + cmd +":" ; sendLen= rStr.length() + msgLen +2; //2 is "$E" mySerial.println("AT+CIPSEND=0," + (String)sendLen); delay(10); mySerial.print(rStr); for(int i=0;i<msgLen;i++){ mySerial.write(msg[i]); } mySerial.println("$E"); } } void cls(){ while(mySerial.available()){ mySerial.read(); } } void delayAndRead(int waitTime){ delay(waitTime); while(mySerial.available()){ Serial.write(mySerial.read()); } } void handleTXR(){ while(mySerial.available()){ byte c=(byte) mySerial.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){ // Serial.println(" ============="); // for(int i=0;i<len;i++){ // Serial.print(data[i],DEC); // Serial.print(","); // } 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; analogWrite(ENA,Left_Speed[Left_Speed_Hold]); } }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; analogWrite(ENB,Right_Speed[Right_Speed_Hold]); } } 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; }
上位机Android，参考QQ云盘