dynamic route

CommandDecoder.cpp

@@ -1,12 +1,12 @@
 // 
-// 
+// ZigBeeÖ¸Áî½âÎö
 // 
 
 #include "CommandDecoder.h"
 
 byte CommandDecoder::_CheckSUM(byte main, byte data1, byte data2, byte data3)
 {
-	return ((byte)((main + data1 + data2 + data3) % 0xFF));
+	return ((byte)((main + data1 + data2 + data3) % 256));
 }
 
 CommandDecoder::CommandDecoder()

DebugOpt.h

@@ -4,7 +4,6 @@
 #include "WirelessChargerCommand.h"
 #include "TrafficLightCommand.h"
 #include "SpecTerrainCommand.h"
-#include "VoiceRepotCommand.h"
 #include "LEDDisplayCommand.h"
 #include "AlarmTowerCommand.h"
 #include "3DDisplayCommand.h"

GlobalDatas.cpp

@@ -8,6 +8,8 @@ uint8_t trackdi_buf[8] = { 0x55,0x02,0x91,0x00,0x00,0x00,0x00,0xBB };	//OpenMVѭ
 
 uint8_t servo_buf[8] = { 0x55,0x02,0x91,0x03,0x00,0x00,0x00,0xBB };   // 给OpenMV发送舵机角度
 
+String dynamic_route_recv;
+
 //全局变量
 uint8_t ATM_Data[ATM_Data_Length];
 uint8_t ZigBee_command[8];

GlobalDatas.h

@@ -36,6 +36,15 @@ namespace CommandData
 		const byte AutoMarkSystem_2 = 0x06;
 		const byte MainCar = 0x02;
 	};
+
+	namespace MainCarCommand
+	{
+		const byte StartRun = 0xC1;
+		const byte StartRunSub = 0x01;
+		const byte RunningRoute = 0xC2;
+		const byte RunningRouteData = 0xC3;
+		const byte RouteDataStop = 0xFF;
+	};
 };
 
 namespace DataTool
@@ -62,6 +71,8 @@ extern uint8_t trackdi_buf[8];	//OpenMVѭ
 
 extern uint8_t servo_buf[8];   // 给OpenMV发送舵机角度
 
+extern String dynamic_route_recv;
+
 //全局变量
 extern uint8_t ATM_Data[ATM_Data_Length];
 extern uint8_t ZigBee_command[8];

Handler.cpp

@@ -132,6 +132,47 @@ void Handler::OpenMVRx_Handler(uint8_t* mac)
 
 void Handler::MainCarRx_Handler(uint8_t* com)
 {
+	CommandDecoder decoder(com);
+	if (!decoder.CommandValidate())
+		return;
+	switch(decoder.GetMainCommand())
+	{
+	case CommandData::MainCarCommand::StartRun:
+		if (decoder.GetDataBit(1) == CommandData::MainCarCommand::StartRunSub)
+		{
+			int point_cnt = 0;
+			AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("B7-B6-D6-F6-F4-D4-B4-B2-D2-D1", point_cnt);
+			AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::X_Positive);
+			delay(1500);
+		}
+		break;
+	case CommandData::MainCarCommand::RunningRoute:
+		dynamic_route_recv = "";
+		dynamic_route_recv.concat((char)decoder.GetDataBit(1));
+		dynamic_route_recv.concat((char)decoder.GetDataBit(2));
+		dynamic_route_recv.concat('-');
+		Serial.print("Route Command: ");
+		Serial.println(dynamic_route_recv);
+		break;
+	case CommandData::MainCarCommand::RunningRouteData:
+		dynamic_route_recv.concat((char)decoder.GetDataBit(1));
+		dynamic_route_recv.concat((char)decoder.GetDataBit(2));
+		if (decoder.GetDataBit(3) == CommandData::MainCarCommand::RouteDataStop)
+		{
+			Serial.println(dynamic_route_recv);
+			Serial.println("Run!");
+			int point_cnt = 0;
+			AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute(dynamic_route_recv.c_str(), point_cnt);
+			AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::X_Positive);
+			delay(1500);
+		}
+		else
+		{
+			dynamic_route_recv.concat('-');
+			Serial.println(dynamic_route_recv);
+		}
+		break;
+	}
 	switch (com[2])
 	{
 	case 0x01:                                         //ֹͣ

Handler.h

@@ -2,6 +2,7 @@
 #define __HANDLER__
 
 #include "VoiceIdentifyCommand.h"
+#include "VoiceReportCommand.h"
 #include "AutoPathRunner.h"
 #include "CommandDecoder.h"
 #include "GlobalDatas.h"