diff --git a/2021_Arduino_Demo.ino b/2021_Arduino_Demo.ino
index 442386e..1b0c666 100644
--- a/2021_Arduino_Demo.ino
+++ b/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数据
@@ -61,32 +59,37 @@ void loop()
if ((Data_OTABuf[0] == 0x55) && (Data_OTABuf[1] == 0x02))
{
ExtSRAMInterface.ExMem_Write_Bytes(0x6180, Data_OTABuf, Data_Length); //使用自定义数据区上传OpenMV识别结果
- Handler::OpenMVRx_Handler(Data_OTABuf); //接收OpenMV，数据处理函数
+ Handler::OpenMVRx_Handler(Data_OTABuf); //接收OpenMV，数据处理函数
}
}
- 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();
-}
\ No newline at end of file
+}
+
+//获取和上传任务板数据：已禁用。
+//{
+// 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);
+// }
+//}
\ No newline at end of file
diff --git a/2021_Arduino_Demo.vcxproj b/2021_Arduino_Demo.vcxproj
index 257bfe2..d287c92 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\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)
+ $(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)
true
@@ -107,6 +107,7 @@
+
@@ -132,6 +133,7 @@
+
diff --git a/2021_Arduino_Demo.vcxproj.filters b/2021_Arduino_Demo.vcxproj.filters
index cd75dc1..33cce93 100644
--- a/2021_Arduino_Demo.vcxproj.filters
+++ b/2021_Arduino_Demo.vcxproj.filters
@@ -87,6 +87,9 @@
Header Files
+
+ Header Files
+
@@ -152,5 +155,8 @@
Source Files
+
+ Source Files
+
\ No newline at end of file
diff --git a/AccurateMotor.cpp b/AccurateMotor.cpp
index 1620502..9546f75 100644
--- a/AccurateMotor.cpp
+++ b/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)
diff --git a/AccurateMotor.h b/AccurateMotor.h
index 4d54fa7..d92f2fc 100644
--- a/AccurateMotor.h
+++ b/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:
diff --git a/AutoPathRunner.cpp b/AutoPathRunner.cpp
new file mode 100644
index 0000000..62080d4
--- /dev/null
+++ b/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));
+}
\ No newline at end of file
diff --git a/AutoPathRunner.h b/AutoPathRunner.h
new file mode 100644
index 0000000..75bc181
--- /dev/null
+++ b/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
+#include
+#include
+#include "OpenMVOpt.h"
+#include
+#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
+
diff --git a/DebugOpt.cpp b/DebugOpt.cpp
index e43508e..90575c6 100644
--- a/DebugOpt.cpp
+++ b/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()
diff --git a/Handler.cpp b/Handler.cpp
index b7051ae..a1a6f90 100644
--- a/Handler.cpp
+++ b/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)
diff --git a/Handler.h b/Handler.h
index 7c841ae..439b39a 100644
--- a/Handler.h
+++ b/Handler.h
@@ -1,8 +1,10 @@
#ifndef __HANDLER__
#define __HANDLER__
+#include "AutoPathRunner.h"
#include "CommandDecoder.h"
#include "GlobalDatas.h"
+#include
#include "DebugOpt.h"
namespace Handler
diff --git a/OpenMVOpt.cpp b/OpenMVOpt.cpp
index 1c7ddf8..08baa2f 100644
--- a/OpenMVOpt.cpp
+++ b/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,35 +137,48 @@ 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))
{
- num++;
- Serial.println(num);
- if (Data_OpenMVBuf[4] == 1) // ·
+ if(Data_OpenMVBuf[2] == 0x91)
+ {
+ num++;
+ Serial.println(num);
+ if (Data_OpenMVBuf[4] == 1) // ·
+ {
+ DCMotor.Stop();
+ OpenMVTrack_Disc_CloseUp();
+ break;
+ }
+ else //
+ AdjustCarPosition(Car_Speed);
+ }
+ else if ((Data_OpenMVBuf[2] == 0x80) && enable_white_detect)
{
DCMotor.Stop();
OpenMVTrack_Disc_CloseUp();
- break;
+ if (cb())
+ {
+ AccurateMotor.RunBackward(5);
+ AccurateMotor.DelayUntilCarStop();
+ OpenMVTrack_Disc_StartUp();
+ }
}
- else //
- AdjustCarPosition(Car_Speed);
}
}
}
diff --git a/OpenMVOpt.h b/OpenMVOpt.h
index 046b701..fffd874 100644
--- a/OpenMVOpt.h
+++ b/OpenMVOpt.h
@@ -2,23 +2,45 @@
#define __OPENMV_OPT__
#include
+#include "AccurateMotor.h"
#include "GlobalDatas.h"
-#include
#include
+#include
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();
diff --git a/__vm/Compile.vmps.xml b/__vm/Compile.vmps.xml
index 6676c93..e6ef9f4 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 6676c93..e6ef9f4 100644
--- a/__vm/Upload.vmps.xml
+++ b/__vm/Upload.vmps.xml
@@ -2,7 +2,7 @@
-
+