舵机云台实现体感姿态跟随功能

1. 功能说明

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

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

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

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

      编程环境：Arduino 1.8.19

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

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

 3.1 测试数据

     根据已测数据编写程序，下面提供一个参考例程（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   //调用舵机库函数
 
#include
 
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();   //根据陀螺仪传感器的数据实现姿态跟随
 
}

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