#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[]={,,,,,,,,,};//左侧速度档位
int Right_Speed[]={,,,,,,,,,};//右侧速度档位
int Left_Speed_Hold=;//定义左侧速度变量
int Right_Speed_Hold=;//定义右侧速度变量
byte Fireing=;
long FireStopTime=;
unsigned long lastSendTime=;
unsigned long lastReceiveTime=;
byte RecCache[];
volatile int CacheIndex=;
byte STOP=;
//=============================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=; //定义定时器中断标志，用于判断是否发生中断
long counter_val_right[] = {,}; //定义数组，用于存放外部中断计数值
byte CurCnt_right = ; //定义当前计数器标志，用于判断当前正在计数的数组
byte Direction_right=;
int rightSpeed=;
float rightPWM=0.0;
long counter_val_left[] = {,}; //定义数组，用于存放外部中断计数值
byte CurCnt_left = ; //定义当前计数器标志，用于判断当前正在计数的数组
byte Direction_left=;
int leftSpeed = ;
float leftPWM=0.0;
unsigned long lastPrintTime=;
//========================End PID=========================
void setup() {
Serial.begin();//初始化波特率为115200
initWifi();
initIO();
setCache(,);
attachInterrupt(,counter_left,RISING);
attachInterrupt(,counter_right, RISING);//设置中断方式为上升沿
Timer1.initialize();// 设置定时器中断时间，单位微秒，此处为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>)leftPWM=;
if(leftPWM<)leftPWM=;
if(rightPWM>)rightPWM=;
if(rightPWM<)rightPWM=;
analogWrite(ENA,rightPWM);
analogWrite(ENB,leftPWM);
}
void speedControl(){
if(flag==) //判断是否发生定时器中断，即定时时间是否到达
{
flag=; //清除定时器中断标志位
if((CurCnt_left&0x01) == ) //当前使用的是偶数计数器，则上次频率值存放在第二个元素中
{
leftSpeed =counter_val_left[]; //读取数组第二个元素中的数值
counter_val_left[]=; //读完清除原来的数值，以便下次使用
}
else //当前使用的是奇数计数器，则上次频率值存放在第一个元素中
{
leftSpeed =counter_val_left[]; //读取数组第二个元素中的数值
counter_val_left[]=; //读完清除原来的数值，以便下次使用
}
if((CurCnt_right&0x01) == ) //当前使用的是偶数计数器，则上次频率值存放在第二个元素中
{
rightSpeed =counter_val_right[]; //读取数组第二个元素中的数值
counter_val_right[]=; //读完清除原来的数值，以便下次使用
}
else //当前使用的是奇数计数器，则上次频率值存放在第一个元素中
{
rightSpeed =counter_val_right[]; //读取数组第二个元素中的数值
counter_val_right[]=; //读完清除原来的数值，以便下次使用
}
if(!STOP) setSpeed();
if((millis()-lastPrintTime)>){
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] +=;
Direction_left=digitalRead(D_Left_PIN);
}
void counter_right()
{
//通过当前计数器来实现对外部中断计数值存储的切换
counter_val_right[CurCnt_right& 0x01] += ; //发生一次中断则加1
Direction_right=digitalRead(D_Right_PIN);
}
//定时器中断处理函数
void timerIsr()
{
flag=; //置位定时器中断标志位
CurCnt_right++; //当前计数器的值加1，实现另一个计数值切换
CurCnt_left++;
}
//===================End PID ===============
void initWifi(){
Serial2.begin();
delayAndRead();
Serial2.println("AT+CIOBAUD=38400");
delayAndRead();
Serial2.println("AT+RST");
delayAndRead();
Serial2.begin();
Serial2.println("AT+CIPMUX=1");
delayAndRead();
Serial2.println("AT+CIPSERVER=1");
delayAndRead();
Serial2.println("AT+CIPSTO=60");
delayAndRead();
}
void fire(long fireDelay){
if(Fireing==)return;
digitalWrite(FIREPIN,HIGH);
Fireing=;
FireStopTime=millis() + fireDelay;
Serial.println(FIREPIN);
Serial.println("fireDelay" + (String)fireDelay);
Serial.println(FireStopTime);
}
void checkStopFire(){
//check stop fire
if(Fireing== && (FireStopTime <=millis())){
Fireing=;
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();
Serial2.println(rStr);
}else{
rStr="$FEE-" + cmd +":" +msg +"$E";
Serial2.println("AT+CIPSEND=0," + (String)rStr.length());
delay();
Serial2.println(rStr);
}
}
void replyBytes(bool isOk,String cmd,byte msg[],int msgLen){
String rStr="";
int sendLen=;
if(isOk){
rStr="$FOK-" + cmd +":" ;
sendLen= rStr.length() + msgLen +; //2 is "$E"
Serial2.println("AT+CIPSEND=0," + (String)sendLen);
delay();
Serial2.print(rStr);
for(int i=;i<msgLen;i++){
Serial2.write(msg[i]);
}
Serial2.println("$E");
}else{
rStr="$FEE-" + cmd +":" ;
sendLen= rStr.length() + msgLen +; //2 is "$E"
Serial2.println("AT+CIPSEND=0," + (String)sendLen);
delay();
Serial2.print(rStr);
for(int i=;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>){
CacheIndex=;
setCache(,);
Serial.println("Cut");
}
int bIndex=bIndexOf("$F",);
if(bIndex>=){
int eIndex=bIndexOf("$E",bIndex);
if(eIndex>bIndex){
//Extra Data
int dataLen= eIndex - (bIndex +);
byte data[dataLen];
for(int i=;i<dataLen;i++){
data[i]= RecCache[bIndex+ +i] ;
}
for(int w=(eIndex +);w<CacheIndex && w<;w++){
RecCache[w-(eIndex + )]=RecCache[w];
}
CacheIndex = CacheIndex - (eIndex +);
setCache(CacheIndex,);
lastReceiveTime=millis();
handleCmd(data,dataLen);
}
}
if(CacheIndex>){
CacheIndex=;
setCache(,);
}
}
void setCache(int start,int endIndex){
for(int i=start;i<endIndex;i++){
RecCache[i]=;
}
}
boolean bStartsWith(byte data[],int len, String fStr){
int fLen=fStr.length() ;
byte fBytes[fLen + ];
fStr.getBytes(fBytes,fLen+);
if(len<=)return false;
if(len<fLen)return false;
byte flag=;
for(int j=;j<fLen;j++){
if(fBytes[j]!=data[j])
{
flag=;
break;
}
}
if(flag) return true;
return false;
}
int bIndexOf(String fStr,int start){
int fLen=fStr.length() ;
byte fBytes[fLen + ];
fStr.getBytes(fBytes,fLen+);
for(int i=start;i<(CacheIndex + -fLen);i++){
if(RecCache[i]==fBytes[])
{
byte flag=;
for(int j=;j<fLen;j++){
if(fBytes[j]!=RecCache[i+j])
{
flag=;
break;
}
}
if(flag)return i;
}
}
return -;
}
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[];
unsigned long mlis=millis();
long2byte(mlis,bs);
Serial.println(mlis);
replyBytes(true,"HB",bs,);
}
void fireCmd(byte data[], int len){
byte duration=data[];
if(duration>) duration=;
if(duration<=)duration=;
long longDuration= duration * ;
fire(longDuration);
reply(true,"F","");
}
void controlCmd(byte data[], int len){
byte bs[]={,};
bs[]=data[];bs[]=data[];
byte isMatch=;
if(data[]==0x01 && data[]==0x01){
//Forward
isMatch=;
MOTOR_GO_FORWARD;
}else if(data[]==0x01 && data[]==0x02){
//Back
isMatch=;
MOTOR_GO_BACK;
}else if(data[]==0x01 && data[]==0x03){
//Turn Left
isMatch=;
MOTOR_GO_LEFT;
}else if(data[]==0x01 && data[]==0x04){
//Turn Right
isMatch=;
MOTOR_GO_RIGHT;
}else if(data[]==0x01 && data[]==0x05){
//Stop
isMatch=;
MOTOR_GO_STOP;
Serial.println("Stop");
}else if(data[]==0x02 && data[]==0x01){
//Left Speed
isMatch=;
byte ena=data[];
if(ena>= && ena<=){
Left_Speed_Hold=ena;
}
}else if(data[]==0x02 && data[]==0x02){
//Right Speed
isMatch=;
byte enb=data[];
if(enb>= && enb<=){
Right_Speed_Hold=enb;
}
}
if(isMatch==)replyBytes(true,"C",bs,);
}
void long2byte(unsigned long res,byte targets[] ) {
targets[] = (byte) (res & 0xff);// 鏈�浣庝綅
targets[] = (byte) ((res >> ) & 0xff);// 娆′綆浣�
targets[] = (byte) ((res >> ) & 0xff);// 娆￠珮浣�
targets[] = (byte) (res >> );// 鏈�楂樹綅,鏃犵鍙峰彸绉汇��
}
unsigned long bytes2long(byte buf[]) {
unsigned long firstByte = ;
unsigned long secondByte = ;
unsigned long thirdByte = ;
unsigned long fourthByte = ;
int index = ;
firstByte = (0x000000FFU & ( buf[index+]));
secondByte = (0x000000FFU & ( buf[index + ]));
thirdByte = (0x000000FFU & ( buf[index + ]));
fourthByte = (0x000000FFU & ( buf[index ]));
index = index + ;
return ((unsigned long) (firstByte << | secondByte << | thirdByte << | fourthByte)) & 0xFFFFFFFFUL;
}