UnknownObject
/
RaceSubCar
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

commit after race

Browse Source
master
UnknownObject 2 years ago
parent af59a049ac
commit ac062e5932
16 changed files with 260 additions and 77 deletions
Show Stats Download Patch File Download Diff File

2
2021_Arduino_Demo.ino
Unescape Escape View File

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

2
2021_Arduino_Demo.vcxproj
Unescape Escape View File

@ -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> <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>stdcpp17</LanguageStandard>
<LanguageStandard_C>stdc11</LanguageStandard_C> <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> </ClCompile>
<Link> <Link>
<GenerateDebugInformation>true</GenerateDebugInformation> <GenerateDebugInformation>true</GenerateDebugInformation>

3
AccurateMotor.h
Unescape Escape View File

@ -21,7 +21,8 @@ private:
bool car_running; bool car_running;
const uint16_t car_speed = 50; const uint16_t car_speed = 50;
const uint16_t car_slow_speed = 10; 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 turn_multplyer = 7;
const uint16_t run_multplyer = 100; const uint16_t run_multplyer = 100;

46
AutoPathRunner.cpp
Unescape Escape View File

@ -16,12 +16,12 @@ AutoPathRunner::MapPoint::MapPoint(int x, int y)
this->y = y; this->y = y;
} }
int AutoPathRunner::MapPoint::GetX() int AutoPathRunner::MapPoint::GetX() const
{ {
return x; return x;
} }
int AutoPathRunner::MapPoint::GetY() int AutoPathRunner::MapPoint::GetY() const
{ {
return y; return y;
} }
@ -160,11 +160,24 @@ AutoPathRunner::MapPoint AutoPathRunner::Mapping(const char* name)
return MapPoint(); 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) for (auto& [key, value] : std_points)
{
/*//Serial.print(String("Current Value: (") + value.GetX());
//Serial.print(String(", ") + value.GetY());
//Serial.println(")");*/
if (value == point) if (value == point)
return key; {
////Serial.println("----------End Reverse Map----------");
return String(key);
}
}
////Serial.println("----------End(Error) Reverse Map----------");
return ""; return "";
} }
@ -258,7 +271,7 @@ AutoPathRunner::MapPoint* AutoPathRunner::DecodeRoute(const char* route, int& po
return list; 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舵机至循迹角度 //调整K210舵机至循迹角度
OpenMVOpt::Servo_Control(-60); OpenMVOpt::Servo_Control(-60);
@ -268,12 +281,18 @@ void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos)
if (i != 1) if (i != 1)
{ {
//向前一定距离 //向前一定距离
//Serial.println("RunForward(3)");
AccurateMotor.RunForward(3); AccurateMotor.RunForward(3);
AccurateMotor.DelayUntilCarStop(); 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)) if (last_point.NeedTurn(points[i], pos))
{ {
//TurnFlag = true; //Serial.print("Turn ");
switch (last_point.CalcTurnDirection(points[i], pos)) switch (last_point.CalcTurnDirection(points[i], pos))
{ {
case TurnDirection::Left: case TurnDirection::Left:
@ -281,23 +300,32 @@ void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos)
AccurateMotor.TurnLeft(90); AccurateMotor.TurnLeft(90);
AccurateMotor.DelayUntilCarStop(); AccurateMotor.DelayUntilCarStop();
ChangePositon(pos, TurnDirection::Left); ChangePositon(pos, TurnDirection::Left);
//Serial.println("Left");
break; break;
case TurnDirection::Right: case TurnDirection::Right:
//右转 //右转
AccurateMotor.TurnRight(90); AccurateMotor.TurnRight(90);
AccurateMotor.DelayUntilCarStop(); AccurateMotor.DelayUntilCarStop();
ChangePositon(pos, TurnDirection::Right); ChangePositon(pos, TurnDirection::Right);
//Serial.println("Right");
break; break;
default: default:
break; 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 else
{ {
//向前循迹 //向前循迹
OpenMVOpt::OpenMV_Track(8, TrackCallback); OpenMVOpt::OpenMV_Track(8, TrackCallback, false);
////Serial.println("OpenMVOpt::OpenMV_Track(8, TrackCallback, false);");
} }
last_point = points[i]; last_point = points[i];
} }
@ -362,7 +390,7 @@ void AutoPathRunner::ReversingIntoGarage(bool a_b_select, uint8_t target_floor)
} }
} }
} while (true); } while (true);
AccurateMotor.TurnLeft(90); AccurateMotor.TurnLeft(180);
AccurateMotor.DelayUntilCarStop(); AccurateMotor.DelayUntilCarStop();
OpenMVOpt::OpenMV_TrackInPlace(); OpenMVOpt::OpenMV_TrackInPlace();
do do

10
AutoPathRunner.h
Unescape Escape View File

@ -44,8 +44,8 @@ namespace AutoPathRunner
MapPoint(); MapPoint();
MapPoint(int x, int y); MapPoint(int x, int y);
public: public:
int GetX(); int GetX() const;
int GetY(); int GetY() const;
void SetX(int x); void SetX(int x);
void SetY(int y); void SetY(int y);
bool NeedTurn(MapPoint next, CarHeadPosition pos); bool NeedTurn(MapPoint next, CarHeadPosition pos);
@ -64,8 +64,10 @@ namespace AutoPathRunner
PointMapper(const char* n, const MapPoint& p); PointMapper(const char* n, const MapPoint& p);
}; };
typedef void(*MCRCallback)(String point, CarHeadPosition pos);
MapPoint Mapping(const char* name); MapPoint Mapping(const char* name);
const char* ReverseMapping(MapPoint point); String ReverseMapping(const MapPoint& point);
void ChangePositon(CarHeadPosition& pos, TurnDirection turn); void ChangePositon(CarHeadPosition& pos, TurnDirection turn);
CarHeadPosition Int2Position(int pos); CarHeadPosition Int2Position(int pos);
bool TrackCallback(); bool TrackCallback();
@ -73,7 +75,7 @@ namespace AutoPathRunner
//解析路线 //解析路线
MapPoint* DecodeRoute(const char* route, int& point_cnt); 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卡和特殊地形标志物 //区分RFID卡和特殊地形标志物
bool IsSpecTerrainItem(); bool IsSpecTerrainItem();
//倒车入库 //倒车入库

4
DebugOpt.cpp
Unescape Escape View File

@ -112,8 +112,8 @@ void DebugOpt::Speech_Disc()
2-6为指定词条识别，具体信息可查看通信协议*/ 2-6为指定词条识别，具体信息可查看通信协议*/
sph_id = BKRC_Voice.BKRC_Voice_Extern(0); sph_id = BKRC_Voice.BKRC_Voice_Extern(0);
delay(200); delay(200);
Serial.print("识别到的词条ID为："); //Serial.print("识别到的词条ID为：");
Serial.println(sph_id, HEX); //Serial.println(sph_id, HEX);
//voice_trm_buf[2] = sph_id; //voice_trm_buf[2] = sph_id;
//ExtSRAMInterface.ExMem_Write_Bytes(0x6008, voice_trm_buf, 8);//上传识别结果至评分终端 //ExtSRAMInterface.ExMem_Write_Bytes(0x6008, voice_trm_buf, 8);//上传识别结果至评分终端
} }

12
GlobalDatas.h
Unescape Escape View File

@ -51,15 +51,15 @@ namespace DataTool
{ {
template<typename T = uint8_t> void PrintDataArray(T* arr, uint8_t len) 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++) 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------");
} }
}; };

173
Handler.cpp
Unescape Escape View File

@ -1,5 +1,67 @@
#include "Handler.h" #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) void Handler::ZigBeeRx_Handler(uint8_t* mar)
{ {
CommandDecoder decoder(mar); CommandDecoder decoder(mar);
@ -18,15 +80,15 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar)
GarageCommand cmd; GarageCommand cmd;
if (cmd.IsPositionData(mar)) 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)) 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; break;
} }
@ -37,24 +99,24 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar)
byte m, d, h, mm, s; byte m, d, h, mm, s;
if (cmd.ReadRTCDate(mar, y, m, d)) 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 else
Serial.println("Invalidate Date"); //Serial.println("Invalidate Date");
if (cmd.ReadRTCTime(mar, h, mm, s)) 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 else
Serial.println("Invalidate Time"); //Serial.println("Invalidate Time");
break; break;
} }
case CommandData::Devices::AlarmTower: //ºìÍⱨ¾¯±êÖ¾Îï case CommandData::Devices::AlarmTower: //ºìÍⱨ¾¯±êÖ¾Îï
@ -62,8 +124,8 @@ void Handler::ZigBeeRx_Handler(uint8_t* mar)
AlarmTowerCommand cmd; AlarmTowerCommand cmd;
if (cmd.IsAlarmTowerCommand(mar)) 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; break;
} }
@ -138,38 +200,45 @@ void Handler::MainCarRx_Handler(uint8_t* com)
switch(decoder.GetMainCommand()) switch(decoder.GetMainCommand())
{ {
case CommandData::MainCarCommand::StartRun: case CommandData::MainCarCommand::StartRun:
if (decoder.GetDataBit(1) == CommandData::MainCarCommand::StartRunSub)
{
int point_cnt = 0; int point_cnt = 0;
AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute("B7-B6-D6-F6-F4-D4-B4-B2-D2-D1", point_cnt); CommandEncoder encoder;
AutoPathRunner::DoRun(points, point_cnt, AutoPathRunner::CarHeadPosition::X_Positive); 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); delay(1500);
} AutoPathRunner::ReversingIntoGarage(true, 3);
break; break;
case CommandData::MainCarCommand::RunningRoute: case CommandData::MainCarCommand::RunningRoute:
dynamic_route_recv = ""; dynamic_route_recv = "";
dynamic_route_recv.concat((char)decoder.GetDataBit(1)); dynamic_route_recv.concat((char)decoder.GetDataBit(1));
dynamic_route_recv.concat((char)decoder.GetDataBit(2)); dynamic_route_recv.concat((char)decoder.GetDataBit(2));
dynamic_route_recv.concat('-'); dynamic_route_recv.concat('-');
Serial.print("Route Command: "); //Serial.print("Route Command: ");
Serial.println(dynamic_route_recv); //Serial.println(dynamic_route_recv);
break; break;
case CommandData::MainCarCommand::RunningRouteData: case CommandData::MainCarCommand::RunningRouteData:
dynamic_route_recv.concat((char)decoder.GetDataBit(1)); dynamic_route_recv.concat((char)decoder.GetDataBit(1));
dynamic_route_recv.concat((char)decoder.GetDataBit(2)); dynamic_route_recv.concat((char)decoder.GetDataBit(2));
if (decoder.GetDataBit(3) == CommandData::MainCarCommand::RouteDataStop) 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; int point_cnt = 0;
AutoPathRunner::MapPoint* points = AutoPathRunner::DecodeRoute(dynamic_route_recv.c_str(), point_cnt); 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); delay(1500);
AutoPathRunner::ReversingIntoGarage(true, 3);
} }
else else
{ {
dynamic_route_recv.concat('-'); dynamic_route_recv.concat('-');
Serial.println(dynamic_route_recv); //Serial.println(dynamic_route_recv);
} }
break; break;
} }
@ -278,20 +347,22 @@ void Handler::MainCarRx_Handler(uint8_t* com)
void Handler::Key_0() void Handler::Key_0()
{ {
int point_cnt = 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); delay(1500);
AccurateMotor.TurnLeft(90); AutoPathRunner::ReversingIntoGarage(true, 3);
AccurateMotor.DelayUntilCarStop();
AccurateMotor.RunForward(20);
AccurateMotor.DelayUntilCarStop();
} }
void Handler::Key_1() void Handler::Key_1()
{ {
OpenMVOpt::OpenMVTrack_Disc_CloseUp(); /*OpenMVOpt::OpenMVTrack_Disc_CloseUp();
delay(500); 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() void Handler::Key_2()
@ -299,19 +370,29 @@ void Handler::Key_2()
OpenMVOpt::Servo_Control(-60); OpenMVOpt::Servo_Control(-60);
delay(500); delay(500);
OpenMVOpt::OpenMV_Track(8, []() { return false; }, false); OpenMVOpt::OpenMV_Track(8, []() { return false; }, false);
AutoPathRunner::ReversingIntoGarage(false, 0x04); AutoPathRunner::ReversingIntoGarage(true, 0x03);
} }
void Handler::Key_3() 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); delay(200);
cmd.CMD_IR_CarLicenseMode_5to6_XY(str[4], str[5], 'B', '6');
} }
} }

3
Handler.h
Unescape Escape View File

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

51
OpenMVOpt.cpp
Unescape Escape View File

@ -153,7 +153,7 @@ void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_wh
if(Data_OpenMVBuf[2] == 0x91) if(Data_OpenMVBuf[2] == 0x91)
{ {
num++; num++;
Serial.println(num); //Serial.println(num);
if (Data_OpenMVBuf[4] == 1) // ·¿Ú if (Data_OpenMVBuf[4] == 1) // ·¿Ú
{ {
DCMotor.Stop(); DCMotor.Stop();
@ -226,3 +226,52 @@ void OpenMVOpt::Servo_Control(int8_t angle)
ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8); ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8);
delay(1000); 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 "";
}

4
OpenMVOpt.h
Unescape Escape View File

@ -49,6 +49,10 @@ namespace OpenMVOpt
void OpenMVQr_Disc_StartUp(); void OpenMVQr_Disc_StartUp();
//停止二维码识别 //停止二维码识别
void OpenMVQr_Disc_CloseUp(); 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); void Servo_Control(int8_t angle);

4
RodeLightCommand.cpp
Unescape Escape View File

@ -75,8 +75,8 @@ byte RodeLightCommand::GetCurrentLevel()
if (light_arr[i] < light_arr[j]) if (light_arr[i] < light_arr[j])
Swap(light_arr[i], light_arr[j]); Swap(light_arr[i], light_arr[j]);
/*DataTool::PrintDataArray(light_arr, 4); /*DataTool::PrintDataArray(light_arr, 4);
Serial.print("Current: "); //Serial.print("Current: ");
Serial.println(current, HEX);*/ //Serial.println(current, HEX);*/
return CalcLevel(light_arr, current); return CalcLevel(light_arr, current);
} }

12
TrafficLightCommand.cpp
Unescape Escape View File

@ -4,6 +4,12 @@
#include "TrafficLightCommand.h" #include "TrafficLightCommand.h"
uint8_t TrafficLightCommand::RandomDisc()
{
randomSeed(micros());
return random(0x01, 0x04);
}
TrafficLightCommand::TrafficLightCommand() TrafficLightCommand::TrafficLightCommand()
{ {
SetDevice(CommandData::Devices::TrafficLight_A); SetDevice(CommandData::Devices::TrafficLight_A);
@ -31,6 +37,12 @@ byte* TrafficLightCommand::CMD_CheckResult(Color color)
return GetCommandArray(); return GetCommandArray();
} }
byte* TrafficLightCommand::CMD_CheckRandomResult()
{
SetCommand(0x02, RandomDisc());
return GetCommandArray();
}
bool TrafficLightCommand::IsTrafficLightCommand(byte* cmd) bool TrafficLightCommand::IsTrafficLightCommand(byte* cmd)
{ {
if ((cmd[1] == CommandData::Devices::TrafficLight_A) || (cmd[1] == CommandData::Devices::TrafficLight_B)) if ((cmd[1] == CommandData::Devices::TrafficLight_A) || (cmd[1] == CommandData::Devices::TrafficLight_B))

3
TrafficLightCommand.h
Unescape Escape View File

@ -20,6 +20,8 @@ public:
Green = 0x02, Green = 0x02,
Yellow = 0x03 Yellow = 0x03
}; };
private:
uint8_t RandomDisc();
public: public:
TrafficLightCommand(); TrafficLightCommand();
public: public:
@ -28,6 +30,7 @@ public:
public: public:
byte* CMD_EnterIdentifyMode(); byte* CMD_EnterIdentifyMode();
byte* CMD_CheckResult(Color color); byte* CMD_CheckResult(Color color);
byte* CMD_CheckRandomResult();
public: public:
bool IsTrafficLightCommand(byte* cmd); bool IsTrafficLightCommand(byte* cmd);
bool ModeChangeSuccess(byte* cmd); bool ModeChangeSuccess(byte* cmd);

2
__vm/Compile.vmps.xml
View File

File diff suppressed because one or more lines are too long

2
__vm/Upload.vmps.xml
View File

File diff suppressed because one or more lines are too long
Write Preview
Loading…
Cancel
Save