升级TT飞机，拥有前向避障+向上避障 简单

/*!
 * MindPlus
 * telloesp32
 *
 */
#include <RMTT_Libs.h>
#include <DFRobot_URM10.h>
#include <DFRobot_VL53L0X.h>
// 创建对象
DFRobot_VL53L0X vl53l0x;
RMTT_RGB        tt_rgb;
RMTT_Protocol   protocol;
DFRobot_URM10   urm10;


// 主程序开始
void setup() {
	tt_rgb.Init();
	led_effect_init();
	Serial1.begin(1000000, 23, 18, SERIAL_8N1);
	// 此传感器的使用之前得执行很多的初始化工作，这个积木干这个活
	vl53l0x.begin();
	// 此时设置为低精度，够用而且降低处理器的负载，测量模式设置为连续
	vl53l0x.setMode(vl53l0x.Low, vl53l0x.Continuous);
	// 此时让我们的RGB在两个颜色之间变换
	led_effect_blink(255, 255, 255, 255, 255, 255, 1);
	delay(1000);
	// 不要忘记这个积木的选择
	protocol.startUntilControl();
	delay(1000);
	// 飞机预热中
	protocol.sendTelloCtrlMsg("motoron");
	delay(1000);
	// 向上飞行50厘米
	protocol.sendTelloCtrlMsg("takeoff");
	// 飞机离地一米
	protocol.sendTelloCtrlMsg((char *)String(String("up ")+int(50)).c_str());
}
void loop() {
	// 此时飞机离地1.5米，再向前飞50厘米
	protocol.sendTelloCtrlMsg((char *)String(String("up ")+int(50)).c_str());
	protocol.sendTelloCtrlMsg((char *)String(String("forward ")+int(50)).c_str());
	// 这个是单说向前的传感器的情况
	// 此时超声波是向上避障，监测是否小于25厘米
	if (((urm10.getDistanceCM(13, 14))<25)) {
		// 报警的颜色，红色
		tt_rgb.SetRGB(255,0,0);
		// 悬停，不要做任何动作，开始避开障碍
		protocol.sendTelloCtrlMsg("stop");
		delay(1000);
		// 先下降20厘米，避开先上的障碍物
		protocol.sendTelloCtrlMsg((char *)String(String("down ")+int(20)).c_str());
		delay(1000);
		// 就是不在一个直角的伽罗里面，两个传感器的值都很大
		// 此时超声波测量大于25的话，执行
		if ((((urm10.getDistanceCM(13, 14))>25) && (vl53l0x.getDistance()<25))) {
			// 寻找三个方向去避障
			for (int index = 0; index < 3; index++) {
				tt_rgb.SetRGB(0,0,255);
				protocol.sendTelloCtrlMsg((char *)String(String("cw ")+int(90)).c_str());
				protocol.sendTelloCtrlMsg((char *)String(String("forward ")+int(50)).c_str());
			}
		}
	}
	else if ((vl53l0x.getDistance()<25)) {
		protocol.sendTelloCtrlMsg("stop");
		delay(1000);
		protocol.sendTelloCtrlMsg((char *)String(String("back ")+int(50)).c_str());
		// 也是循环三个方向寻找无障碍的方向飞行
		for (int index = 0; index < 3; index++) {
			if ((vl53l0x.getDistance()<25)) {
				tt_rgb.SetRGB(0,255,0);
				delay(1000);
				protocol.sendTelloCtrlMsg((char *)String(String("cw ")+int(90)).c_str());
				protocol.sendTelloCtrlMsg((char *)String(String("forward ")+int(20)).c_str());
			}
		}
	}
	else {
		// 这个代码的意思是如果两个传感器的值是相同的，先倒后降。然后降落
		if (((urm10.getDistanceCM(13, 14))==vl53l0x.getDistance())) {
			tt_rgb.SetRGB(255,255,0);
			protocol.sendTelloCtrlMsg((char *)String(String("back ")+int(20)).c_str());
			protocol.sendTelloCtrlMsg((char *)String(String("down ")+int(20)).c_str());
			delay(1000);
			protocol.sendTelloCtrlMsg("land");
		}
	}
}