#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 Errors
float right_LastError=0.0; // Error[-1]
float right_SumError=0.0; // Sums of Errors
int 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<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 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){
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;
}
}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;
}
