1. 功能说明
本文示例将实现R207样机舵机云台根据六轴陀螺仪传感器数据实现姿态跟随的功能。
2. 电子硬件
在这个示例中，我们采用了以下硬件，请大家参考：
| 主控板 | |
| 扩展板 | |
| 传感器 | 六轴陀螺仪 |
| 电池 | 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(); //根据陀螺仪传感器的数据实现姿态跟随
}注意：本次实验测试数据对应的程序位置如下图所示，大家可尝试输入自己的实验数据参数，修改这段程序，使舵机云台的姿态跟随更加流畅。
体感姿态跟随-程序源代码资料内容详见 舵机云台-体感姿态跟随