1. 기능 설명
이륜 트롤리에 6축 자이로 센서를 설치하고 이 글의 예제는 이륜 트롤리의 자체 균형 기능을 구현합니다.
2. 전자 하드웨어
이 예에서는 다음 하드웨어를 사용합니다. 참조하십시오.
| 주 제어 보드 | Basra 메인 제어 보드(Arduino Uno와 호환 가능) |
| 확장 보드 | Bigfish2.1 확장 보드 |
| 감지기 | 6축 자이로스코프 |
| 배터리 | 7.4V 리튬 배터리 |
회로 연결:
① 6축 자이로 센서(GND, VCC, RX, TX)는 Bigfish 확장 보드의 Gnd, Vcc(3.3v), RX, TX에 연결됩니다.
② Bigfish 확장보드의 (5,6)과 (9,10)에 DC모터 2개를 각각 연결한다.
3. 기능 구현
프로그래밍 환경: 아두이노 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_Y 1.13 //读取到陀螺仪 Y 轴向后的极限数值
#define Gyroscope_Middle_LimitAngle_Y 0.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[11],counter=0; //存储陀螺仪数据的变量
unsigned char sign=0; //定义是否接收到陀螺仪数据的标志位
float a[3]; //用于存储x、y、z三轴的角速度包数值
int motor_pin[4] = {5,6,9,10};//定义电机引脚
int map_to_int[3] = {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[i],INPUT); //将电机的四个引脚设置为输出模式
}
map_to_int[0] = Gyroscope_left_LimitAngle_Y*100; //重新赋予陀螺仪 Y 轴向前的极限数值
map_to_int[1] = Gyroscope_Right_LimitAngle_Y*100; //重新赋予陀螺仪 Y 轴向后的极限数值
map_to_int[2] = Gyroscope_Middle_LimitAngle_Y*100; //重新赋予陀螺仪 Y 轴平放时的数值
}
void loop()
{
Get_gyroscope_And_Control();//读取陀螺仪参数，并判断平衡小车前倾、后仰或者是平放状态
}
//实时读取陀螺仪发送的数据(serialEvent()函数会自动运行)
void serialEvent() {
while (Serial.available()) {
Re_buf[counter]=(unsigned char)Serial.read();
if(counter==0&&Re_buf[0]!=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[0]==0x55) //检查帧头
{
switch(Re_buf [1])
{
case 0x51:
{
a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
//把陀螺仪的加速度包的Y轴数据转换为直流电机的速度
map_data = fabs(a[1]) * 100; //fabs()取浮点数的绝对值
if( (map_data >= (map_to_int[2]-10)) && (map_data <= (map_to_int[2]+10)) )
{
//小车处于平衡态
Motor_State(Stop,0,0);
}
else if( (map_data < (map_to_int[2]-10)) && (map_data >= map_to_int[0]) )
{
/*当小车前倾，自行调整至平衡
假如现在获取到陀螺仪数据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[2]-10),map_to_int[0],Motor_Speed_Mix,Motor_Speed_Max),right_Motor_Speed_Init+map(map_data,(map_to_int[2]-10),map_to_int[0],Motor_Speed_Mix,Motor_Speed_Max) );
}
else if( (map_data <= map_to_int[1]) && (map_data > (map_to_int[2]+10)) )
{
//当小车后仰，自行调整至平衡
Motor_State(Back,left_Motor_Speed_Init+map(map_data,(map_to_int[2]+10),map_to_int[1],Motor_Speed_Mix,Motor_Speed_Max),right_Motor_Speed_Init+map(map_data,(map_to_int[2]+10),map_to_int[1],Motor_Speed_Mix,Motor_Speed_Max) );
}
}break;
}
}
}
}
void Motor_State(int _mode, int _left, int _right)//电机状态函数
{
switch(_mode)
{
case Forward: {analogWrite(motor_pin[0],_right);analogWrite(motor_pin[1],0);analogWrite(motor_pin[2],_left);analogWrite(motor_pin[3],0);}break;
case Back: {analogWrite(motor_pin[1],_right);analogWrite(motor_pin[0],0);analogWrite(motor_pin[3],_left);analogWrite(motor_pin[2],0);}break;
case Stop: {analogWrite(motor_pin[0],0);analogWrite(motor_pin[1],0);analogWrite(motor_pin[2],0);analogWrite(motor_pin[3],0);}break;
}
}