1. 功能说明

本文示例将实现R207样机舵机云台根据六轴陀螺仪传感器数据实现姿态跟随的功能。

2. 电子硬件

在这个示例中，我们采用了以下硬件，请大家参考：

主控板	Basra主控板（兼容Arduino Uno）‍
扩展板	Bigfish2.1扩展板‍
传感器	六轴陀螺仪
电池	7.4V锂电池

电路连接说明：

① 六轴陀螺仪传感器连接在Bigfish扩展板的GND、VCC（3.3V）、RX、TX；

② 底座舵机0号连接在Bigfish扩展板的（GND、VCC、D4）；

③ 摆动舵机1号连接在Bigfish扩展板的（GND、VCC、D3）。

3. 功能实现

编程环境：Arduino 1.8.19

实现思路：舵机云台可以根据陀螺仪传感器的数据实现姿态跟随。

六轴陀螺仪实物与空间坐标系的对应关系：

3.1 测试数据

① 测试舵机云台的两个舵机初始角度，记录数据；

② 测试六轴陀螺仪传感器的姿态数据。

下表是本实验中测试出的实验数据，大家可参考格式，测试出自己的实验数据。

物料	‍测试数据
舵机0	0 --- 180‍
舵机1	0 --- 110‍
陀螺仪的加速度包X轴数据	从左到右转动陀螺仪，数据从（+1.01，-1.05）逐渐减小；中值为0；
陀螺仪的加速度包Y轴数据	沿Y轴俯仰陀螺仪时，数据从（-1.15,1.01）逐渐增大；中值为0；

3.2 示例程序

根据已测数据编写程序，下面提供一个参考例程（body_feeling_attitude.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-24 https://www.robotway.com/

  ------------------------------*/

/*   

    功能：云台可以根据陀螺仪传感器的数据实现姿态跟随

    接线：陀螺仪传感器接在扩展板的Gnd、Vcc（3.3v）、RX、TX；

          底座舵机0号接在扩展板的（Gnd、Vcc、D4）；

          摆动舵机1号接在扩展板的（Gnd、Vcc、D3）

*/

#include<Servo.h>   //调用舵机库函数

#include<Math.h>

Servo myservo[2];   //声明两个舵机

int myservopin[2] = {4, 3}; // 定义舵机的引脚

#define Gyroscope_left_LimitAngle_X   -1.05   //读取到陀螺仪 X 轴向左偏的极限数值

#define Gyroscope_Right_LimitAngle_X   1.01   //读取到陀螺仪 X 轴向右偏的极限数值

#define Gyroscope_Middle_LimitAngle_X   0    //读取到陀螺仪 X 轴平放时的数值

#define Gyroscope_left_LimitAngle_Y   -1.05   //读取到陀螺仪 Y 轴向左偏的极限数值

#define Gyroscope_Right_LimitAngle_Y   1.01   //读取到陀螺仪 Y 轴向右偏的极限数值

#define Gyroscope_Middle_LimitAngle_Y   0    //读取到陀螺仪 Y 轴平放时的数值


#define Servo_One_Mix_Angle 0       //1号舵机最小角度

#define Servo_One_Max_Angle 180     //1号舵机最大角度

#define Servo_Two_Mix_Angle 0       //2号舵机最小角度

#define Servo_Two_Max_Angle 110     //2号舵机最大角度

#define Servo_Speed 10              //舵机速度


unsigned char Re_buf[11],counter=0;

unsigned char sign=0;

float a[3],w[3],angle[3],T;

int servo_angle_current[2] = {0,0};

float value_init[2]={90,90};

float f = 10.0;                     //舵机的频率


void setup()

{

   Serial.begin(115200);           //打开串口，并设置波特率为115200

   myservo[0].attach(myservopin[0]);

   myservo[1].attach(myservopin[1]);

}


void loop()

{

   Get_gyroscope_And_Control();   //根据陀螺仪传感器的数据实现姿态跟随

}

注意：本次实验测试数据对应的程序位置如下图所示，大家可尝试输入自己的实验数据参数，修改这段程序，使舵机云台的姿态跟随更加流畅。