commit after race

2021_Arduino_Demo.ino

@@ -16,7 +16,7 @@
 
 void setup()
 {
-	Serial.begin(115200);
+	//Serial.begin(115200);
 	CoreLED.Initialization();
 	CoreKEY.Initialization();
 	CoreBeep.Initialization();

2021_Arduino_Demo.vcxproj

@@ -76,7 +76,7 @@
       <PreprocessorDefinitions>__AVR_atmega2560__;__AVR_ATmega2560__;F_CPU=16000000L;ARDUINO=108010;ARDUINO_AVR_MEGA2560;ARDUINO_ARCH_AVR;__cplusplus=201103L;_VMICRO_INTELLISENSE;%(PreprocessorDefinitions)</PreprocessorDefinitions>
       <LanguageStandard>stdcpp17</LanguageStandard>
       <LanguageStandard_C>stdc11</LanguageStandard_C>
-      <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\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)</AdditionalIncludeDirectories>
+      <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)</AdditionalIncludeDirectories>
     </ClCompile>
     <Link>
       <GenerateDebugInformation>true</GenerateDebugInformation>

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;
 

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

AutoPathRunner.h

@@ -44,8 +44,8 @@ namespace AutoPathRunner
 		MapPoint();
 		MapPoint(int x, int y);
 	public:
-		int GetX();
+		int GetX() const;
-		int GetY();
+		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();
 	//倒车入库

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.print("识别到的词条ID为：");
-	Serial.println(sph_id, HEX);
+	//Serial.println(sph_id, HEX);
 	//voice_trm_buf[2] = sph_id;
 	//ExtSRAMInterface.ExMem_Write_Bytes(0x6008, voice_trm_buf, 8);//上传识别结果至评分终端
 }

GlobalDatas.h

@@ -51,15 +51,15 @@ namespace DataTool
 {
 	template<typename T = uint8_t> 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("arr[");
-			Serial.print(i);
+			//Serial.print(i);
-			Serial.print("] = 0x");
+			//Serial.print("] = 0x");
-			Serial.println(arr[i], HEX);
+			//Serial.println(arr[i], HEX);
 		}
-		Serial.println("------Array End------");
+		//Serial.println("------Array End------");
 	}
 };
 

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.print("Garage at: ");
-			Serial.println(cmd.CurrentPos(mar));
+			//Serial.println(cmd.CurrentPos(mar));
 		}
 		if (cmd.IsIRData(mar))
 		{
-			Serial.print("Front IR: ");
+			//Serial.print("Front IR: ");
-			Serial.println(cmd.FrontIRTiggered(mar) ? "Triggered" : "Not Triggered");
+			//Serial.println(cmd.FrontIRTiggered(mar) ? "Triggered" : "Not Triggered");
-			Serial.print("Back IR: ");
+			//Serial.print("Back IR: ");
-			Serial.println(cmd.BackIRTiggered(mar) ? "Triggered" : "Not Triggered");
+			//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(y);
-			Serial.print("-");
+			//Serial.print("-");
-			Serial.print(m);
+			//Serial.print(m);
-			Serial.print("-");
+			//Serial.print("-");
-			Serial.println(d);
+			//Serial.println(d);
 		}
 		else
-			Serial.println("Invalidate Date");
+			//Serial.println("Invalidate Date");
 		if (cmd.ReadRTCTime(mar, h, mm, s))
 		{
-			Serial.print(h);
+			//Serial.print(h);
-			Serial.print(":");
+			//Serial.print(":");
-			Serial.print(mm);
+			//Serial.print(mm);
-			Serial.print(":");
+			//Serial.print(":");
-			Serial.println(s);
+			//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.print("Rescue Position: ");
-			Serial.println(cmd.ReadRescuePosition(mar), HEX);
+			//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;
-		{
+		CommandEncoder encoder;
-			int point_cnt = 0;
+		encoder.SetDevice(0xAA);	//主车
-			AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("B7-B6-D6-F6-F4-D4-B4-B2-D2-D1", point_cnt);
+		encoder.SetCommand(0xAA, 0xAA, 0xAA, 0xAA);
-			AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::X_Positive);
+		AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt);
-			delay(1500);
+		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.print("Route Command: ");
-		Serial.println(dynamic_route_recv);
+		//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);
+			dynamic_route_recv += "-B7";
-			Serial.println("Run!");
+			//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::MapPoint* points = AutoPathRunner::DecodeRoute("E2-D2-B2-B4-B6-B7", point_cnt);
-	AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::X_Positive);
+	AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::Y_Negitive, AutoRunnerCallback);
 	delay(1500);
-	AccurateMotor.TurnLeft(90);
+	AutoPathRunner::ReversingIntoGarage(true, 3);
-	AccurateMotor.DelayUntilCarStop();
-	AccurateMotor.RunForward(20);
-	AccurateMotor.DelayUntilCarStop();
 }
 
 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;
+	uint8_t data[128] = { 0 };
-	TextEncoder encoder("主车不动安如山，从车乱飞墙撞翻。");
+	OpenMVOpt::OpenMVQr_Disc_StartUp();
-	while(encoder.HasNextChar())
+	if (!OpenMVOpt::OpenMV_WaitQRData(data, 128, 2000))
 	{
-		byte b1, b2;
+		OpenMVOpt::OpenMVQr_Disc_CloseUp();
-		encoder.GetNextChar(b1, b2);
+		LED.LeftTurnOn();
-		ZigBeeOperator.SendCommand(cmd.CMD_ZIGBEE_CustomTextAdd(b1, b2, encoder.IsLastChar()));
+	}
+	else
+	{
+		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');
 	}
 }
 

Handler.h

@@ -11,6 +11,9 @@
 
 namespace Handler
 {
+	//自动运行系统回调
+	void AutoRunnerCallback(String point, AutoPathRunner::CarHeadPosition pos);
+
 	//ZigBee信息接收处理
 	void ZigBeeRx_Handler(uint8_t* mar);
 

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 "";
+}

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);

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.print("Current: ");
-	Serial.println(current, HEX);*/
+	//Serial.println(current, HEX);*/
 	return CalcLevel(light_arr, current);
 }
 

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))

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);