基于麦克纳姆履带陆空结合环境探索系统--学术论文.docx

附件2：
编号：

第十五届“挑战杯”全国大学生
课外学术科技作品竞赛
作品申报书

作品名称：陆空结合环境探索系统

学院名称：机电工程学院

申报者姓名
（集体名称）：基电工程

类别：
■自然科学类学术论文
□哲学社会科学类社会调查报告和学术论文
□科技发明制作A类
□科技发明制作B类

基于麦克纳姆履带陆空结合探测系统
摘要
本文设计陆空结合履带车，车体基于麦克纳姆轮带，在传统麦克纳姆轮360度运动上履带可克服复杂地形，而相较于传统火星车，陆空结合设计将飞机代替摄像头，平时收在履带车内，摄像头露出与传统火星车无区别，但当需要侦查周遭环境可陆空结合实时侦测周围，可应用于人类无法进入的恶劣环境工作采样观察，而且可在第一时间空投抗灾地区为被困灾民提供支援。经多次物理模拟此方案确实可行，目前正在制作样机，因技术工艺较为复杂，履带仍需要时间加工制作。

关键词：麦克纳姆轮 飞行器 空间探索 陆空结合

一， 设计任务
以传统火星车探索机器人“好奇号”为基础，将传统履带与麦克纳姆轮进行结合，实现在克服复杂地形的同时达到增加横向自由度的目的。使车的灵活性大大增加，减少车的操作难度。并且通过四旋翼进行空中监测，相较于传统火星车大大增加了车身的可利用空间，而飞行器也增大观察的灵活度。
1.1应用对象
星际探索，核泄漏等人类无法适应的困难环境进行实验作业。在抗震救灾中能第一时间投放灾区并向被困灾民运送物资或搬运工作。
1.2论文灵感
当看到麦克纳姆轮能横着走的时候非常的好奇，而且本身也对外形探索拥有浓厚的兴趣，于是经过大量计算设计了这款麦克纳姆履带。本论文作品以麦克纳母履带为基本思路，在增加横向移动的基础上改良的麦克纳母轮无法适应复杂地形的缺点，并且通过空中无人机监控周围环境。
1.3作品的科学性、先进性及独特之处
相较于传统移动设备和加工设备其自由度和活动范围往往因为结构的缺点而受到限制，因为通过履带和麦克纳母轮的结合克服了复杂地形以及无法横向移动的困难。而通过轮子和机械手臂的结合改变了机械加工范围受限以及搬运困难的难题。而空中摄像头则大大改善了传统将摄像头安装在机构上视野受限以及阻碍其他机构运动的问题。本文将通过履带车，飞行器两个模块进行说明。

二， 履带车模块
2.1履带车
2.1.1好奇号（谨以此模块对NASA科学家表达敬意）
好奇号，这个代表全世界好奇之心的小东西，有着自己的大脑、眼睛、身体、手臂和腿脚。我身长约3.0m，宽约2.7m，高约2.1m，大小和一辆小型SUV汽车相当。和他的兄弟勇气号和机遇号相比，好奇号重多了，达到850～900kg。好奇号运行的计算机系统内存为256Mbyte，闪存为2Gbyte。好奇号有10只眼睛。我这些凝聚了智慧的火眼金睛，像孙悟空的眼睛一样，能洞察瞬息变化的世间万物。好奇号的的头上、身上、手臂上都长着眼睛，能让他从各个方向、各个角度对这片红色热土进行探索。好奇号虽然只有1只手臂，但是这只手臂非常灵活。它的手臂有3个关节：肩、肘和手腕。好奇号的手臂能够像人类的手臂那样伸展、弯曲和定位，可以拍摄照片、打磨岩石、分析岩石和土壤的组成，能干许许多多的活。
好奇号有6条腿。和人类的腿不一样，它的6条腿是6个车轮。好奇号的2条前腿和2条后腿能够独立转向。我还能够在原地转弯360°，翻越约65～75cm。
2.1.2履带车差速控制（基于模糊运算）
履带车辆具有良好的机动性和越野性，因此，被广泛应用于民用和军用等领域。电传动履带车一般采用电池和双交流电机驱动，通过分配两侧电机转速或转矩实现对车辆两侧驱动力的协调控制，从而达到直线行驶或转弯目的。永磁同步电机相对于传统交流电机具有高效、高功率密度以及良好的调速性能，正逐渐成为新能源车辆领域驱动电机的首选之一。其在电传动履带驱动领域已经得到了广泛的应用。
相比普通机械车辆，由于采用双电机分别驱动两侧主动轮的电传动履带车取消了转向和变速机构，可通过实时控制两侧电机的转速或转矩输出实现直线行走或转弯。为此需要设计差速控制，一般来说可以通过整车控制器实现电子差速控制功能。整车控制器通过总线技术和电机控制器进行交互通讯，电机控制器实现差速设定值的自调整控制，实现电机驱动系统进行动力分配。电传动履带车辆通过整车控制器协调控制两侧驱动电机的转速或转矩，实现任意转向，提高车辆转向稳定性 [7-10] 。电子差速转向控制是电传动履带车辆的关键技术之一，特别是驱动电机的控制特性直接影响差速转向性能。电子差速算法是基于采样标定值、智能控制理论等设计相应的控制策略，控制左右2个驱动电机转速或转矩，从而实现电动汽车差速转向。
本模块介绍的履带车模块运算已经在ican创新创业大赛的作品上进行了应用，并且已经证明切实可行。
1，整车控制器设计
此图为差速控制系统结构图

电传动履带车每个电机有单独的控制器，整车电子差速控制器通过接收无人驾驶车主控单元给定的车辆速度和转角以及来自传感器的反馈等信号，根据内部控制单元计算每个电机需要提供的转速，并向电机控制器发出指令，调节电机的转速，从而控制各个驱动车轮的转速。
整车控制器就是汽车上动力总成控制器，它是整个汽车的核心控制部件。整车控制器主要功能是通过采集车速信号 v 、驾驶转角信号后，计算出输出信号ω1和ω2，也就是电机的期望输入信号。
本模块的控制策略如下：
首先整车控制器根据输入 v和驾驶转角信号。通过下面的模糊建模离线标定出输出模糊模型，然后在线采集在线实时计算出左右电机的期望输入，通过 CAN 总线传递给电机控制器。接下来电机控制器用矢量控制算法调整电机的输出，从而达到电机的期望输入。

3 标定整车控制器的差速模糊模型
本模块大多数为手稿计算，且很多符号本人无法输出，故上传文稿，给您带来不便十分抱歉。
然后采用加权平均反模糊化可得到下面的模糊模型。其中，参数向量的取值为结论变量模糊集对应隶属函数值取得最大值时所对应的点 。 同理，也可以得到右电机的期望转速模糊模型。通过上面的离线标定模糊模型后，整车控制器上电后可直接运行该模型。通过实时检测到的参数向量和 v 值在线实时计算电机的期望转速w1和w2差速系统控制系统结构图，

4交流电机矢量控制
矢量控制是当前工业系统变频系统应用的主流，它是通过分析电机数学模型对电压、电流等变量进行解耦而实现的。转矩方程表明，如果 d 轴励磁电流分量恒定，则永磁同步电机转矩与 q 轴转矩电流分量成正比，这也是矢量控制的基本控制思路。
永磁同步电机矢量控制可以采用电压法实现

2.2麦克纳姆履带
在竞赛机器人和特殊工种机器人中，全向移动经常是一个必需的功能。「全向移动」意味着可以在平面内做出任意方向平移同时自转的动作。为了实现全向移动，一般机器人会使用「全向轮」（Omni Wheel）或「麦克纳姆轮」（Mecanum Wheel）这两种特殊轮子。作者在去年的ican创新大赛中第一次看到了麦轮，并产生了浓厚的兴趣。
2.2.1工作原理
其实麦克纳姆履带和麦克纳姆轮工作原理相同，只是通过履带达到克服复杂地形的能力。轮毂和辊子（roller）。轮毂是整个轮子的主体支架，辊子则是安装在轮毂上的鼓状物。全向轮的轮毂轴与辊子转轴相互垂直，而麦克纳姆轮的轮毂轴与辊子转轴呈 45° 角。理论上，这个夹角可以是任意值，根据不同的夹角可以制作出不同的轮子，但最常用的还是这两种。

全向轮与麦克纳姆轮（以下简称「麦轮」）在结构、力学特性、运动学特性上都有差异，其本质原因是轮毂轴与辊子转轴的角度不同。经过分析，二者的运动学和力学特性区别可以通过以下表格来体现。

计算过程如下，供参考。

近年来，麦轮的应用逐渐增多，特别是在 Robocon、FRC 等机器人赛事上。这是因为麦克纳姆轮可以像传统轮子一样，安装在相互平行的轴上。而若想使用全向轮完成类似的功能，几个轮毂轴之间的角度就必须是 60°，90° 或 120° 等角度，这样的角度生产和制造起来比较麻烦。所以许多工业全向移动平台都是使用麦克纳姆轮而不是全向轮。

2.2.2安装
麦轮一般是四个一组使用，两个左旋轮，两个右旋轮。左旋轮和右旋轮呈手性对称，安装方式有多种，主要分为：X-正方形（X-square）、X-长方形（X-rectangle）、O-正方形（O-square）、O-长方形（O-rectangle）。其中 X 和 O 表示的是与四个轮子地面接触的辊子所形成的图形；正方形与长方形指的是四个轮子与地面接触点所围成的形状。

· X-正方形：轮子转动产生的力矩会经过同一个点，所以 yaw 轴无法主动旋转，也无法主动保持 yaw 轴的角度。一般几乎不会使用这种安装方式。
· X-长方形：轮子转动可以产生 yaw 轴转动力矩，但转动力矩的力臂一般会比较短。这种安装方式也不多见。
· O-正方形：四个轮子位于正方形的四个顶点，平移和旋转都没有任何问题。受限于机器人底盘的形状、尺寸等因素，这种安装方式虽然理想，但可遇而不可求。
· O-长方形：轮子转动可以产生 yaw 轴转动力矩，而且转动力矩的力臂也比较长。是最常见的安装方式。
2.2.3麦轮底盘的正逆运动学模型
以O-长方形的安装方式为例，四个轮子的着地点形成一个矩形。正运动学模型（forward kinematic model）将得到一系列公式，让我们可以通过四个轮子的速度，计算出底盘的运动状态；而逆运动学模型（inverse kinematic model）得到的公式则是可以根据底盘的运动状态解算出四个轮子的速度。需要注意的是，底盘的运动可以用三个独立变量来描述：X轴平动、Y轴平动、yaw 轴自转；而四个麦轮的速度也是由四个独立的电机提供的。所以四个麦轮的合理速度是存在某种约束关系的，逆运动学可以得到唯一解，而正运动学中不符合这个约束关系的方程将无解。
先试图构建逆运动学模型，由于麦轮底盘的数学模型比较复杂，我们在此分四步进行：
①将底盘的运动分解为三个独立变量来描述；
②根据第一步的结果，计算出每个轮子轴心位置的速度；
③根据第二步的结果，计算出每个轮子与地面接触的辊子的速度；
④根据第三部的结果，计算出轮子的真实转速。
一、底盘运动的分解
我们知道，刚体在平面内的运动可以分解为三个独立分量：X轴平动、Y轴平动、yaw 轴自转。如下图所示，底盘的运动也可以分解为三个量：
 表示 X 轴运动的速度，即左右方向，定义向右为正；
 表示 Y 轴运动的速度，即前后方向，定义向前为正；
 表示 yaw 轴自转的角速度，定义逆时针为正。
以上三个量一般都视为四个轮子的几何中心（矩形的对角线交点）的速度。

二、计算出轮子轴心位置的速度
定义：
 为从几何中心指向轮子轴心的矢量；
 为轮子轴心的运动速度矢量；
 为轮子轴心沿垂直于  的方向（即切线方向）的速度分量；
那么可以计算出：

分别计算 X、Y 轴的分量为：

同理可以算出其他三个轮子轴心的速度。

三、计算辊子的速度
根据轮子轴心的速度，可以分解出沿辊子方向的速度  和垂直于辊子方向的速度  。其中  是可以无视的（思考题：为什么垂直方向的速度可以无视？），而

其中  是沿辊子方向的单位矢量。

四、计算轮子的速度
从辊子速度到轮子转速的计算比较简单：

根据上图所示的  和  的定义，有

结合以上四个步骤，可以根据底盘运动状态解算出四个轮子的转速：

以上方程组就是O-长方形麦轮底盘的逆运动学模型，而正运动学模型可以直接根据逆运动学模型中的三个方程解出来，此处不再赘述。

2.2.3控制程序
根据麦轮的底盘的运动学模型，要完全控制它的运动，需要有三个控制量：X轴速度、Y轴速度、自转角速度。要产生这三个控制量，有很多种方法，本文将使用一个 USB 游戏手柄，左边的摇杆产生平移速度，右边的摇杆产生角速度。
首先将一个 USB Host 模块连接到 Orion 主板的 3 口。

然后插上一个无线 USB 游戏手柄。

然后再添加其他细节，就大功告成啦！

其他细节：
#include <Wire.h>
#include <SoftwareSerial.h>
#include "MeOrion.h"

MeUSBHost joypad(PORT_3);
// 手柄代码（红灯亮模式）
// 默认：128-127-128-127-15-0-0-128
// 左一：128-127-128-127-15-1-0-128
// 右一：128-127-128-127-15-2-0-128
// 左二：128-127-128-127-15-4-0-128
// 右二：128-127-128-127-15-8-0-128
// 三角：128-127-128-127-31-0-0-128 （0001 1111）
// 方形：128-127-128-127-143-0-0-128 （1000 1111）
// 叉号：128-127-128-127-79-0-0-128 （0100 1111）
// 圆圈：128-127-128-127-47-0-0-128 （0010 1111）
// 向上：128-127-128-127-0-0-0-128 （0000 0000）
// 向下：128-127-128-127-4-0-0-128 （0000 0100）
// 向左：128-127-128-127-6-0-0-128 （0000 0110）
// 向右：128-127-128-127-2-0-0-128 （0000 0010）
// 左上：128-127-128-127-7-0-0-128 （0000 0111）
// 左下：128-127-128-127-5-0-0-128 （0000 0101）
// 右上：128-127-128-127-1-0-0-128 （0000 0001）
// 右下：128-127-128-127-3-0-0-128 （0000 0011）
// 选择：128-127-128-127-15-16-0-128
// 开始：128-127-128-127-15-32-0-128
// 摇杆：右X-右Y-左X-左Y-15-0-0-128

MeEncoderMotor motor1(0x02, SLOT2);
MeEncoderMotor motor2(0x02, SLOT1);
MeEncoderMotor motor3(0x0A, SLOT2);
MeEncoderMotor motor4(0x0A, SLOT1);

// 底盘：a = 130mm, b = 120mm

float linearSpeed = 100;
float angularSpeed = 100;
float maxLinearSpeed = 200;
float maxAngularSpeed = 200;
float minLinearSpeed = 30;
float minAngularSpeed = 30;

void setup()
{
// 要上电才能工作，不能只是插上 USB 线来调试。
motor1.begin();
motor2.begin();
motor3.begin();
motor4.begin();

Serial.begin(57600);
joypad.init(USB1_0);
}

void loop()
{
Serial.println("loop:");
//setEachMotorSpeed(100, 50, 50, 100);
if(!joypad.device_online)
{
// 若一直输出离线状态，重新拔插 USB Host 的 RJ25 线试一下。
Serial.println("Device offline.");
joypad.probeDevice();
delay(1000);
}
else
{
int len = joypad.host_recv();
parseJoystick(joypad.RECV_BUFFER);
delay(5);
}
//delay(500);
}

void setEachMotorSpeed(float speed1, float speed2, float speed3, float speed4)
{
motor1.runSpeed(speed1);
motor2.runSpeed(-speed2);
motor3.runSpeed(-speed3);
motor4.runSpeed(-speed4);
}

void parseJoystick(unsigned char *buf) //Analytic function, print 8 bytes from USB Host
{
// 输出手柄的数据，调试用
// int i = 0;
// for(i = 0; i < 7; i++)
// {
// Serial.print(buf[i]); //It won't work if you connect to the Makeblock Orion.
// Serial.print('-');
// }
// Serial.println(buf[7]);
// delay(10);

// 速度增减
switch (buf[5])
{
case 1:
linearSpeed += 5;
if (linearSpeed > maxLinearSpeed)
{
linearSpeed = maxLinearSpeed;
}
break;
case 2:
angularSpeed += 5;
if (angularSpeed > maxAngularSpeed)
{
angularSpeed = maxAngularSpeed;
}
break;
case 4:
linearSpeed -= 5;
if (linearSpeed < minLinearSpeed)
{
linearSpeed = minLinearSpeed;
}
break;
case 8:
angularSpeed -= 5;
if (angularSpeed < minAngularSpeed)
{
angularSpeed = minAngularSpeed;
}
break;
default:
break;
}

if ((128 != buf[0]) || (127 != buf[1]) || (128 != buf[2]) || (127 != buf[3]))
{
// 处理摇杆
float x = ((float)(buf[2]) - 127) / 128;
float y = (127 - (float)(buf[3])) / 128;
float a = (127 - (float)(buf[0])) / 128;
mecanumRun(x * linearSpeed, y * linearSpeed, a * angularSpeed);
}
else
{
switch (buf[4])
{
case 0:
mecanumRun(0, linearSpeed, 0);
break;
case 4:
mecanumRun(0, -linearSpeed, 0);
break;
case 6:
mecanumRun(-linearSpeed, 0, 0);
break;
case 2:
mecanumRun(linearSpeed, 0, 0);
break;
case 7:
mecanumRun(-linearSpeed/2, linearSpeed/2, 0);
break;
case 5:
mecanumRun(-linearSpeed/2, -linearSpeed/2, 0);
break;
case 1:
mecanumRun(linearSpeed/2, linearSpeed/2, 0);
break;
case 3:
mecanumRun(linearSpeed/2, -linearSpeed/2, 0);
break;
default:
mecanumRun(0, 0, 0);
break;
}
}
}

void mecanumRun(float xSpeed, float ySpeed, float aSpeed)
{
float speed1 = ySpeed - xSpeed + aSpeed;
float speed2 = ySpeed + xSpeed - aSpeed;
float speed3 = ySpeed - xSpeed - aSpeed;
float speed4 = ySpeed + xSpeed + aSpeed;

float max = speed1;
if (max < speed2) max = speed2;
if (max < speed3) max = speed3;
if (max < speed4) max = speed4;

if (max > maxLinearSpeed)
{
speed1 = speed1 / max * maxLinearSpeed;
speed2 = speed2 / max * maxLinearSpeed;
speed3 = speed3 / max * maxLinearSpeed;
speed4 = speed4 / max * maxLinearSpeed;
}

setEachMotorSpeed(speed1, speed2, speed3, speed4);
}
附上理论成品图

三， 飞行器模块
四旋翼飞行器是无人飞行器中一个热门的研究分支，随着惯性导航技术的发展与惯导传感器精度的提高，四旋翼飞行器在近些年得到了快速的发展。 
为了满足四旋翼飞行的设计要求，系统以STM32F103VET6作为四旋翼自主飞行器控制的核心，处理器内核为ARM32位Cortex-M3 CPU，最高72MHz工作频率，工作电压3.3V-5.5V。该四旋翼由电源模块、电机电调调速控制模块、传感器检测模块、飞行器控制模块等构成。飞行姿态检测模块是通过采用MPU-6050模块，整合3轴陀螺仪、3轴加速度计，检测飞行器实时飞行姿态，实现飞行器运动速度和转向的精准控制。传感器检测模块包括红外障碍传感器、超声波测距模块，在动力学模型的基础上，将四旋翼飞行器实时控制算法分为两个PID 控制回路，即位置控制回路和姿态控制回路。测试结果表明系统可通过各个模块的配合实现对电机的精确控制，具有平均速度快、定位误差小、运行较为稳定等特点。
3.1电机的选择与论证 
四旋翼无人飞行器是通过控制四个不同无刷直流电机的转速，达到控制四旋翼无人飞行器的飞行姿态和位置，与传统直升机通过控制舵机来改变螺旋桨的桨距角，达到控制直升机的目的不同。在电机的选型上，主要有直流有刷电机和直流无刷电机两种。 
方案一：直流有刷电机是当前普遍使用的一种直流电机，它的驱动电路简单、控制方法成熟，但是直流有刷电机使用电刷进行换向，换向时电刷与线圈触电存在机械接触，电机长时间高速转动使极易因磨损导致电气接触不良等问题，而且有刷电机效率低、力矩小、重量大，不适合对功率重量比敏感的电动小型飞行器。     方案二：直流无刷电机能量密度高、力矩大、重量轻，采用非接触式的电子换向方法，消除了电刷磨损，较好地解决了直流有刷电机的缺点，适用于对功率重量比敏感的用途，同时增强了电机的可靠性。所以选择直流无刷电机作为动力源。
电机驱动方案的选择与论证 
方案一:采用电阻网络或数字电位器调整电动机的分压，从而达到调速的目的。但是电阻网络只能实现有级调速，而数字电阻的元器件价格比较昂贵。更主要的问题在于一般电动机的电阻很小，但电流很大；分压不仅会降低效率，而且实现很困难。 
方案二:采用继电器对电动机的开或关进行控制，通过开关的切换对小车的速度进行调整。这个方案的优点是电路较为简单，缺点是继电器的响应时间慢、机械结 构易损坏、寿命较短、可靠性不高。 
方案三:采用全桥驱动PWM 电路。这种驱动的优点是使管子工作在占空比可调的开关状态，提高使用效率实现电机转速的微调。并且保证了可以简单的方式实现方向控制。 
基于上述理论分析，选择方案三。
3.2PID控制运算
由于四旋翼飞行器由四路电机带动两对反向螺旋桨来产生推力，所以如何保证电机在平稳悬浮或上升状态时转速的一致性及不同动作时各个电机转速的比例关系是飞行器按照期望姿态飞行的关键。所以这里我们采用到pid控制理论把飞机的当前姿态调整到期望姿态。
Pid控制是通过姿态采集模块发送回来的数据与期望姿态进行比对，如果存在误差，就对误差进行比例、积分、微分的调整，再将调整后的值加到当前电机上，从而达到调整的目的。比例调节的反应速度较快，而且调节作用明显，飞机出现俯仰和翻滚时能快速调节回来，但是稳定性较差，往往会调节过火；积分调节可以消除长期误差，排除外界因素的干扰，但是同样会降低系统整体的稳定性，使飞机发生震荡；微分调节可以预测被控设备的将来状态，及时的进行调整，而且对比例调节有抑制作用，加强单比例调节的稳定性，排除调节过度的问题。所以通过pid控制可以完全考虑到整个系统的过去、现在、将来，以使系统达到稳定。
3.3飞行姿态控制单元 
飞行器模拟图如下图，姿态控制是通过陀螺仪模块进行数据的采集，根据它采集回来的俯仰角（pitch），翻滚角（roll），四旋翼采用十字型连接，这样的话能明确分离俯仰姿态和翻滚姿态，进行分别控制。这时如果飞机处于俯仰状态就调机头和机尾的电机，那边高就减小那边电机的转速，相应的那边低则加大那边电机的转速。如果飞机处于翻滚状态，则调左右电机。
3.4 测试方案与测试结果 
3.4.1、硬件测试 
首先，先把四轴飞行器分块拆解，用最小的最轻的元件和电路板按照配重的需要安装在四轴飞行器上。再用物理方法测量重心，使其重心维持在四轴飞行器的中心。 
连线接头要保证其牢固性，电池，主控板，陀螺仪要等机械硬件要使其牢固安装在飞机上。 
这个硬件测试最关键的是PID的是三个参数调试，先调p参数，p参数是调整反应速度和力度的，当它反应迅速且两边等幅振动时即可确定p参数，再调d参数，d参数是一个抑制的作用，抑制它调制过大，使它保持在平衡位置的，当从任意角度都可以一次直接返回平衡位置时即可，最后调i参数，i参数是积分项，当哪一边反应过小时可以加一个i参数。这样就测试出一组适合的PID参数了。 
3.4.2、软件仿真测试 
在调试程序之前，可用串口显示每个电机PWM输出，观察各种姿态下PID控制后电机油门的大小。 
3.4.3、硬件软件联调 
通过stm32编程，模仿出PWM，并测量是否能通过电机驱动来使飞行器起飞，通过多次测试，找出飞行器起飞时的PWM值。 
mpu6050模块通过串口向主控板发送数据，并在电脑上利用串口接收，检测数据是否正确，通过软件编程针对显示的数据进行修改。 
通过mpu6050模块使四轴飞行器稳定的起飞，并悬停在空中；再进行测试，使四轴飞行器前进和后退；最后进行降落的测试。 
通过超声波的测量使其悬停在100cm的空中，在进行前进后退的校准，使其飞行足够准确，之后进行对引导线的识别，使飞机平稳前进。 
最后通过通过对直径为10cm的黑圆圈进行测试，使其测量到，并使四轴飞行器投弹打靶，打靶完毕后降落在黑色圆圈内。
include "data_transfer.h"

#define BYTE0(dwTemp) (*(char *)(&dwTemp))
#define BYTE1(dwTemp) (*((char *)(&dwTemp) + 1))
#define BYTE2(dwTemp) (*((char *)(&dwTemp) + 2))
#define BYTE3(dwTemp) (*((char *)(&dwTemp) + 3))

uint8_t Data_Check,Send_Status,Send_Senser,Send_Offset;

uint8_t data_to_send[50];

void Data_Receive_Anl(uint8_t *data_buf,uint8_t num)
{
int rc_value_temp;
uint8_t sum = 0,i;

for(i = 0;i<(num-1);i++)
sum += *(data_buf+i);
if(!(sum==*(data_buf+num-1))) return;
if(!(*(data_buf)==0xAA && *(data_buf+1)==0xAF)) return;

if(*(data_buf+2)==0X01)
{
if(*(data_buf+4)==0X01)
{
Mode.ACC_CALIBRATED = 1;
}

if(*(data_buf+4)==0X02)
{
Mode.GYRO_CALIBRATED = 1;
}

if(*(data_buf+4)==0X03)
{
Mode.ACC_CALIBRATED = 1;
Mode.GYRO_CALIBRATED = 1;
}
if(*(data_buf+4)==0XA0)
{
Mode.ARMED = 0;
}
if(*(data_buf+4)==0XA1)
{
Mode.ARMED = 1;
}
}

if(*(data_buf+2)==0X02)
{
if(*(data_buf+4)==0X01)
{

}
if(*(data_buf+4)==0X02)
Send_Offset = 1;
}

if(*(data_buf+2)==0X03)
{

}

if(*(data_buf+2)==0X16) //OFFSET
{
Data_Send_Check(sum);
}
}

void Data_Exchange(void)
{
static uint8_t cnt = 0;

if(cnt == 0)
{
Data_Send_Status();
}
else if(cnt == 1)
{
Data_Send_Senser();
}
cnt++;
if(cnt == 2)
{
cnt = 0;
}

else if(Send_Offset)
{
Send_Offset = 0;
Data_Send_OFFSET();
}
}

void Data_Send_Status(void)
{
long _temp2;
int _temp;
uint8_t _cnt=0,sum = 0,i;

data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0xAA;
data_to_send[_cnt++]=0x01;
data_to_send[_cnt++]=0;
_temp = (int)(Angle[ROLL]*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Angle[PITCH]*100);
data_to_send[_cnt++]=BYTE1(_temp);
data_to_send[_cnt++]=BYTE0(_temp);
_temp = (int)(Angle[YAW]*100);
data_to_send[_c