hihi好久没更新了,上大三后感觉各种学业上的压力还是挺大的,好多专业课要学。没有时间踏踏实实的做点东西,平衡车断断续续的做了有一个多月吧,从画板子到贴片,到最后小车队组装和调试,也算是完整的做了一个小项目,踩了不少坑。下面把我这次做平衡车的经验分享给大家吧。
实物展示
一、硬件部分
1、主控PCB板
我用的是自己画的板子,集成了esp32、TB6612、陀螺仪、降压模块还有电源和电机接口。博主给它起了个名“HAPPY”系列,这所以叫系列呢,是因为我们这个板子已经改了四代了,哭哭。
之前三代因为各种各样的问题寄了。。。
(例如一代直插芯片的引脚画反了,然后钽电容还贴反了,结果就是上电直接炸boom~)
但是经过我们不懈努力下,HAPPY-3代终于能用了!真是普天同庆,下面给大家放几张图
全部贴片焊完的实物大概就是这样
怎么样 还不错吧,如有需要这个板子的工程文件,可以私信我要,免费欧
2、元件使用
带编码器的直流减速电机 两个
陀螺仪mpu6050 一个
esp32开发板 一个
航模电池 一块
3、底板
原先底板是我自己用雕版机器打的,但是由于不太好看 后来换成了合金的。
整个小车组合完毕就是这样
二、原理讲解
平衡原理:小车运动方向与倾斜方向一致,通过陀螺仪测量小车的倾角与倾角角速度,进而控制车轮的加速度来抵消小车的倾角和倾角角速度,小车即可保持平衡。也就是当小车向左倾斜,车轮向左加速运行;小车垂直,车轮保持静止;小车向右倾斜,车轮向右加速运行。
三、软件代码及调试
1、mpu6050陀螺仪角度方向和静态平衡角度测试
硬件搭建完毕后,就需要测陀螺仪静态平衡点了,下面是教程
/*说明:
1【陀螺仪补偿值的计算】
试时提前用calcGyroOffsets(true)函数计算出,补偿值。
知道mpu6050的补偿值后用setGyroOffsets()直接设置补偿值。
避免每次开机都要计算补偿值(注:检测大概需要3-5秒时间)
2【静态平衡角度获取】
普通做法是将小车两轮悬空,两轮间底盘放置在水平置物架上(如包裹小纸盒),读取此时的角度。可以大概作为静态平衡角度。
但因安装时各电器器件和电池安装不会绝对居中,所以下车的中心不一定在水平上。
最好通过放置小车轮子在桌面,用手前后调整找到保持平衡的静态角度。
*/
// 20220902测试,静态角度为-1.14度。陀螺仪补偿值测定后为setGyroOffsets(8.24, 1.43, -0.34);
#include <MPU6050_tockn.h>
#include <Wire.h>
MPU6050 mpu6050(Wire); //实例化mpu对象
void setup()
{
Serial.begin(9600);
Wire.begin(21, 22);
mpu6050.begin();
// mpu6050.calcGyroOffsets(true);
// mpu6050.setGyroOffsets(-0.02,-0.02, -0.02);
}
void loop()
{
mpu6050.update();//陀螺仪刷新
float AngleX = mpu6050.getAngleX();
Serial.println(AngleX);
// float AngleY = mpu6050.getAngleY();
// Serial.println(AngleY);
//检测Z轴偏转时的加速度数值大小值,作为转向环PWM计算参考。正常直行时在-10-10之间波动。
//顺时针为负值,逆时针为正值
// float GyroZ = mpu6050.getGyroZ();
// Serial.println(GyroZ);
}
2、电机驱动
这个根据大家自选的电机及驱动写程序即可,我用的是TB6612搭配直流减速电机
/*
* ==程序编写2==
* 【马达驱动方向和死区大小测试】
*
* 测试结果 6v电压供给下。
* pwm+为前进,pwm+-为后退
* 左马达IN1 IN2,OUT1 OUT2 , pwm 36起转。
* 右马达IN3 IN4,OUT3 OUT4, PWM 39起转。
*
*/
#define IN1 14
#define IN2 12
#define IN3 27
#define IN4 26
int pwm = 0;
void motor(int left_EN, int right_EN)
{
if (left_EN == 0)
{
analogWrite(PWMA, 0);
analogWrite(IN1, 0);
analogWrite(IN2, 0);
}
if (left_EN < 0)
{
if (left_EN < -255)
left_EN = -255;
analogWrite(PWMA, 0 - left_EN);
analogWrite(IN1, 0);
analogWrite(IN2, 0 - left_EN);
}
if (left_EN > 0)
{
if (left_EN > 255)
left_EN = 255;
analogWrite(PWMA, left_EN);
analogWrite(IN1, left_EN);
analogWrite(IN2, 0);
}
if (right_EN == 0)
{
analogWrite(PWMB, 0);
analogWrite(IN3, 0);
analogWrite(IN4, 0);
}
if (right_EN < 0)
{
if (right_EN < -255)
right_EN = -255;
analogWrite(PWMB, 0 - right_EN);
analogWrite(IN3, 0);
analogWrite(IN4, 0 - right_EN);
}
if (right_EN > 0)
{
if (right_EN > 255)
right_EN = 255;
analogWrite(PWMB, right_EN);
analogWrite(IN3, right_EN);
analogWrite(IN4, 0);
}
}
void serial_debug()
{
if (Serial.available() > 0)
{
delay(5);
char DATA = Serial.read();
switch (DATA)
{
case '1':
pwm++;
Serial.print("PWM: ");
Serial.println(pwm);
break;
case '0':
pwm--;
Serial.print("PWM: ");
Serial.println(pwm);
break;
}
}
}
void setup()
{
Serial.begin(9600);
}
void loop()
{
serial_debug();
motor(pwm, pwm);
}
3、小车控制及调pid
大家把静态平衡点以及电机驱动死区测完后就可以写控制程序进行调PID了,给大家推荐一个b站的老师讲解如何调试pid【Arduino ESP32】 C++自平衡小车入门(共42集)_哔哩哔哩_bilibili
调pid简单来讲就是:
首先确定P的极性,也就是P参数在你的控制函数里的正负:清零其他参数,设置P=+1,如果向前倾,车轮前转,则极性正确,反之错误,到达第一档,说明你的P已经在一个良好的适合调节的范围,继续增大P,中间可能出现更好的更稳定的现象,但不要停止增大参数(因为你不知道最好的阈值在哪里),最终目标是增大到小车出现大幅低频抖动。
接下来确定Kd,关于直立环D的确定也是先确定极性,清空之前设置的Kp及其他PID参数(单一变量),设置Kd=+0.5,Kd极性正确的现象与Kp一致,都是向哪里倾倒就像哪里转动。然后确定大小,从小到大设置Kd,直到出现高频率的震荡可以看出此时的晃动幅度被抑制,但是晃动频率变高。
最后一点一点增加Ki,使得小车稳定为止。
滴滴滴 源码来了~
下面就是整个平衡车控制的全部代码,控制小车前进后退及转向需要用蓝牙控制,大家可以自行安装一个蓝牙助手 便于调试
/*
====小车器件清单==================================
控制器:ESP32
马达:MG513减速电机 1:30 12v
马达驱动模块:TB6612FNG
陀螺仪模块:MPU6050
====马达测试参数 12v电压供给下=======================
pwm+为前进,pwm+-为后退,均测试
左马达IN1 IN2, OUT1 OUT2 , PWM10起转。
右马达IN3 IN4,OUT3 OUT4 , PWM10起转。
====陀螺仪测试参数================================
小车静态机械平衡时的角度为-4.8度。2
====PID调试结果==================================
P I D 调试结果1 P:12 I:0.7 D:0.8
P I D 调试结果2 P: I: D:
------------------------------------------------------
*/
#include "BluetoothSerial.h"
#include <MPU6050_tockn.h>
#include <Wire.h>
#define AIN1 14 // A电机的输入端口1
#define AIN2 12 // A电机的输入端口2
#define PWMA 13 // A电机的PWM信号输入端口
#define BIN1 27 // B电机的输入端口1
#define BIN2 26 // B电机的输入端口2
#define PWMB 25 // B电机的PWM信号输入端口
int pwma=0;
int pwmb=0;
//int sda_pin = 13, scl_pin = 15; //自定义eps32的I2C引脚下
/*---调试和预设定值的变量--------*/
float Balance_Angle_raw = 2; //测试出的静态机械平衡角度。
int leftMotorPwmOffset = 1, rightMotorPwmOffset = 1; //测试出的左右轮的启动pwm值,pwm达到一定电压马达才开始转动。
float ENERGY = 3.5; //设定的前进后退倾倒角度幅度(越大,前冲后退越快)
float turn_ENERGY = 600; //设定的转向幅度,转向的角速度幅度。
float kp = 6, ki = 0.2, kd = 0.3; //根据调试测试得到设置kp ki kd的值
/*---调试和预设定的变量 end--------*/
/*---调试和控制变量--------*/
float Keep_Angle, bias, integrate; //保持角度,角度偏差,偏差积分变量
float AngleX, GyroX, GyroZ; // mpu6050输出的角度值为浮点数,两位有效小数
int vertical_PWM, turn_PWM, PWM, L_PWM, R_PWM; //各种PWM计算值
float turn_kp, turn_spd; //转向pwm比例向,转向速度设定
/*---调试和控制变量 end--------*/
char flag; //预留标签
BluetoothSerial SerialBT; //实例化esp32的蓝牙串口对象
MPU6050 mpu6050(Wire); //实例化mpu6050对象
void motor(int left_EN, int right_EN)
{
if (left_EN == 0)
{
analogWrite(PWMA, 0);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, LOW);
}
if (left_EN < 0)
{
if (left_EN < -255)
left_EN = -255;
analogWrite(PWMA, 0 - left_EN);
digitalWrite(AIN1, LOW);
digitalWrite(AIN2, HIGH);
}
if (left_EN > 0)
{
if (left_EN > 255)
left_EN = 255;
analogWrite(PWMA, left_EN);
digitalWrite(AIN1, HIGH);
digitalWrite(AIN2, LOW);
}
if (right_EN == 0)
{
analogWrite(PWMB, 0);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, LOW);
}
if (right_EN < 0)
{
if (right_EN < -255)
right_EN = -255;
analogWrite(PWMB, 0 - right_EN);
digitalWrite(BIN1, LOW);
digitalWrite(BIN2, HIGH);
}
if (right_EN > 0)
{
if (right_EN > 255)
right_EN = 255;
analogWrite(PWMB, right_EN);
digitalWrite(BIN1, HIGH);
digitalWrite(BIN2, LOW);
}
}
void serial_debug() //蓝牙串口调试和控制函数,根据手机端发送来的串口数据调试或控制
{
if (SerialBT.available() > 0)
{
char DATA = SerialBT.read();
delay(5);
switch (DATA)
{
/*机械平衡点调整*/
case 'u':
Keep_Angle += 0.1;
break;
case 'd':
Keep_Angle -= 0.1;
break; //调节物理平衡点,a前倾,b后仰
/*直立环调试*/
case '0':
kp -= 1; //调节直立环 比例kp项-
break;
case '1':
kp += 1; //调节直立环 比例kp项+
break;
case '2':
ki -= 0.1; //调节直立环 积分项ki-
break;
case '3':
ki += 0.1; //调节直立环 积分项ki+
break;
case '4':
kd -= 0.1; //调节直立环 微分项kd-
break;
case '5':
kd += 0.1; //调节直立环 微分项kd+
break;
/*转向环调试(只用到了比例项)*/
case '6':
turn_kp -= 0.2; //调节转向环 比例项-
break;
case '7':
turn_kp += 0.2; //调节转向环 比例项+
break;
/*蓝牙串口控制程序*/
case 's': //停车
Keep_Angle = Balance_Angle_raw; //调节物理平衡点为机械平衡角度值,原地平衡
break;
case 'a': //前进
Keep_Angle = Balance_Angle_raw + ENERGY; //通过设定需保持的倾角,使得小车前进后退
break;
case 'b': //后退
Keep_Angle = Balance_Angle_raw - ENERGY;
break;
case 'z': //不转向
flag = 'z';
turn_spd = 0; //设定Z轴目标角速度为0,即是不转向
break;
case 'l': //左转
flag = 'l';
turn_spd = turn_ENERGY; //顺时针旋转,Z轴目标角速度为正
break;
case 'r': //右转
flag = 'r';
turn_spd = -turn_ENERGY; //逆时针旋转,Z轴目标角速度为负
break;
}
/*调试时PID极性限制*/
if (kp < 0)kp = 0;
if (ki < 0)ki = 0;
if (kd < 0)kd = 0;
/*串口打印输出显示*/
SerialBT.print("Keep_Angle: ");
SerialBT.println(Keep_Angle);
SerialBT.print("kp:");
SerialBT.print(kp);
SerialBT.print(" ki:");
SerialBT.print(ki);
SerialBT.print(" kd:");
SerialBT.println(kd);
SerialBT.print(" turn_kp:");
SerialBT.println(turn_kp);
SerialBT.println("--------------------");
}
}
void vertical_pwm_calculation() //直立PMW计算
{
AngleX = mpu6050.getAngleX();
GyroX = mpu6050.getGyroX();
bias = AngleX - Keep_Angle; // 计算角度偏差。bias为小车角度是静态平衡角度的差值。
integrate += bias; //偏差的积分,integrate为全局变量,一直积累。
integrate = constrain(integrate, -1000, 1000); //限定误差积分的最大和最小值
/*==直立PID计算PWM==通0过陀螺仪返回数据计算,前倾陀螺仪Y轴为正,后仰陀螺仪Y轴为负。
前倾车前进,后仰车后退,保持直立。但可能为了直立,车会随时移动。*/
vertical_PWM = kp * bias + ki * integrate + kd * GyroX;
}
void turn_pwm_calculation() //转向PMW计算
{
GyroZ = mpu6050.getGyroZ(); //获取陀螺仪Z轴角速度
turn_PWM = turn_kp * (turn_spd - GyroZ);
// turn_PWM = constrain(turn_PWM, -180, 180);
}
void motor_control() //马达PWM控制函数
{
/*---【补偿右轮pwm差值】------------*/
if (PWM >= 0)
{
L_PWM = PWM + leftMotorPwmOffset; //左右轮的启动pwm达到一定电压马达才开始转动。这里为补偿值。
R_PWM = PWM + rightMotorPwmOffset; //左右轮的启动pwm达到一定电压马达才开始转动。这里为补偿值。
}
if (PWM < 0)
{
L_PWM = PWM - leftMotorPwmOffset; //左右轮的启动pwm达到一定电压马达才开始转动。这里为补偿值。
R_PWM = PWM - rightMotorPwmOffset; //左右轮的启动pwm达到一定电压马达才开始转动。这里为补偿值。
}
if (PWM == 0)
{
L_PWM = 0; //
R_PWM = 0; //
}
/*---【转向控制 左右轮pwm修正值】-------------*/
L_PWM -= turn_PWM;
R_PWM += turn_PWM; //控制两轮的pwm差值,通过左轮加速,右轮减速。使其右转
/*---【控制马达输出】-------------*/
L_PWM = constrain(L_PWM, -255, 255); //计算出来的PWM限定大小。255为输出上限。
R_PWM = constrain(R_PWM, -255, 255);
motor(L_PWM, R_PWM);
/*--------判断是否小车倒下,此时停止马达和编码器计数-----*/
if (AngleX > 45 || AngleX < -45) //倾角过大(车倒下时),停止马达输出
{
motor(0, 0);
}
}
void setup() { //初始化
SerialBT.begin("ESP32_car"); // Bluetooth device name
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(false);
// mpu6050.setGyroOffsets(*, *, *);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
Keep_Angle = Balance_Angle_raw; //平衡角度初始化为静态平衡时的陀螺仪角度。Keep_Angle可以改变,才可以控制前进后退。
motor(0, 0); //机器启动时马达确保停止。
delay(10); //循环前延时,确保各种初始和准备完成
}
void loop() {
/*====串口PID调试+控制===*/
serial_debug();
/*====陀螺仪刷新===*/
mpu6050.update();
/*====PWM计算====*/
vertical_pwm_calculation(); //直立环PWM计算
turn_pwm_calculation(); //转向PWM计算
PWM = vertical_PWM;
/*====马达输出=====*/
motor_control();
}