#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;  }