Wifi小车资料

下位机代码

#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云盘