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);// 设置定时器中断时间&#