通过电机编码器AB相输出确定电机转向

AB相输出相差90度，即当A相"正跳变"时如果B相是高电平那么是"正转"，反之是"反转"

 

图片:

正转

反转

#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);//