From ac062e5932b391117ae36becbefc57912c1045c7 Mon Sep 17 00:00:00 2001 From: UnknownObject Date: Thu, 22 Jun 2023 15:03:21 +0800 Subject: [PATCH] commit after race --- 2021_Arduino_Demo.ino | 2 +- 2021_Arduino_Demo.vcxproj | 2 +- AccurateMotor.h | 3 +- AutoPathRunner.cpp | 46 ++++++++-- AutoPathRunner.h | 10 ++- DebugOpt.cpp | 4 +- GlobalDatas.h | 12 +-- Handler.cpp | 177 +++++++++++++++++++++++++++----------- Handler.h | 3 + OpenMVOpt.cpp | 51 ++++++++++- OpenMVOpt.h | 4 + RodeLightCommand.cpp | 4 +- TrafficLightCommand.cpp | 12 +++ TrafficLightCommand.h | 3 + __vm/Compile.vmps.xml | 2 +- __vm/Upload.vmps.xml | 2 +- 16 files changed, 260 insertions(+), 77 deletions(-) diff --git a/2021_Arduino_Demo.ino b/2021_Arduino_Demo.ino index a17044f..851ee2f 100644 --- a/2021_Arduino_Demo.ino +++ b/2021_Arduino_Demo.ino @@ -16,7 +16,7 @@ void setup() { - Serial.begin(115200); + //Serial.begin(115200); CoreLED.Initialization(); CoreKEY.Initialization(); CoreBeep.Initialization(); diff --git a/2021_Arduino_Demo.vcxproj b/2021_Arduino_Demo.vcxproj index 65effb8..3b956bd 100644 --- a/2021_Arduino_Demo.vcxproj +++ b/2021_Arduino_Demo.vcxproj @@ -76,7 +76,7 @@ __AVR_atmega2560__;__AVR_ATmega2560__;F_CPU=16000000L;ARDUINO=108010;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions) stdcpp17 stdc11 - $(ProjectDir)..\Arduino;G:\Users\15819\Documents\Arduino\libraries\ExtSRAMInterface;G:\Users\15819\Documents\Arduino\libraries\DCMotor;G:\Users\15819\Documents\Arduino\libraries\MsTimer2;G:\Users\15819\Documents\Arduino\libraries\Command;G:\Users\15819\Documents\Arduino\libraries\BEEP;G:\Users\15819\Documents\Arduino\libraries\Ultrasonic;G:\Users\15819\Documents\Arduino\libraries\BKRC_Voice;G:\Users\15819\Documents\Arduino\libraries\CoreBeep;G:\Users\15819\Documents\Arduino\libraries\CoreKEY;G:\Users\15819\Documents\Arduino\libraries\CoreLED;G:\Users\15819\Documents\Arduino\libraries\BH1750;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\libraries\Wire\src;G:\Users\15819\Documents\Arduino\libraries\Infrares;G:\Users\15819\Documents\Arduino\libraries\LED;G:\Users\15819\Documents\Arduino\libraries\Metro;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\libraries\Wire\src\utility;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\variants\mega;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\cores\arduino;$(ProjectDir)..\..\..\RACECA~1\SUBCAR~1\Arduino;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\\lib\gcc\avr\7.3.0\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\\lib\gcc\avr\7.3.0\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include-fixed;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.2\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.2\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories) + $(ProjectDir)..\Arduino;G:\Users\15819\Documents\Arduino\libraries\ExtSRAMInterface;G:\Users\15819\Documents\Arduino\libraries\DCMotor;G:\Users\15819\Documents\Arduino\libraries\MsTimer2;G:\Users\15819\Documents\Arduino\libraries\Command;G:\Users\15819\Documents\Arduino\libraries\BEEP;G:\Users\15819\Documents\Arduino\libraries\Ultrasonic;G:\Users\15819\Documents\Arduino\libraries\BKRC_Voice;G:\Users\15819\Documents\Arduino\libraries\CoreBeep;G:\Users\15819\Documents\Arduino\libraries\CoreKEY;G:\Users\15819\Documents\Arduino\libraries\CoreLED;G:\Users\15819\Documents\Arduino\libraries\BH1750;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\libraries\Wire\src;G:\Users\15819\Documents\Arduino\libraries\Infrares;G:\Users\15819\Documents\Arduino\libraries\LED;G:\Users\15819\Documents\Arduino\libraries\Metro;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\variants\mega;C:\Users\15819\AppData\Local\arduino15\packages\arduino\hardware\avr\1.8.6\cores\arduino;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\\lib\gcc\avr\7.3.0\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\\lib\gcc\avr\7.3.0\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include-fixed;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\avr\include\avr;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.2\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.2\include;C:\Users\15819\AppData\Local\arduino15\packages\arduino\tools\avr-gcc\7.3.0-atmel3.6.1-arduino7\lib\gcc\avr\4.9.3\include;%(AdditionalIncludeDirectories) true diff --git a/AccurateMotor.h b/AccurateMotor.h index 49cc526..bdf9729 100644 --- a/AccurateMotor.h +++ b/AccurateMotor.h @@ -21,7 +21,8 @@ private: bool car_running; const uint16_t car_speed = 50; const uint16_t car_slow_speed = 10; - const uint16_t car_turn_speed = 100; + //const uint16_t car_turn_speed = 100; + const uint16_t car_turn_speed = 97; const uint16_t turn_multplyer = 7; const uint16_t run_multplyer = 100; diff --git a/AutoPathRunner.cpp b/AutoPathRunner.cpp index 0c72ea4..68fc5cd 100644 --- a/AutoPathRunner.cpp +++ b/AutoPathRunner.cpp @@ -16,12 +16,12 @@ AutoPathRunner::MapPoint::MapPoint(int x, int y) this->y = y; } -int AutoPathRunner::MapPoint::GetX() +int AutoPathRunner::MapPoint::GetX() const { return x; } -int AutoPathRunner::MapPoint::GetY() +int AutoPathRunner::MapPoint::GetY() const { return y; } @@ -160,11 +160,24 @@ AutoPathRunner::MapPoint AutoPathRunner::Mapping(const char* name) return MapPoint(); } -const char* AutoPathRunner::ReverseMapping(MapPoint point) +String AutoPathRunner::ReverseMapping(const MapPoint& point) { + /*//Serial.println("----------Begin Reverse Map----------"); + //Serial.print(String("Target Value: (") + point.GetX()); + //Serial.print(String(",") + point.GetY()); + //Serial.println(")");*/ for (auto& [key, value] : std_points) + { + /*//Serial.print(String("Current Value: (") + value.GetX()); + //Serial.print(String(", ") + value.GetY()); + //Serial.println(")");*/ if (value == point) - return key; + { + ////Serial.println("----------End Reverse Map----------"); + return String(key); + } + } + ////Serial.println("----------End(Error) Reverse Map----------"); return ""; } @@ -258,7 +271,7 @@ AutoPathRunner::MapPoint* AutoPathRunner::DecodeRoute(const char* route, int& po return list; } -void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos) +void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos, MCRCallback cb) { //调整K210舵机至循迹角度 OpenMVOpt::Servo_Control(-60); @@ -268,12 +281,18 @@ void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos) if (i != 1) { //向前一定距离 + //Serial.println("RunForward(3)"); AccurateMotor.RunForward(3); AccurateMotor.DelayUntilCarStop(); + if ((cb != nullptr) && (!last_point.NeedTurn(points[i], pos))) + { + //Serial.println("Callback Here"); + cb(ReverseMapping(last_point), pos); + } } if (last_point.NeedTurn(points[i], pos)) { - //TurnFlag = true; + //Serial.print("Turn "); switch (last_point.CalcTurnDirection(points[i], pos)) { case TurnDirection::Left: @@ -281,23 +300,32 @@ void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos) AccurateMotor.TurnLeft(90); AccurateMotor.DelayUntilCarStop(); ChangePositon(pos, TurnDirection::Left); + //Serial.println("Left"); break; case TurnDirection::Right: //右转 AccurateMotor.TurnRight(90); AccurateMotor.DelayUntilCarStop(); ChangePositon(pos, TurnDirection::Right); + //Serial.println("Right"); break; default: break; } + if (cb != nullptr) + { + //Serial.println("Callback Here"); + cb(ReverseMapping(last_point), pos); + } //向前循迹 - OpenMVOpt::OpenMV_Track(8, TrackCallback); + OpenMVOpt::OpenMV_Track(8, TrackCallback, false); + ////Serial.println("OpenMVOpt::OpenMV_Track(8, TrackCallback, false);"); } else { //向前循迹 - OpenMVOpt::OpenMV_Track(8, TrackCallback); + OpenMVOpt::OpenMV_Track(8, TrackCallback, false); + ////Serial.println("OpenMVOpt::OpenMV_Track(8, TrackCallback, false);"); } last_point = points[i]; } @@ -362,7 +390,7 @@ void AutoPathRunner::ReversingIntoGarage(bool a_b_select, uint8_t target_floor) } } } while (true); - AccurateMotor.TurnLeft(90); + AccurateMotor.TurnLeft(180); AccurateMotor.DelayUntilCarStop(); OpenMVOpt::OpenMV_TrackInPlace(); do diff --git a/AutoPathRunner.h b/AutoPathRunner.h index 369b824..3c64feb 100644 --- a/AutoPathRunner.h +++ b/AutoPathRunner.h @@ -44,8 +44,8 @@ namespace AutoPathRunner MapPoint(); MapPoint(int x, int y); public: - int GetX(); - int GetY(); + int GetX() const; + int GetY() const; void SetX(int x); void SetY(int y); bool NeedTurn(MapPoint next, CarHeadPosition pos); @@ -64,8 +64,10 @@ namespace AutoPathRunner PointMapper(const char* n, const MapPoint& p); }; + typedef void(*MCRCallback)(String point, CarHeadPosition pos); + MapPoint Mapping(const char* name); - const char* ReverseMapping(MapPoint point); + String ReverseMapping(const MapPoint& point); void ChangePositon(CarHeadPosition& pos, TurnDirection turn); CarHeadPosition Int2Position(int pos); bool TrackCallback(); @@ -73,7 +75,7 @@ namespace AutoPathRunner //解析路线 MapPoint* DecodeRoute(const char* route, int& point_cnt); //执行路径指令 - void DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos); + void DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos, MCRCallback cb = nullptr); //区分RFID卡和特殊地形标志物 bool IsSpecTerrainItem(); //倒车入库 diff --git a/DebugOpt.cpp b/DebugOpt.cpp index 90575c6..86f3fac 100644 --- a/DebugOpt.cpp +++ b/DebugOpt.cpp @@ -112,8 +112,8 @@ void DebugOpt::Speech_Disc() 2-6为指定词条识别，具体信息可查看通信协议*/ sph_id = BKRC_Voice.BKRC_Voice_Extern(0); delay(200); - Serial.print("识别到的词条ID为："); - Serial.println(sph_id, HEX); + //Serial.print("识别到的词条ID为："); + //Serial.println(sph_id, HEX); //voice_trm_buf[2] = sph_id; //ExtSRAMInterface.ExMem_Write_Bytes(0x6008, voice_trm_buf, 8);//上传识别结果至评分终端 } diff --git a/GlobalDatas.h b/GlobalDatas.h index 053c5ae..28549d3 100644 --- a/GlobalDatas.h +++ b/GlobalDatas.h @@ -51,15 +51,15 @@ namespace DataTool { template void PrintDataArray(T* arr, uint8_t len) { - Serial.println("------Data Array------"); + //Serial.println("------Data Array------"); for (uint8_t i = 0; i < len; i++) { - Serial.print("arr["); - Serial.print(i); - Serial.print("] = 0x"); - Serial.println(arr[i], HEX); + //Serial.print("arr["); + //Serial.print(i); + //Serial.print("] = 0x"); + //Serial.println(arr[i], HEX); } - Serial.println("------Array End------"); + //Serial.println("------Array End------"); } }; diff --git a/Handler.cpp b/Handler.cpp index 4eb2d2f..858c5a7 100644 --- a/Handler.cpp +++ b/Handler.cpp @@ -1,5 +1,67 @@ #include "Handler.h" +void Handler::AutoRunnerCallback(String point, AutoPathRunner::CarHeadPosition pos) +{ + //Serial.print("Callback("); + //Serial.print((point == "") ? "EMPTY" : point); + //Serial.println(String(",") + (int)pos + ")"); + AccurateMotor.ForceStop(); + OpenMVOpt::OpenMVTrack_Disc_CloseUp(); + AccurateMotor.ForceStop(); + if ((point == "D2")/* && (pos == AutoPathRunner::CarHeadPosition::Y_Negitive)*/) + { + TrafficLightCommand cmd; + ZigBeeOperator.SendCommand(cmd.CMD_EnterIdentifyMode()); + delay(500); + ZigBeeOperator.SendCommand(cmd.CMD_CheckRandomResult()); + delay(500); + AccurateMotor.RunBackwardSlow(3); + AccurateMotor.DelayUntilCarStop(); + OpenMVOpt::OpenMV_TrackInPlace(); + AccurateMotor.RunForwardSlow(9); + AccurateMotor.DelayUntilCarStop(); + } + else if (point == "B4") + { + _3DDisplayCommand cmd; + uint8_t data[128] = { 0 }; + AccurateMotor.TurnLeft(45); + AccurateMotor.DelayUntilCarStop(); + cmd.CMD_IR_CarLicenseMode_1to4('A', 'B', 'C', 'D'); + delay(200); + cmd.CMD_IR_CarLicenseMode_5to6_XY('E', 'F', 'E', '2'); + AccurateMotor.TurnRight(45); + AccurateMotor.DelayUntilCarStop(); + /*AccurateMotor.TurnRight(90); + AccurateMotor.DelayUntilCarStop(); + OpenMVOpt::Servo_Control(0); + delay(500); + OpenMVOpt::OpenMVQr_Disc_StartUp(); + bool ret = OpenMVOpt::OpenMV_WaitQRData(data, 128, 2000); + OpenMVOpt::OpenMVQr_Disc_CloseUp(); + AccurateMotor.TurnLeft(135); + AccurateMotor.DelayUntilCarStop(); + if (!ret) + { + cmd.CMD_IR_CarLicenseMode_1to4('A', 'B', 'C', 'D'); + delay(200); + cmd.CMD_IR_CarLicenseMode_5to6_XY('E', 'F', 'E', '2'); + } + else + { + String str = OpenMVOpt::DecodeQRData(data, 128); + _3DDisplayCommand cmd; + cmd.CMD_IR_CarLicenseMode_1to4(str[0], str[1], str[2], str[3]); + delay(200); + cmd.CMD_IR_CarLicenseMode_5to6_XY(str[4], str[5], 'E', '2'); + } + OpenMVOpt::Servo_Control(-60); + delay(500); + AccurateMotor.TurnRight(135); + AccurateMotor.DelayUntilCarStop();*/ + } +} + void Handler::ZigBeeRx_Handler(uint8_t* mar) { CommandDecoder decoder(mar); @@ -18,15 +80,15 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar) GarageCommand cmd; if (cmd.IsPositionData(mar)) { - Serial.print("Garage at: "); - Serial.println(cmd.CurrentPos(mar)); + //Serial.print("Garage at: "); + //Serial.println(cmd.CurrentPos(mar)); } if (cmd.IsIRData(mar)) { - Serial.print("Front IR: "); - Serial.println(cmd.FrontIRTiggered(mar) ? "Triggered" : "Not Triggered"); - Serial.print("Back IR: "); - Serial.println(cmd.BackIRTiggered(mar) ? "Triggered" : "Not Triggered"); + //Serial.print("Front IR: "); + //Serial.println(cmd.FrontIRTiggered(mar) ? "Triggered" : "Not Triggered"); + //Serial.print("Back IR: "); + //Serial.println(cmd.BackIRTiggered(mar) ? "Triggered" : "Not Triggered"); } break; } @@ -37,24 +99,24 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar) byte m, d, h, mm, s; if (cmd.ReadRTCDate(mar, y, m, d)) { - Serial.print(y); - Serial.print("-"); - Serial.print(m); - Serial.print("-"); - Serial.println(d); + //Serial.print(y); + //Serial.print("-"); + //Serial.print(m); + //Serial.print("-"); + //Serial.println(d); } else - Serial.println("Invalidate Date"); + //Serial.println("Invalidate Date"); if (cmd.ReadRTCTime(mar, h, mm, s)) { - Serial.print(h); - Serial.print(":"); - Serial.print(mm); - Serial.print(":"); - Serial.println(s); + //Serial.print(h); + //Serial.print(":"); + //Serial.print(mm); + //Serial.print(":"); + //Serial.println(s); } else - Serial.println("Invalidate Time"); + //Serial.println("Invalidate Time"); break; } case CommandData::Devices::AlarmTower: //红外报警标志物 @@ -62,8 +124,8 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar) AlarmTowerCommand cmd; if (cmd.IsAlarmTowerCommand(mar)) { - Serial.print("Rescue Position: "); - Serial.println(cmd.ReadRescuePosition(mar), HEX); + //Serial.print("Rescue Position: "); + //Serial.println(cmd.ReadRescuePosition(mar), HEX); } break; } @@ -138,38 +200,45 @@ void Handler::MainCarRx_Handler(uint8_t* com) 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); - } + int point_cnt = 0; + CommandEncoder encoder; + encoder.SetDevice(0xAA); //主车 + encoder.SetCommand(0xAA, 0xAA, 0xAA, 0xAA); + AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt); + AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback); + ZigBeeOperator.SendCommand(encoder.GetCommandArray()); + delay(200); + ZigBeeOperator.SendCommand(encoder.GetCommandArray()); + OpenMVOpt::Servo_Control(-60); + delay(1500); + AutoPathRunner::ReversingIntoGarage(true, 3); 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); + //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!"); + dynamic_route_recv += "-B7"; + //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); + AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback); delay(1500); + AutoPathRunner::ReversingIntoGarage(true, 3); } else { dynamic_route_recv.concat('-'); - Serial.println(dynamic_route_recv); + //Serial.println(dynamic_route_recv); } break; } @@ -278,20 +347,22 @@ void Handler::MainCarRx_Handler(uint8_t* com) void Handler::Key_0() { 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); + AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt); + AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback); delay(1500); - AccurateMotor.TurnLeft(90); - AccurateMotor.DelayUntilCarStop(); - AccurateMotor.RunForward(20); - AccurateMotor.DelayUntilCarStop(); + AutoPathRunner::ReversingIntoGarage(true, 3); } void Handler::Key_1() { - OpenMVOpt::OpenMVTrack_Disc_CloseUp(); + /*OpenMVOpt::OpenMVTrack_Disc_CloseUp(); delay(500); - OpenMVOpt::Servo_Control(0); + OpenMVOpt::Servo_Control(0);*/ + AccurateMotor.TurnLeft(90); + AccurateMotor.DelayUntilCarStop(); + delay(2000); + AccurateMotor.TurnLeft(180); + AccurateMotor.DelayUntilCarStop(); } void Handler::Key_2() @@ -299,19 +370,29 @@ void Handler::Key_2() OpenMVOpt::Servo_Control(-60); delay(500); OpenMVOpt::OpenMV_Track(8, []() { return false; }, false); - AutoPathRunner::ReversingIntoGarage(false, 0x04); + AutoPathRunner::ReversingIntoGarage(true, 0x03); } void Handler::Key_3() { - _3DDisplayCommand cmd; - TextEncoder encoder("主车不动安如山，从车乱飞墙撞翻。"); - while(encoder.HasNextChar()) + uint8_t data[128] = { 0 }; + OpenMVOpt::OpenMVQr_Disc_StartUp(); + if (!OpenMVOpt::OpenMV_WaitQRData(data, 128, 2000)) + { + OpenMVOpt::OpenMVQr_Disc_CloseUp(); + LED.LeftTurnOn(); + } + else { - byte b1, b2; - encoder.GetNextChar(b1, b2); - ZigBeeOperator.SendCommand(cmd.CMD_ZIGBEE_CustomTextAdd(b1, b2, encoder.IsLastChar())); + OpenMVOpt::OpenMVQr_Disc_CloseUp(); + LED.RightTurnOn(); + AccurateMotor.TurnLeft(135); + AccurateMotor.DelayUntilCarStop(); + String str = OpenMVOpt::DecodeQRData(data, 128); + _3DDisplayCommand cmd; + cmd.CMD_IR_CarLicenseMode_1to4(str[0], str[1], str[2], str[3]); delay(200); + cmd.CMD_IR_CarLicenseMode_5to6_XY(str[4], str[5], 'B', '6'); } } diff --git a/Handler.h b/Handler.h index 6dcbccc..7951d6e 100644 --- a/Handler.h +++ b/Handler.h @@ -11,6 +11,9 @@ namespace Handler { + //自动运行系统回调 + void AutoRunnerCallback(String point, AutoPathRunner::CarHeadPosition pos); + //ZigBee信息接收处理 void ZigBeeRx_Handler(uint8_t* mar); diff --git a/OpenMVOpt.cpp b/OpenMVOpt.cpp index 9f57dbf..ff4c109 100644 --- a/OpenMVOpt.cpp +++ b/OpenMVOpt.cpp @@ -153,7 +153,7 @@ void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_wh if(Data_OpenMVBuf[2] == 0x91) { num++; - Serial.println(num); + //Serial.println(num); if (Data_OpenMVBuf[4] == 1) // 路口 { DCMotor.Stop(); @@ -226,3 +226,52 @@ void OpenMVOpt::Servo_Control(int8_t angle) ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8); delay(1000); } + +bool OpenMVOpt::OpenMV_WaitQRData(uint8_t* data, uint16_t max_len, uint16_t timeout) +{ + uint16_t time_passed = 0; + while (ExtSRAMInterface.ExMem_Read(0x6038) == 0x00) + { + delay(1); + if (timeout != 0) + { + time_passed++; + if (time_passed >= timeout) + return false; + } + } + ExtSRAMInterface.ExMem_Read_Bytes(0x6038, data, max_len); + if ((data[0] != 0x55) || (data[1] != 0x92) || (data[2] != 0x01)) + return false; + else + return true; +} + +String OpenMVOpt::DecodeQRData(uint8_t* data, uint16_t len) +{ + bool require_letter = true; + bool require_number = false; + uint8_t data_len = data[3]; + String result = ""; + for (int i = 0; i < (data_len + 4); i++) + { + if (require_letter && ((data[i] >= 'A') && (data[i] <= 'Z'))) + { + require_letter = false; + require_number = true; + result.concat((char)data[i]); + } + else if (require_number && ((data[i] >= '0') && (data[i] <= '9'))) + { + result.concat((char)data[i]); + if (result.length() >= 4) + { + require_letter = true; + require_number = false; + } + } + if (result.length() >= 6) + return result; + } + return ""; +} diff --git a/OpenMVOpt.h b/OpenMVOpt.h index 54ea1b0..2df2d3d 100644 --- a/OpenMVOpt.h +++ b/OpenMVOpt.h @@ -49,6 +49,10 @@ namespace OpenMVOpt void OpenMVQr_Disc_StartUp(); //停止二维码识别 void OpenMVQr_Disc_CloseUp(); + //等待二维码数据回传（超时：毫秒） + bool OpenMV_WaitQRData(uint8_t* data, uint16_t max_len, uint16_t timeout = 0); + //解析二维码数据 + String DecodeQRData(uint8_t* data, uint16_t len); //舵机角度控制 void Servo_Control(int8_t angle); diff --git a/RodeLightCommand.cpp b/RodeLightCommand.cpp index 19bc20d..32f7965 100644 --- a/RodeLightCommand.cpp +++ b/RodeLightCommand.cpp @@ -75,8 +75,8 @@ byte RodeLightCommand::GetCurrentLevel() if (light_arr[i] < light_arr[j]) Swap(light_arr[i], light_arr[j]); /*DataTool::PrintDataArray(light_arr, 4); - Serial.print("Current: "); - Serial.println(current, HEX);*/ + //Serial.print("Current: "); + //Serial.println(current, HEX);*/ return CalcLevel(light_arr, current); } diff --git a/TrafficLightCommand.cpp b/TrafficLightCommand.cpp index ea697b1..18c0fc3 100644 --- a/TrafficLightCommand.cpp +++ b/TrafficLightCommand.cpp @@ -4,6 +4,12 @@ #include "TrafficLightCommand.h" +uint8_t TrafficLightCommand::RandomDisc() +{ + randomSeed(micros()); + return random(0x01, 0x04); +} + TrafficLightCommand::TrafficLightCommand() { SetDevice(CommandData::Devices::TrafficLight_A); @@ -31,6 +37,12 @@ byte* TrafficLightCommand::CMD_CheckResult(Color color) return GetCommandArray(); } +byte* TrafficLightCommand::CMD_CheckRandomResult() +{ + SetCommand(0x02, RandomDisc()); + return GetCommandArray(); +} + bool TrafficLightCommand::IsTrafficLightCommand(byte* cmd) { if ((cmd[1] == CommandData::Devices::TrafficLight_A) || (cmd[1] == CommandData::Devices::TrafficLight_B)) diff --git a/TrafficLightCommand.h b/TrafficLightCommand.h index e75d237..4a9e892 100644 --- a/TrafficLightCommand.h +++ b/TrafficLightCommand.h @@ -20,6 +20,8 @@ public: Green = 0x02, Yellow = 0x03 }; +private: + uint8_t RandomDisc(); public: TrafficLightCommand(); public: @@ -28,6 +30,7 @@ public: public: byte* CMD_EnterIdentifyMode(); byte* CMD_CheckResult(Color color); + byte* CMD_CheckRandomResult(); public: bool IsTrafficLightCommand(byte* cmd); bool ModeChangeSuccess(byte* cmd); diff --git a/__vm/Compile.vmps.xml b/__vm/Compile.vmps.xml index 92d0c6d..6318dfc 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 92d0c6d..6318dfc 100644 --- a/__vm/Upload.vmps.xml +++ b/__vm/Upload.vmps.xml @@ -2,7 +2,7 @@ - +