commit all

2021_Arduino_Demo.ino

@@ -1,3 +1,4 @@
+#include "AutoPathRunner.h"
 #include "ZigBeeOperator.h"
 #include "CommandEncoder.h"
 #include "CommandDecoder.h"
@@ -38,9 +39,6 @@ void setup()
 
 void loop()
 {
-	uint8_t si = 0;
-	frisrtime = millis();
-
 	CoreKEY.Kwhile(Handler::KEY_Handler);                                  //按键检测
 
 	if (ExtSRAMInterface.ExMem_Read(ZigBee_Read_Address) != 0x00)              //从车接收ZigBee数据
@@ -65,28 +63,33 @@ void loop()
 		}
 	}
 
-	if (((millis() - frisrtime >= TSendCycle) || (Tcount >= TSendCycle)) && (sendflag == 1))  //获取、上传任务版数据
-	{
-		uint16_t tp = (uint16_t)(Ultrasonic.Ranging(CM) * 10.0);
-		ZigBee_back[5] = (tp >> 8) & 0xff;
-		ZigBee_back[4] = tp & 0xff;
-
-		tp = BH1750.ReadLightLevel();
-		ZigBee_back[7] = (tp >> 8) & 0xff;
-		ZigBee_back[6] = tp & 0xff;
-
-		ZigBee_back[9] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6003);
-		if (ZigBee_back[9] >= 0x80)	ZigBee_back[9] = 0xff - ZigBee_back[9];
-		ZigBee_back[8] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6002);
-
-		ExtSRAMInterface.ExMem_Write_Bytes(0x6080, ZigBee_back, 16);
-		Tcount = 0x00;
-	}
-	else if (sendflag == 1)
-	{
-		Tcount += (millis() - frisrtime);
-	}
-
 	//空闲时进行流水灯
 	DebugOpt::DoLEDFlow();
 }
+
+//获取和上传任务板数据：已禁用。
+//{
+//	uint8_t si = 0;
+//	frisrtime = millis();
+//	if (((millis() - frisrtime >= TSendCycle) || (Tcount >= TSendCycle)) && (sendflag == 1))  //获取、上传任务版数据
+//	{
+//		uint16_t tp = (uint16_t)(Ultrasonic.Ranging(CM) * 10.0);
+//		ZigBee_back[5] = (tp >> 8) & 0xff;
+//		ZigBee_back[4] = tp & 0xff;
+//
+//		tp = BH1750.ReadLightLevel();
+//		ZigBee_back[7] = (tp >> 8) & 0xff;
+//		ZigBee_back[6] = tp & 0xff;
+//
+//		ZigBee_back[9] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6003);
+//		if (ZigBee_back[9] >= 0x80)	ZigBee_back[9] = 0xff - ZigBee_back[9];
+//		ZigBee_back[8] = (uint8_t)ExtSRAMInterface.ExMem_Read(0x6002);
+//
+//		ExtSRAMInterface.ExMem_Write_Bytes(0x6080, ZigBee_back, 16);
+//		Tcount = 0x00;
+//	}
+//	else if (sendflag == 1)
+//	{
+//		Tcount += (millis() - frisrtime);
+//	}
+//}

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\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\Command;G:\Users\15819\Documents\Arduino\libraries\BEEP;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\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>
     </ClCompile>
     <Link>
       <GenerateDebugInformation>true</GenerateDebugInformation>
@@ -107,6 +107,7 @@
   <ItemGroup>
     <ClInclude Include="AccurateMotor.h" />
     <ClInclude Include="AlarmTowerCommand.h" />
+    <ClInclude Include="AutoPathRunner.h" />
     <ClInclude Include="BarrierCommand.h" />
     <ClInclude Include="CommandDecoder.h" />
     <ClInclude Include="CommandEncoder.h" />
@@ -132,6 +133,7 @@
   <ItemGroup>
     <ClCompile Include="AccurateMotor.cpp" />
     <ClCompile Include="AlarmTowerCommand.cpp" />
+    <ClCompile Include="AutoPathRunner.cpp" />
     <ClCompile Include="BarrierCommand.cpp" />
     <ClCompile Include="CommandDecoder.cpp" />
     <ClCompile Include="CommandEncoder.cpp" />

2021_Arduino_Demo.vcxproj.filters

@@ -87,6 +87,9 @@
     <ClInclude Include="TextEncoder.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="AutoPathRunner.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="OpenMVOpt.cpp">
@@ -152,5 +155,8 @@
     <ClCompile Include="TextEncoder.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="AutoPathRunner.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>

AccurateMotor.cpp

@@ -19,7 +19,7 @@ void AccurateMotorClass::TurnLeft(uint8_t degree)
 {
 	if (car_running)
 		return;
-	DCMotor.TurnLeft(car_speed, car_speed);
+	DCMotor.TurnLeft(car_turn_speed, car_turn_speed);
 	SetCarRunning();
 	MsTimer2::set(degree * turn_multplyer, []()
 		{
@@ -43,7 +43,7 @@ void AccurateMotorClass::TurnRight(uint8_t degree)
 {
 	if (car_running)
 		return;
-	DCMotor.TurnRight(car_speed, car_speed);
+	DCMotor.TurnRight(car_turn_speed, car_turn_speed);
 	SetCarRunning();
 	MsTimer2::set(degree * turn_multplyer, []()
 		{
@@ -82,6 +82,7 @@ void AccurateMotorClass::DelayUntilCarStop()
 {
 	while (car_running)
 		delay(10);
+	delay(100);	//缓冲，防止因惯性导致位置偏差
 }
 
 void AccurateMotorClass::RunBackward(uint8_t distence)

AccurateMotor.h

@@ -20,7 +20,8 @@ class AccurateMotorClass
 private:
 	bool car_running;
 	const uint8_t car_speed = 50;
-	const uint8_t turn_multplyer = 13;
+	const uint8_t car_turn_speed = 100;
+	const uint8_t turn_multplyer = 8;
 	const uint8_t run_multplyer = 100;
 
 private:

AutoPathRunner.cpp

@@ -0,0 +1,334 @@
+// 
+// 自动路径运行程序 - 修改自原MCR，去除了对STL库的依赖
+// 
+
+#include "AutoPathRunner.h"
+
+AutoPathRunner::MapPoint::MapPoint()
+{
+	x = 0;
+	y = 0;
+}
+
+AutoPathRunner::MapPoint::MapPoint(int x, int y)
+{
+	this->x = x;
+	this->y = y;
+}
+
+int AutoPathRunner::MapPoint::GetX()
+{
+	return x;
+}
+
+int AutoPathRunner::MapPoint::GetY()
+{
+	return y;
+}
+
+void AutoPathRunner::MapPoint::SetX(int x)
+{
+	this->x = x;
+}
+
+void AutoPathRunner::MapPoint::SetY(int y)
+{
+	this->y = y;
+}
+
+bool AutoPathRunner::MapPoint::NeedTurn(MapPoint next, CarHeadPosition pos)
+{
+	if ((pos == CarHeadPosition::X_Positive) || (pos == CarHeadPosition::X_Negitive))
+		return (x != next.GetX());
+	else
+		return (y != next.GetY());
+}
+
+AutoPathRunner::TurnDirection AutoPathRunner::MapPoint::CalcTurnDirection(MapPoint next, CarHeadPosition pos)
+{
+	TurnDirection direction;
+	if (x == next.GetX())
+	{
+		if (y < next.GetY())
+			direction = TurnDirection::Right;
+		else if (y > next.GetY())
+			direction = TurnDirection::Left;
+		else
+			direction = TurnDirection::Null;
+	}
+	else if (y == next.GetY())
+	{
+		if (x < next.GetX())
+			direction = TurnDirection::Right;
+		else if (x > next.GetX())
+			direction = TurnDirection::Left;
+		else
+			direction = TurnDirection::Null;
+	}
+	else
+		direction = TurnDirection::Null;
+	if ((pos == CarHeadPosition::X_Negitive) || (pos == CarHeadPosition::Y_Negitive))
+	{
+		if (direction == TurnDirection::Left)
+			direction = TurnDirection::Right;
+		else if (direction == TurnDirection::Right)
+			direction = TurnDirection::Left;
+	}
+	return direction;
+}
+
+AutoPathRunner::MapPoint AutoPathRunner::MapPoint::operator=(const MapPoint& obj)
+{
+	this->x = obj.x;
+	this->y = obj.y;
+	return (*this);
+}
+
+bool AutoPathRunner::MapPoint::operator==(const MapPoint& obj)
+{
+	return ((x == obj.x) && (y == obj.y));
+}
+
+AutoPathRunner::PointMapper::PointMapper()
+{
+}
+
+AutoPathRunner::PointMapper::PointMapper(const char* n, const MapPoint& p)
+{
+	strncpy(name, n, 3);
+	point = p;
+}
+
+AutoPathRunner::PointMapper std_points[49] =
+{
+	{"A1", AutoPathRunner::MapPoint(0,0)},
+	{"A2", AutoPathRunner::MapPoint(0,1)},
+	{"A3", AutoPathRunner::MapPoint(0,2)},
+	{"A4", AutoPathRunner::MapPoint(0,3)},
+	{"A5", AutoPathRunner::MapPoint(0,4)},
+	{"A6", AutoPathRunner::MapPoint(0,5)},
+	{"A7", AutoPathRunner::MapPoint(0,6)},
+	{"B1", AutoPathRunner::MapPoint(1,0)},
+	{"B2", AutoPathRunner::MapPoint(1,1)},
+	{"B3", AutoPathRunner::MapPoint(1,2)},
+	{"B4", AutoPathRunner::MapPoint(1,3)},
+	{"B5", AutoPathRunner::MapPoint(1,4)},
+	{"B6", AutoPathRunner::MapPoint(1,5)},
+	{"B7", AutoPathRunner::MapPoint(1,6)},
+	{"C1", AutoPathRunner::MapPoint(2,0)},
+	{"C2", AutoPathRunner::MapPoint(2,1)},
+	{"C3", AutoPathRunner::MapPoint(2,2)},
+	{"C4", AutoPathRunner::MapPoint(2,3)},
+	{"C5", AutoPathRunner::MapPoint(2,4)},
+	{"C6", AutoPathRunner::MapPoint(2,5)},
+	{"C7", AutoPathRunner::MapPoint(2,6)},
+	{"D1", AutoPathRunner::MapPoint(3,0)},
+	{"D2", AutoPathRunner::MapPoint(3,1)},
+	{"D3", AutoPathRunner::MapPoint(3,2)},
+	{"D4", AutoPathRunner::MapPoint(3,3)},
+	{"D5", AutoPathRunner::MapPoint(3,4)},
+	{"D6", AutoPathRunner::MapPoint(3,5)},
+	{"D7", AutoPathRunner::MapPoint(3,6)},
+	{"E1", AutoPathRunner::MapPoint(4,0)},
+	{"E2", AutoPathRunner::MapPoint(4,1)},
+	{"E3", AutoPathRunner::MapPoint(4,2)},
+	{"E4", AutoPathRunner::MapPoint(4,3)},
+	{"E5", AutoPathRunner::MapPoint(4,4)},
+	{"E6", AutoPathRunner::MapPoint(4,5)},
+	{"E7", AutoPathRunner::MapPoint(4,6)},
+	{"F1", AutoPathRunner::MapPoint(5,0)},
+	{"F2", AutoPathRunner::MapPoint(5,1)},
+	{"F3", AutoPathRunner::MapPoint(5,2)},
+	{"F4", AutoPathRunner::MapPoint(5,3)},
+	{"F5", AutoPathRunner::MapPoint(5,4)},
+	{"F6", AutoPathRunner::MapPoint(5,5)},
+	{"F7", AutoPathRunner::MapPoint(5,6)},
+	{"G1", AutoPathRunner::MapPoint(6,0)},
+	{"G2", AutoPathRunner::MapPoint(6,1)},
+	{"G3", AutoPathRunner::MapPoint(6,2)},
+	{"G4", AutoPathRunner::MapPoint(6,3)},
+	{"G5", AutoPathRunner::MapPoint(6,4)},
+	{"G6", AutoPathRunner::MapPoint(6,5)},
+	{"G7", AutoPathRunner::MapPoint(6,6)}
+};
+
+AutoPathRunner::MapPoint AutoPathRunner::Mapping(const char* name)
+{
+	for (auto& [key, value] : std_points)
+		if (strcmp(key, name) == 0)
+			return value;
+	return MapPoint();
+}
+
+const char* AutoPathRunner::ReverseMapping(MapPoint point)
+{
+	for (auto& [key, value] : std_points)
+		if (value == point)
+			return key;
+	return "";
+}
+
+void AutoPathRunner::ChangePositon(CarHeadPosition& pos, TurnDirection turn)
+{
+	switch (pos)
+	{
+	case CarHeadPosition::X_Positive:
+		if (turn == TurnDirection::Left)
+			pos = CarHeadPosition::Y_Negitive;
+		else if (turn == TurnDirection::Right)
+			pos = CarHeadPosition::Y_Positive;
+		break;
+	case CarHeadPosition::X_Negitive:
+		if (turn == TurnDirection::Left)
+			pos = CarHeadPosition::Y_Positive;
+		else if (turn == TurnDirection::Right)
+			pos = CarHeadPosition::Y_Negitive;
+		break;
+	case CarHeadPosition::Y_Positive:
+		if (turn == TurnDirection::Left)
+			pos = CarHeadPosition::X_Positive;
+		else if (turn == TurnDirection::Right)
+			pos = CarHeadPosition::X_Negitive;
+		break;
+	case CarHeadPosition::Y_Negitive:
+		if (turn == TurnDirection::Left)
+			pos = CarHeadPosition::X_Negitive;
+		else if (turn == TurnDirection::Right)
+			pos = CarHeadPosition::X_Positive;
+		break;
+	}
+}
+
+AutoPathRunner::CarHeadPosition AutoPathRunner::Int2Position(int pos)
+{
+	switch (pos)
+	{
+	case 0:
+		return CarHeadPosition::X_Positive;
+	case 1:
+		return CarHeadPosition::X_Negitive;
+	case 2:
+		return CarHeadPosition::Y_Positive;
+	default:
+		return CarHeadPosition::Y_Negitive;
+	}
+}
+
+bool AutoPathRunner::TrackCallback()
+{
+	AccurateMotor.RunBackward(6);
+	AccurateMotor.DelayUntilCarStop();
+	if (IsSpecTerrainItem())
+		AccurateMotor.RunForward(30);
+	else
+	{
+		AccurateMotor.RunForward(12);
+	}
+	AccurateMotor.DelayUntilCarStop();
+	return true;
+}
+
+AutoPathRunner::MapPoint* AutoPathRunner::DecodeRoute(const char* route, int& point_cnt)
+{
+	char temp[3] = { 0 };
+	MapPoint* list = new MapPoint[(strlen(route) + 1) / 3];
+	int str_index = 0, point_index = 0;
+	char* route_cpy = new char[strlen(route) + 1];
+	memset(route_cpy, 0, strlen(route) + 1);
+	strncpy(route_cpy, route, strlen(route));
+	memset(list, 0, sizeof(list));
+	while (sscanf(route_cpy, "%c%c-", &temp[0], &temp[1]) != 0)
+	{
+		str_index += 3;
+		if (str_index >= strlen(route))
+			break;
+		if (point_index < ((strlen(route) + 1) / 3))
+			list[point_index++] = Mapping(temp);
+		memset(temp, 0, sizeof(temp));
+		memset(route_cpy, 0, strlen(route) + 1);
+		strncpy(route_cpy, route + str_index, strlen(route) - str_index);
+	}
+	point_cnt = point_index;
+	delete[] route_cpy;
+	return list;
+}
+
+void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos)
+{
+	//调整K210舵机至循迹角度
+	OpenMVOpt::Servo_Control(-60);
+	MapPoint last_point = points[0];
+	for (int i = 1; i < point_cnt; i++)
+	{
+		if (i != 1)
+		{
+			//向前一定距离
+			AccurateMotor.RunForward(3);
+			AccurateMotor.DelayUntilCarStop();
+		}
+		if (last_point.NeedTurn(points[i], pos))
+		{
+			//TurnFlag = true;
+			switch (last_point.CalcTurnDirection(points[i], pos))
+			{
+			case TurnDirection::Left:
+				//左转
+				AccurateMotor.TurnLeft(90);
+				AccurateMotor.DelayUntilCarStop();
+				ChangePositon(pos, TurnDirection::Left);
+				break;
+			case TurnDirection::Right:
+				//右转
+				AccurateMotor.TurnRight(90);
+				AccurateMotor.DelayUntilCarStop();
+				ChangePositon(pos, TurnDirection::Right);
+				break;
+			default:
+				break;
+			}
+			//向前循迹
+			OpenMVOpt::OpenMV_Track(8, TrackCallback);
+		}
+		else
+		{
+			//向前循迹
+			OpenMVOpt::OpenMV_Track(8, TrackCallback);
+		}
+		last_point = points[i];
+	}
+	delete[] points;
+	//恢复K210舵机角度
+	delay(1000);
+	OpenMVOpt::OpenMVTrack_Disc_CloseUp();
+	delay(500);
+	OpenMVOpt::Servo_Control(0);
+}
+
+bool AutoPathRunner::IsSpecTerrainItem()
+{
+	double dis = Ultrasonic.Ranging(CM);
+	AccurateMotor.TurnLeft(90);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.RunForward(5);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.TurnRight(90);
+	AccurateMotor.DelayUntilCarStop();
+	delay(200);
+	double dis_left = Ultrasonic.Ranging(CM);
+	AccurateMotor.TurnRight(90);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.RunForward(10);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.TurnLeft(90);
+	AccurateMotor.DelayUntilCarStop();
+	delay(200);
+	double dis_right = Ultrasonic.Ranging(CM);
+	AccurateMotor.TurnRight(90);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.RunBackward(5);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.TurnLeft(90);
+	AccurateMotor.DelayUntilCarStop();
+	delay(200);
+	return (((dis - dis_left) >= 10) || ((dis - dis_right) >= 10));
+}

AutoPathRunner.h

@@ -0,0 +1,79 @@
+// AutoPathRunner.h
+
+#ifndef _AUTOPATHRUNNER_h
+#define _AUTOPATHRUNNER_h
+
+#if defined(ARDUINO) && ARDUINO >= 100
+	#include "arduino.h"
+#else
+	#include "WProgram.h"
+#endif
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include "OpenMVOpt.h"
+#include <Ultrasonic.h>
+#include "AccurateMotor.h"
+
+namespace AutoPathRunner
+{
+	enum class TurnDirection
+	{
+		Null,
+		Left,
+		Right
+	};
+
+	enum class CarHeadPosition
+	{
+		X_Positive,
+		Y_Positive,
+		X_Negitive,
+		Y_Negitive
+	};
+
+	class MapPoint
+	{
+	private:
+		int x, y;
+	public:
+		MapPoint();
+		MapPoint(int x, int y);
+	public:
+		int GetX();
+		int GetY();
+		void SetX(int x);
+		void SetY(int y);
+		bool NeedTurn(MapPoint next, CarHeadPosition pos);
+		TurnDirection CalcTurnDirection(MapPoint next, CarHeadPosition pos);
+	public:
+		MapPoint operator=(const MapPoint& obj);
+		bool operator==(const MapPoint& obj);
+	};
+
+	struct PointMapper
+	{
+		char name[3] = { 0 };
+		MapPoint point;
+
+		PointMapper();
+		PointMapper(const char* n, const MapPoint& p);
+	};
+
+	MapPoint Mapping(const char* name);
+	const char* ReverseMapping(MapPoint point);
+	void ChangePositon(CarHeadPosition& pos, TurnDirection turn);
+	CarHeadPosition Int2Position(int pos);
+	bool TrackCallback();
+
+	//解析路线
+	MapPoint* DecodeRoute(const char* route, int& point_cnt);
+	//执行路径指令
+	void DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos);
+	//区分RFID卡和特殊地形标志物
+	bool IsSpecTerrainItem();
+};
+
+#endif
+

DebugOpt.cpp

@@ -45,7 +45,7 @@ void DebugOpt::DoLEDFlow()
 	led_flow_cnt++;
 	if (led_flow_cnt >= 4)
 		led_flow_cnt = 0;
-	delay(10);
+	delay(15);
 }
 
 void DebugOpt::DebugRun()

Handler.cpp

@@ -236,20 +236,30 @@ void Handler::MainCarRx_Handler(uint8_t* com)
 
 void Handler::Key_0()
 {
-	OpenMVOpt::Servo_Control(-60);
-	OpenMVOpt::OpenMV_Track(1);
+	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);
+	AccurateMotor.TurnLeft(90);
+	AccurateMotor.DelayUntilCarStop();
+	AccurateMotor.RunForward(20);
+	AccurateMotor.DelayUntilCarStop();
 }
 
 void Handler::Key_1()
 {
-	_3DDisplayCommand cmd;
-	Infrare.Transmition(cmd.CMD_IR_DisplayAlarmSign(_3DDisplayCommand::AlarmSign::NoDrinkDrive), 6);
+	OpenMVOpt::OpenMVTrack_Disc_CloseUp();
+	delay(500);
+	OpenMVOpt::Servo_Control(0);
 }
 
 void Handler::Key_2()
 {
-	_3DDisplayCommand cmd;
-	Infrare.Transmition(cmd.CMD_IR_SetTextColor(0x00, 0xFF, 0x00), 6);
+	OpenMVOpt::Servo_Control(-60);
+	delay(500);
+	OpenMVOpt::OpenMV_Track(5, []() { return true; }, false);
+	delay(500);
+	OpenMVOpt::Servo_Control(0);
 }
 
 void Handler::Key_3()
@@ -263,8 +273,6 @@ void Handler::Key_3()
 		ZigBeeOperator.SendCommand(cmd.CMD_ZIGBEE_CustomTextAdd(b1, b2, encoder.IsLastChar()));
 		delay(200);
 	}
-	/*OpenMVOpt::Servo_Control(-60);
-	OpenMVOpt::OpenMV_Track(1);*/
 }
 
 void Handler::KEY_Handler(uint8_t k_value)

Handler.h

@@ -1,8 +1,10 @@
 #ifndef __HANDLER__
 #define __HANDLER__
 
+#include "AutoPathRunner.h"
 #include "CommandDecoder.h"
 #include "GlobalDatas.h"
+#include <Ultrasonic.h>
 #include "DebugOpt.h"
 
 namespace Handler

OpenMVOpt.cpp

@@ -1,6 +1,7 @@
 #include "OpenMVOpt.h"
 
 uint8_t OpenMVOpt::Data_OpenMVBuf[8] = { 0 };
+uint8_t OpenMVOpt::ROI_Query_CMD[8] = { 0x55,0x02,0x80,0x00,0x00,0x00,0x00,0xBB };
 
 void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
 {
@@ -67,11 +68,11 @@ void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
 
 		if (Data_OpenMVBuf[5] == 43)     // +  向左调
 		{
-			DCMotor.SpeedCtr(-5, 30);
+			DCMotor.SpeedCtr(-20, 40);
 		}
 		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
 		{
-			DCMotor.SpeedCtr(30, -5);
+			DCMotor.SpeedCtr(40, -20);
 		}
 	}
 	else if (Data_OpenMVBuf[6] > 70)
@@ -79,15 +80,49 @@ void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
 
 		if (Data_OpenMVBuf[5] == 43)     // +  向左调
 		{
-			DCMotor.SpeedCtr(-10, 20);
+			DCMotor.SpeedCtr(-30, 30);
 		}
 		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
 		{
-			DCMotor.SpeedCtr(20, -10);
+			DCMotor.SpeedCtr(30, -30);
 		}
 	}
 }
 
+OpenMVOpt::ImageROIData OpenMVOpt::GetROIData()
+{
+	ImageROIData data = { 0 };
+	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, ROI_Query_CMD, 8);
+read_data:
+	while (ExtSRAMInterface.ExMem_Read(0x6038) == 0x00)
+		delay(1);
+	ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
+	if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02))
+	{
+		if (Data_OpenMVBuf[2] == 0x81)
+		{
+			data.left = Data_OpenMVBuf[3];
+			data.right = Data_OpenMVBuf[4];
+			data.up = Data_OpenMVBuf[5];
+			goto read_data;
+		}
+		else if (Data_OpenMVBuf[2] == 0x82)
+		{
+			data.middle_up = Data_OpenMVBuf[3];
+			data.middle_down = Data_OpenMVBuf[3];
+			data.down = Data_OpenMVBuf[3];
+			data.data_validate = true;
+			return data;
+		}
+	}
+	return { 0 };
+}
+
+bool OpenMVOpt::ROIFoundWhite(const OpenMVOpt::ImageROIData& data)
+{
+	return ((!data.up) || (!data.middle_up) || (!data.middle_down) || (!data.down));
+}
+
 void OpenMVOpt::OpenMVTrack_Disc_StartUp()
 {
 	trackdi_buf[3] = 0x01;                 //开始识别
@@ -102,24 +137,25 @@ void OpenMVOpt::OpenMVTrack_Disc_CloseUp()
 	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
 }
 
-void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed)
+void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_white_detect)
 {
 	uint32_t num = 0;
-	//Car_Speed = 50;
 	// 清空串口缓存
+	if (cb == nullptr)
+		return;
 	while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
 		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1);
+	delay(800);
 	OpenMVTrack_Disc_StartUp();
-	delay(500);
 	DCMotor.SpeedCtr(Car_Speed, Car_Speed);
-	Serial.println("Video Track Start.");
 	while (1)
 	{
-		delay(10);	//增加延迟防止卡死
 		if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
 		{
 			ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
-			if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02) && (Data_OpenMVBuf[2] == 0x91))
+			if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02))
+			{
+				if(Data_OpenMVBuf[2] == 0x91)
 				{
 					num++;
 					Serial.println(num);
@@ -132,6 +168,18 @@ void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed)
 					else  // 调整
 						AdjustCarPosition(Car_Speed);
 				}
+				else if ((Data_OpenMVBuf[2] == 0x80) && enable_white_detect)
+				{
+					DCMotor.Stop();
+					OpenMVTrack_Disc_CloseUp();
+					if (cb())
+					{
+						AccurateMotor.RunBackward(5);
+						AccurateMotor.DelayUntilCarStop();
+						OpenMVTrack_Disc_StartUp();
+					}
+				}
+			}
 		}
 	}
 }

OpenMVOpt.h

@@ -2,23 +2,45 @@
 #define __OPENMV_OPT__
 
 #include <ExtSRAMInterface.h>
+#include "AccurateMotor.h"
 #include "GlobalDatas.h"
-#include <DCMotor.h>
 #include <Command.h>
+#include <BEEP.h>
 
 namespace OpenMVOpt
 {
 	extern uint8_t Data_OpenMVBuf[8];
+	extern uint8_t ROI_Query_CMD[8];
+
+	//bool TrackCallback();
+	//返回false则停止循迹，否则继续循迹
+	typedef bool(*TrackCallback)();
+
+	struct ImageROIData
+	{
+		bool up;
+		bool down;
+		bool left;
+		bool right;
+		bool middle_up;
+		bool middle_down;
+		bool data_validate;
+	};
 
 	//车身姿态调整
 	void AdjustCarPosition(uint8_t Car_Speed);
 
+	//请求ROI数据
+	ImageROIData GetROIData();
+	//检查是否出现了大面积白色
+	bool ROIFoundWhite(const ImageROIData& data);
+
 	//启动视频循迹
 	void OpenMVTrack_Disc_StartUp();
 	//停止视频循迹
 	void OpenMVTrack_Disc_CloseUp();
 	//循迹（到下一路口）
-	void OpenMV_Track(uint8_t Car_Speed);
+	void OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_white_detect = true);
 
 	//启动二维码识别
 	void OpenMVQr_Disc_StartUp();