diff --git a/CommandDecoder.cpp b/CommandDecoder.cpp index 2ea6b4f..71f6ce7 100644 --- a/CommandDecoder.cpp +++ b/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() diff --git a/DebugOpt.h b/DebugOpt.h index a99aa8a..e1924a7 100644 --- a/DebugOpt.h +++ b/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" diff --git a/GlobalDatas.cpp b/GlobalDatas.cpp index acb4f1a..6c63558 100644 --- a/GlobalDatas.cpp +++ b/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]; diff --git a/GlobalDatas.h b/GlobalDatas.h index de7163e..053c5ae 100644 --- a/GlobalDatas.h +++ b/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]; diff --git a/Handler.cpp b/Handler.cpp index 8be17f5..4eb2d2f 100644 --- a/Handler.cpp +++ b/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: //停止 diff --git a/Handler.h b/Handler.h index d1bad38..6dcbccc 100644 --- a/Handler.h +++ b/Handler.h @@ -2,6 +2,7 @@ #define __HANDLER__ #include "VoiceIdentifyCommand.h" +#include "VoiceReportCommand.h" #include "AutoPathRunner.h" #include "CommandDecoder.h" #include "GlobalDatas.h" diff --git a/__vm/Compile.vmps.xml b/__vm/Compile.vmps.xml index 9d09372..92d0c6d 100644 --- a/__vm/Compile.vmps.xml +++ b/__vm/Compile.vmps.xml @@ -2,7 +2,7 @@ - + diff --git a/__vm/Upload.vmps.xml b/__vm/Upload.vmps.xml index 9d09372..92d0c6d 100644 --- a/__vm/Upload.vmps.xml +++ b/__vm/Upload.vmps.xml @@ -2,7 +2,7 @@ - +