Arduino平衡车代码原理部分的一些讲解

作者: 芜青子 | 来源:发表于2020-11-22 20:22 被阅读0次

Arduino平衡车代码原理部分的一些讲解
Arduino平衡车完整代码+详解
Arduino平衡车——只角度环pid的代码
数据结构与算法之AVL树(九)
平衡车原理
老大爷都能看懂的RecyclerView动画原理
arduino和NB-IOT模块M5310-A使用教程，app实
平衡车原理：双轮平衡车怎么操作？
平衡车的原理是什么？怎么操作平衡车？
Arduino技术指南

/************************使用的头文件********************************/
#include <PinChangeInt.h>    //外部中断
#include <MsTimer2.h>        //定时中断



#include "I2Cdev.h"  //I2C协议库
#include "Wire.h"    //服务于I2C通信  
#include "MPU6050_6Axis_MotionApps20.h"//MPU6050库文件

/*******************************************************************/

/************************实例化*********************************************************/


MPU6050 mpu6050;                             //实例化一个 MPU6050 对象，对象名称为 mpu6050

/**************************************************************************************/

/***************************全局变量*************************************************/
int16_t ax, ay, az, gx, gy, gz;  //MPU6050的三轴加速度和三轴陀螺仪（角速度）数据

         /**********引脚分配************/
#define AIN1 9   //TB6612FNG驱动模块控制信号 共6个
#define AIN2 8
#define BIN1 12
#define BIN2 13
#define PWMA 6
#define PWMB 10 // 11号脚是废的

#define ENCODER_L_A 2  //编码器采集引脚 每路2个 共4个 ENCODER_L
#define ENCODER_L_B 3  //DIRECTION_L
#define ENCODER_R_A 4  //ENCODER_R
#define ENCODER_R_B 5  //DIRECTION_R 



        /********************************/

        
#define DIFFERENCE 2       //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。
#define ZHONGZHI -8        //小车的机械中值
float Turn_Amplitude = 40; //转向目标幅度值 
        
int Balance_Pwm, Velocity_Pwm, Turn_Pwm;   //直立 速度 转向环的PWM

int Motor1, Motor2;      //电机叠加之后的PWM
volatile long Velocity_L, Velocity_R = 0;   //左右轮编码器数据
int Velocity_Left, Velocity_Right = 0;     //左右轮速度

int Angle ;      //用于显示的 角度 的临时变量




/**** PID 参数 *********/
float Balance_Kp= 15,  Balance_Kd=0.5;      // 直立PD
float Velocity_Kp= -2.4,  Velocity_Ki= -0.012;    // 速控PI，参考 kp = 2, ki = kp / 200; 
float Turn_Kp = 2, Turn_Kd = 0.001;        // 转向PD
/*******************/








/********************************下为闭环函数*********************************/
/**************************************************************************
函数功能：直立PD控制  作者：平衡小车之家
入口参数：角度、角速度
返回  值：直立控制PWM
**************************************************************************/

int balance(float Angle, float Gyro)
{
  float Bias;
  int balance;
  Bias = Angle - ZHONGZHI;   //===求出平衡的角度中值 和机械相关
  balance = Balance_Kp * Bias + Gyro * Balance_Kd; //===计算平衡控制的电机PWM  PD控制   kp是P系数 kd是D系数; 15 ,0.4
  return balance;
}

/**************************************************************************
函数功能：速度PI控制 作者：平衡小车之家
入口参数：左轮编码器、右轮编码器
返回  值：速度控制PWM
**************************************************************************/
int velocity(int encoder_left, int encoder_right)
{
  static float Velocity, Encoder_Least, Encoder, Movement;
  static float Encoder_Integral, Target_Velocity;
  if       ( Flag_Qian == 1) Movement = -300;
  else   if ( Flag_Hou == 1) Movement = 300;
  else    //这里是停止的时候反转，让小车尽快停下来
  {
    Movement = 0;
    if (Encoder_Integral > 300)   Encoder_Integral -= 200;
    if (Encoder_Integral < -300)  Encoder_Integral += 200;
  }
  //=============速度PI控制器=======================//
  Encoder_Least = (encoder_left + encoder_right) - 0;               //===获取最新速度偏差==测量速度（左右编码器之和）-目标速度（此处为零）
  Encoder *= 0.7;                                                   //===一阶低通滤波器
  Encoder += Encoder_Least * 0.3;                                   //===一阶低通滤波器
  Encoder_Integral += Encoder;                                      //===积分出位移 积分时间：40ms
  Encoder_Integral = Encoder_Integral - Movement;                   //===接收遥控器数据，控制前进后退
  if (Encoder_Integral > 10000)    Encoder_Integral = 10000;        //===积分限,控制最高速度
  if (Encoder_Integral < -10000) Encoder_Integral = -10000;         //===积分限幅
  
  Velocity = Encoder * Velocity_Kp + Encoder_Integral * Velocity_Ki;                  //===速度PI控制
  // if (Turn_Off(KalFilter.angle, Battery_Voltage) == 1 || Flag_Stop == 1)    Encoder_Integral = 0;//小车停止的时候积分清零
  return Velocity;
}

/**************************************************************************
函数功能：转向控制 作者：平衡小车之家
入口参数：Z轴陀螺仪
返回  值：转向控制PWM
**************************************************************************/
int turn(float gyro)//转向控制
{
  static float Turn_Target, Turn, Turn_Convert = 1; // Turn_Convert可以控制转向的快慢
  
  if (1 == Flag_Left)             Turn_Target += Turn_Convert;  //根据遥控指令改变转向偏差
  else if (1 == Flag_Right)       Turn_Target -= Turn_Convert;//根据遥控指令改变转向偏差
  else Turn_Target = 0;
  if (Turn_Target > Turn_Amplitude)  Turn_Target = Turn_Amplitude; //===转向速度限幅
  if (Turn_Target < -Turn_Amplitude) Turn_Target = -Turn_Amplitude;
  
  Turn = - Turn_Target * Turn_Kp  + gyro * Turn_Kd;         //===结合Z轴陀螺仪进行PD控制
  return Turn;
}

/**************************************************************************
函数功能：赋值给PWM寄存器 作者：平衡小车之家
入口参数：左轮PWM、右轮PWM
返回  值：无
**************************************************************************/
void Set_Pwm(int moto1, int moto2)
{
  if (moto1 > 0)   {  digitalWrite(AIN1, HIGH);      digitalWrite(AIN2, LOW); } //TB6612的电平控制
  else            { digitalWrite(AIN1, LOW);       digitalWrite(AIN2, HIGH); }//TB6612的电平控制
  analogWrite(PWMA, abs(moto1)); //赋值给PWM寄存器
  
  if (moto2 < 0)  { digitalWrite(BIN1, HIGH);     digitalWrite(BIN2, LOW); }//TB6612的电平控制
  else       { digitalWrite(BIN1, LOW);     digitalWrite(BIN2, HIGH); }//TB6612的电平控制
  analogWrite(PWMB, abs(moto2));//赋值给PWM寄存器
}

/**************************************************************************
函数功能：限制PWM赋值  作者：平衡小车之家
入口参数：无
返回  值：无
**************************************************************************/
void Xianfu_Pwm(void)
{
  int Amplitude = 250;  //===PWM满幅是255 限制在250
   if(Flag_Qian==1)  Motor2-=DIFFERENCE;  //DIFFERENCE是一个衡量平衡小车电机和机械安装差异的一个变量。直接作用于输出，让小车具有更好的一致性。
   if(Flag_Hou==1)   Motor2-=DIFFERENCE-2;
  if (Motor1 < -Amplitude) Motor1 = -Amplitude;
  if (Motor1 > Amplitude)  Motor1 = Amplitude;
  if (Motor2 < -Amplitude) Motor2 = -Amplitude;
  if (Motor2 > Amplitude)  Motor2 = Amplitude;
}

/********************************************************************************/


/**************************************************************************
函数功能：5ms控制函数 核心代码 作者：平衡小车之家
入口参数：无
返回  值：无
**************************************************************************/
void control()
{
  static int Velocity_Count , Turn_Count, Display_Count;

  sei();//全局中断开启
  
  mpu6050.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);  //获取MPU6050陀螺仪和加速度计的数据
  KalFilter.Angletest(ax, ay, az, gx, gy, gz, dt, Q_angle, Q_gyro, R_angle, C_0, K1);          //通过卡尔曼滤波获取角度
  Angle = KalFilter.angle;//Angle是一个用于显示的整形变量

  //直立PD控制 控制周期5ms
  Balance_Pwm = balance(KalFilter.angle, KalFilter.Gyro_x); 

  //速度PI控制，控制周期40ms
  if (++Velocity_Count >= 8) 
  {
    Velocity_Left = Velocity_L;    Velocity_L = 0;  //读取左轮编码器数据，并清零，这就是通过M法测速（单位时间内的脉冲数）得到速度。
    Velocity_Right = - Velocity_R;    Velocity_R = 0; //读取右轮编码器数据，并清零
    Velocity_Pwm = velocity(Velocity_Left, Velocity_Right);//速度PI控制，控制周期40ms
    Velocity_Count = 0;
  }
  
  //转向PD控制，控制周期20ms
  if (++Turn_Count >= 4)
  {
    Turn_Pwm = turn(gz);
    Turn_Count = 0;
  }


  // 三环融合
  Motor1 = Balance_Pwm - Velocity_Pwm + Turn_Pwm;  //直立速度转向环的叠加
  Motor2 = Balance_Pwm - Velocity_Pwm - Turn_Pwm; //直立速度转向环的叠加

/*****************************************************/ 
  Xianfu_Pwm(); //限幅

  // 角度保护
  if(Angle > 40 || Angle < -50) Motor1=0,Motor2=0;

 
  Set_Pwm(Motor1, Motor2); //如果不存在异常，赋值给PWM寄存器控制电机
  
  //屏幕显示，每1500ms刷新一次
  if(OLED_F % 2 == 1){
    if(++Display_Count >= 300){
      Show_Screen();
      Display_Count = 0;
    }
  }
}

/**************************************************************************
函数功能：初始化，只执行一次
入口参数：无
返回  值：无
**************************************************************************/
void setup() {
  // put your setup code here, to run once:
  pinMode(AIN1, OUTPUT);        //TB6612控制引脚，控制电机1的方向，01为正转，10为反转
  pinMode(AIN2, OUTPUT);          //TB6612控制引脚，
  pinMode(BIN1, OUTPUT);          //TB6612控制引脚，控制电机2的方向，01为正转，10为反转
  pinMode(BIN2, OUTPUT);          //TB6612控制引脚，
  pinMode(PWMA, OUTPUT);         //TB6612控制引脚，电机PWM
  pinMode(PWMB, OUTPUT);         //TB6612控制引脚，电机PWM
  digitalWrite(AIN1, 0);          //TB6612控制引脚拉低
  digitalWrite(AIN2, 0);          //TB6612控制引脚拉低
  digitalWrite(BIN1, 0);          //TB6612控制引脚拉低
  digitalWrite(BIN2, 0);          //TB6612控制引脚拉低
  analogWrite(PWMA, 0);          //TB6612控制引脚拉低
  analogWrite(PWMB, 0);          //TB6612控制引脚拉低

  pinMode(ENCODER_L_A, INPUT);       //编码器引脚
  pinMode(ENCODER_L_B, INPUT);       //编码器引脚
  pinMode(ENCODER_R_A, INPUT);       //编码器引脚
  pinMode(ENCODER_R_B, INPUT);       //编码器引脚


  Serial.begin(9600);       //开启串口，设置波特率为 9600  
  Wire.begin();             //加入 IIC 总线
  delay(1500);              //延时等待初始化完成
  
  mpu6050.initialize();     //初始化MPU6050
  delay(20); 

  
  MsTimer2::set(5, control);  //使用Timer2设置5ms定时器中断，
  MsTimer2::start();          //使用中断使能，Timer2开始计时，每5ms进入一次中断程序control

   /*中断触发类型 
   LOW: 低电平触发。
   CHANGE:管脚状态改变触发。
   RISING:上升沿触发。
   FALLING:下降沿触发
  */
 
  /*开启外部中断 编码器接口1，0代表中断号；
   ATmega168 / 328上有两个外部中断引脚，称为INT0和INT1。 INT0和INT1分别映射到引脚2和3；
   #define ENCODER_L_A 2 ,因此可以触发左轮测速函数READ_ENCODER_L
   */
  attachInterrupt(0, READ_ENCODER_L, CHANGE);    

  /*开启外部中断（引脚变化中断） 编码器接口2；
   引脚变化中断可以在任何引脚上激活；
   #define ENCODER_R_A 4 ,因此可以触发右轮测速函数READ_ENCODER_R
  */
  attachPinChangeInterrupt(4, READ_ENCODER_R, CHANGE);

  
}

/**************************************************************************
函数功能：主循环程序体
入口参数：无
返回  值：无
**************************************************************************/
void loop() {

  


}

/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void READ_ENCODER_L() {
//  Serial.println("L");
  if (digitalRead(ENCODER_L_A) == LOW) {     //如果是下降沿触发的中断
    if (digitalRead(ENCODER_L_B) == LOW)      Velocity_L--;  //根据另外一相电平判定方向
    else      Velocity_L++;
  }
  else {     //如果是上升沿触发的中断
    if (digitalRead(ENCODER_L_B) == LOW)      Velocity_L++; //根据另外一相电平判定方向
    else     Velocity_L--;
  }
}
/**************************************************************************
函数功能：外部中断读取编码器数据，具有二倍频功能 注意外部中断是跳变沿触发
入口参数：无
返回  值：无
**************************************************************************/
void READ_ENCODER_R() {
//  while(1) Serial.println("R");
  if (digitalRead(ENCODER_R_A) == LOW) { //如果是下降沿触发的中断
    if (digitalRead(ENCODER_R_B) == LOW)      Velocity_R--;//根据另外一相电平判定方向
    else      Velocity_R++;
  }
  else {   //如果是上升沿触发的中断
    if (digitalRead(ENCODER_R_B) == LOW)      Velocity_R++; //根据另外一相电平判定方向
    else     Velocity_R--;
  }
}

Arduino平衡车代码原理部分的一些讲解
Arduino平衡车完整代码+详解
代码中使用了两套PID参数进行控制，Kp，Ki，Kd是控制平衡速度的。也就是小车往前倾，加速；后倾，减速这个过程。...
Arduino平衡车——只角度环pid的代码
但是仅靠一个PID控制会出现一个问题：因为小车的平衡角度Setpoint是手动设定的，所以会有一个微小的误差。当小...
数据结构与算法之AVL树(九)
目录基本概念添加节点后恢复平衡操作处理失衡的方法 LL，RR，LR，RL图解 + 核心代码实现 + 原理讲解删除...
平衡车原理
文章由久保洋智能电动车厂家提供（http://www.kubaoya.com/）电动平衡车工作原理是系统以姿态传...
老大爷都能看懂的RecyclerView动画原理
如何阅读本篇文章本文主要讲解RecyclerView Layout变化触发动画执行的原理。前半部分偏重原理和代码...
arduino和NB-IOT模块M5310-A使用教程，app实
第一、先上效果图第二、原理讲解原理简述：利用发布订阅模式。第一步，新建主题，第二步，arduino通过nb-...
平衡车原理：双轮平衡车怎么操作？
=平衡车是一种舒适方便的代步工具，只要站在上面就可以轻松的操纵。现在，平衡车能够环保便利的解决交通堵塞的问题，那么...
平衡车的原理是什么？怎么操作平衡车？
才入手平衡车的你，是不是还在探索着怎么操作平衡车呢？今天【车小秘】就来说说平衡车的原理，给新手菜鸟们做一个简单明了...
Arduino技术指南
《Arduino技术指南》本书主要讲解了Arduino开发板的物理特性与接口功能，Arduino使用的各种AVR微...