平衡小车电机位置测试小实验

1.编码器脉冲计数

const byte LeftMotorInterruptP = 22;  //左电机编码器中断引脚
const byte LeftMotorCountP = 23;      //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19;     //右电机编码器计数引脚const byte MotorDriverEn = 5;         //电机驱动器使能
const byte LeftMotorP1 = 15;          //左电机控制io口1
const byte LeftMotorP2 = 13;          //左电机控制io口2
const byte RightMotorP1 = 16;         //右电机控制io口1
const byte RightMotorP2 = 17;         //右电机控制io口2volatile long LeftMotorCounter = 0;   //左电机中断计数位置
volatile long RightMotorCounter = 0;  //右电机中断计数位置portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;  //声明一个portMUX_TYPE类型的变量，利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;//左电机中断函数
void LeftMotorInterruptF() {portENTER_CRITICAL_ISR(&mux_1);delayMicroseconds(10);  //延时20ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间if(digitalRead(LeftMotorInterruptP) == LOW)  //因为是上拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断{if(digitalRead(LeftMotorCountP)==LOW)LeftMotorCounter++;elseLeftMotorCounter--;Serial.print("左边电机位置：");Serial.println(LeftMotorCounter);}portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {portENTER_CRITICAL_ISR(&mux);delayMicroseconds(10);  //延时2ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间if(digitalRead(RightMotorInterruptP) == LOW)    //因为是下拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断{if(digitalRead(RightMotorCountP)==LOW)RightMotorCounter--;elseRightMotorCounter++;Serial.print("右边电机位置：");Serial.println(RightMotorCounter);} portEXIT_CRITICAL_ISR(&mux);
}void setup(){Serial.begin(115200);Serial.println("中断测试实验"); //off motor enablepinMode(MotorDriverEn,OUTPUT);digitalWrite(MotorDriverEn,LOW);pinMode(LeftMotorP1,OUTPUT);pinMode(LeftMotorP2,OUTPUT);pinMode(RightMotorP1,OUTPUT);pinMode(RightMotorP2,OUTPUT);digitalWrite(LeftMotorP1,LOW);digitalWrite(LeftMotorP2,LOW);digitalWrite(RightMotorP1,LOW);digitalWrite(RightMotorP2,LOW);pinMode(LeftMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式pinMode(RightMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式pinMode(LeftMotorCountP,INPUT);pinMode(RightMotorCountP,INPUT);attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}void loop(){}

PID控制算法

电机速度环和位置环请看这篇文章：

PID控制算法及arduino应用（电机调速和位置控制）_Allen953的博客-CSDN博客

2.编码器计数转换角度


const int CountperCircuit = 390;      //390个脉冲为一圈
const double pi = 3.141592653;        //piconst byte LeftMotorInterruptP = 22;  //左电机编码器中断引脚
const byte LeftMotorCountP = 23;      //左电机编码器计数引脚
const byte RightMotorInterruptP = 18; //右电机编码器中断引脚
const byte RightMotorCountP = 19;     //右电机编码器计数引脚const byte MotorDriverEn = 5;         //电机驱动器使能
const byte LeftMotorP1 = 15;          //左电机控制io口1
const byte LeftMotorP2 = 13;          //左电机控制io口2
const byte RightMotorP1 = 16;         //右电机控制io口1
const byte RightMotorP2 = 17;         //右电机控制io口2volatile long LeftMotorCounter = 0;   //左电机中断计数位置
volatile long RightMotorCounter = 0;  //右电机中断计数位置
volatile double LeftMotordeg = 0.0;   //左电机中断计数位置
volatile double RightMotordeg = 0.0;  //右电机中断计数位置portMUX_TYPE mux = portMUX_INITIALIZER_UNLOCKED;  //声明一个portMUX_TYPE类型的变量，利用其对主代码和中断之间的同步进行处理
portMUX_TYPE mux_1 = portMUX_INITIALIZER_UNLOCKED;//左电机中断函数
void LeftMotorInterruptF() {portENTER_CRITICAL_ISR(&mux_1);delayMicroseconds(10);  //延时20ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间if(digitalRead(LeftMotorInterruptP) == LOW)  //因为是上拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断{if(digitalRead(LeftMotorCountP)==LOW)LeftMotorCounter++;elseLeftMotorCounter--;LeftMotordeg = double(LeftMotorCounter)/double(CountperCircuit)*360.0;Serial.print("左边电机位置：");Serial.print(LeftMotordeg);Serial.println("'C");}portEXIT_CRITICAL_ISR(&mux_1);
}
//右电机中断函数
void RightMotorInterruptF() {portENTER_CRITICAL_ISR(&mux);delayMicroseconds(10);  //延时2ms作为消抖，如果是很稳定的中断可以不加或者加很少的消抖时间if(digitalRead(RightMotorInterruptP) == LOW)    //因为是下拉触发，所以在消抖时间完后读取引脚高低电平，如果还是为低那么就代表出现了一次稳定的中断{if(digitalRead(RightMotorCountP)==LOW)RightMotorCounter--;elseRightMotorCounter++;RightMotordeg = double(RightMotorCounter)/double(CountperCircuit)*360.0;Serial.print("右边电机位置：");Serial.print(RightMotordeg);Serial.println("'C");} portEXIT_CRITICAL_ISR(&mux);
}void setup(){Serial.begin(115200);Serial.println("中断测试实验"); //off motor enablepinMode(MotorDriverEn,OUTPUT);digitalWrite(MotorDriverEn,LOW);pinMode(LeftMotorP1,OUTPUT);pinMode(LeftMotorP2,OUTPUT);pinMode(RightMotorP1,OUTPUT);pinMode(RightMotorP2,OUTPUT);digitalWrite(LeftMotorP1,LOW);digitalWrite(LeftMotorP2,LOW);digitalWrite(RightMotorP1,LOW);digitalWrite(RightMotorP2,LOW);pinMode(LeftMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式pinMode(RightMotorInterruptP, INPUT_PULLUP);  //先把引脚设置为上拉输入模式pinMode(LeftMotorCountP,INPUT);pinMode(RightMotorCountP,INPUT);attachInterrupt(digitalPinToInterrupt(LeftMotorInterruptP), LeftMotorInterruptF, FALLING);attachInterrupt(digitalPinToInterrupt(RightMotorInterruptP), RightMotorInterruptF, FALLING);
}void loop(){}