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 @@
-
+