平衡小车的控制算法（PID,LQR,MPC)及arduino程序导航贴

原创

已于 2022-05-24 19:01:52 修改 · 6.7k 阅读

104 ·

CC 4.0 BY-SA版权

文章标签：

#平衡小车

于 2022-04-22 21:57:03 首次发布

本文介绍了使用Arduino实现平衡小车电机位置的编码器脉冲计数，通过PID算法进行控制，并将编码器计数转换为角度信息。同时，探讨了小车动力学模型和系统的可控性，利用特征值判断系统动态特性，并设计了龙伯格观测器以获取所需的状态信息。

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

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口2

volatile 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++;
    else
        LeftMotorCounter--;
    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--;
    else
        RightMotorCounter++;
    Serial.print("右边电机位置：");Serial.println(RightMotorCounter);
  } 
  portEXIT_CRITICAL_ISR(&mux);
}


void setup(){
  Serial.begin(115200);
  Serial.println("中断测试实验"); 
  
  //off motor enable
  pinMode(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_PULLU