双轮平衡车实现自平衡功能
本帖最后由 机器谱 于 2023-5-26 09:36 编辑1. 功能说明
在双轮小车上安装一个 六轴陀螺仪传感器【https://www.robotway.com/h-col-137.html】 ，本文示例将实现双轮小车自主平衡功能。
https://28846868.s21i.faiusr.com/3/ABUIABADGAAg0aisogYomsiGmwcwsAQ4uwI.gif.webp
2. 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
主控板Basra主控板（兼容Arduino Uno）
扩展板Bigfish2.1扩展板
传感器六轴陀螺仪
电池7.4V锂电池
电路连接：
① 六轴陀螺仪传感器（GND、VCC、RX、TX）连接在Bigfish扩展板的Gnd、Vcc（3.3v）、RX、TX；
② 2个直流电机分别连在Bigfish扩展板的（5,6）、（9,10）。
https://28846868.s21i.faiusr.com/2/ABUIABACGAAg56usogYoiKuDtwIwoB84uBc!600x600.jpg.webp
3. 功能实现
编程环境：Arduino 1.8.19
实现思路：实现小车的自平衡。当人为去打破小车的平衡时，小车能够自行恢复到平衡状态。这里当俯视小车时，小车处于水平位置代表平衡状态；前倾或后仰等均为非平衡状态。
3.1 测试数据
① 需要先测出陀螺仪沿Y轴的状态数据。下表是本实验中测试出的实验数据，大家可参考格式，测试出自己的实验数据。
物料测试数据
陀螺仪的加速度包Y轴数据沿Y轴俯仰陀螺仪时，数据从（-0.05,1.13）逐渐增大；
陀螺仪平放时的值为0.74；
② 找准双轮小车的平衡状态（大家可尝试把锂电池安装在小车底部，让小车默认为平衡态）
3.2 示例程序
将参考例程（Gyroscope_Control_Car.ino）下载到主控板：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2023-04-25 https://www.robotway.com/
------------------------------*/
/*
功能：自平衡小车
接线：陀螺仪传感器(GND、VCC、RX、TX)接在扩展板的( Gnd、Vcc（3.3v）、RX、TX)；
直流电机分别接在（5,6），（9,10）
*/
#include<Math.h>
#define Gyroscope_left_LimitAngle_Y-0.05//读取到陀螺仪 Y 轴向前的极限数值
#define Gyroscope_Right_LimitAngle_Y1.13//读取到陀螺仪 Y 轴向后的极限数值
#define Gyroscope_Middle_LimitAngle_Y0.74//读取到陀螺仪 Y 轴平放时的数值
#define Motor_Pin_Count 4
/*
由于直流电机的物理差异，左右电机的速度值不同；
需要先微调两电机的pwm值，保证小车能走直线(否则会出现小车原地打转)
*/
#define left_Motor_Speed_Init 60//左侧电机的初始速度（0-255）
#define right_Motor_Speed_Init 70//右侧电机的初始速度（0-255）
#define Motor_Speed_Mix 0//电机速度增量最小值
#define Motor_Speed_Max 20//电机速度增量最大值
unsigned char Re_buf,counter=0; //存储陀螺仪数据的变量
unsigned char sign=0;//定义是否接收到陀螺仪数据的标志位
float a;//用于存储x、y、z三轴的角速度包数值
int motor_pin = {5,6,9,10};//定义电机引脚
int map_to_int = {0,0,0};//用于存储Y轴向左偏、向右偏、平放的数值
enum{Forward = 1,Back,Stop};//定义电机前进、后退、停止三种状态
void setup()
{
delay(1000);Serial.begin(115200);//打开串口，并设置波特率为115200
for(int i=0;i<Motor_Pin_Count;i++){
pinMode(motor_pin,INPUT);//将电机的四个引脚设置为输出模式
}
map_to_int = Gyroscope_left_LimitAngle_Y*100;//重新赋予陀螺仪 Y 轴向前的极限数值
map_to_int = Gyroscope_Right_LimitAngle_Y*100;//重新赋予陀螺仪 Y 轴向后的极限数值
map_to_int = Gyroscope_Middle_LimitAngle_Y*100; //重新赋予陀螺仪 Y 轴平放时的数值
}
void loop()
{
Get_gyroscope_And_Control();//读取陀螺仪参数，并判断平衡小车前倾、后仰或者是平放状态
}
//实时读取陀螺仪发送的数据(serialEvent()函数会自动运行)
void serialEvent() {
while (Serial.available()) {
Re_buf=(unsigned char)Serial.read();
if(counter==0&&Re_buf!=0x55) return;//第0号数据不是帧头
counter++;
if(counter==11)//接收到11个数据
{
counter=0;//重新赋值，准备下一帧数据的接收
sign=1;
}
}
}
判断双轮小车前倾、后仰或者是平放状态的参考程序（Gyroscope_Device.ino）如下：
/*------------------------------------------------------------------------------------
版权说明：Copyright 2023 Robottime(Beijing) Technology Co., Ltd. All Rights Reserved.
Distributed under MIT license.See file LICENSE for detail or copy at
https://opensource.org/licenses/MIT
by 机器谱 2023-04-25 https://www.robotway.com/
------------------------------*/
void Get_gyroscope_And_Control()//读取陀螺仪参数，并判断平衡小车前倾、后仰或者是平放状态
{
int gyroscope_acc_data= 0;
int map_data= 0;
if(sign)
{
sign=0;
if(Re_buf==0x55)//检查帧头
{
switch(Re_buf )
{
case 0x51:
{
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
a = (short(Re_buf <<8| Re_buf ))/32768.0*16;
//把陀螺仪的加速度包的Y轴数据转换为直流电机的速度
map_data = fabs(a) * 100;//fabs()取浮点数的绝对值
if( (map_data >= (map_to_int-10)) && (map_data <= (map_to_int+10)) )
{
//小车处于平衡态
Motor_State(Stop,0,0);
}
else if( (map_data < (map_to_int-10)) && (map_data >= map_to_int) )
{
/*当小车前倾，自行调整至平衡
假如现在获取到陀螺仪数据0.50，转换为50并进行映射为直流的pwm值过程
Motor_State(Forward,60+map(50,64,5,0,20), 70+map(50,64,5,0,20))
*/
Motor_State(Forward,left_Motor_Speed_Init+map(map_data,(map_to_int-10),map_to_int,Motor_Speed_Mix,Motor_Speed_Max),right_Motor_Speed_Init+map(map_data,(map_to_int-10),map_to_int,Motor_Speed_Mix,Motor_Speed_Max) );
}
else if( (map_data <= map_to_int) && (map_data > (map_to_int+10)) )
{
//当小车后仰，自行调整至平衡
Motor_State(Back,left_Motor_Speed_Init+map(map_data,(map_to_int+10),map_to_int,Motor_Speed_Mix,Motor_Speed_Max),right_Motor_Speed_Init+map(map_data,(map_to_int+10),map_to_int,Motor_Speed_Mix,Motor_Speed_Max) );
}
}break;
}
}
}
}
void Motor_State(int _mode, int _left, int _right)//电机状态函数
{
switch(_mode)
{
case Forward: {analogWrite(motor_pin,_right);analogWrite(motor_pin,0);analogWrite(motor_pin,_left);analogWrite(motor_pin,0);}break;
case Back:{analogWrite(motor_pin,_right);analogWrite(motor_pin,0);analogWrite(motor_pin,_left);analogWrite(motor_pin,0);}break;
case Stop:{analogWrite(motor_pin,0);analogWrite(motor_pin,0);analogWrite(motor_pin,0);analogWrite(motor_pin,0);}break;
}
}
4. 资料下载
资料内容：自平衡-程序源代码
资料下载地址：https://www.robotway.com/h-col-207.html
想了解更多机器人开源项目资料请关注 机器谱网站 https://www.robotway.com
页:
[1]