From 84d1c68b5d3de674395ee41ee3d18cc4bfa8a962 Mon Sep 17 00:00:00 2001 From: UnknownObject Date: Sun, 18 Jun 2023 19:21:17 +0800 Subject: [PATCH] commit all --- 2021_Arduino_Demo.ino | 2 +- 2021_Arduino_Demo.vcxproj | 6 +- 2021_Arduino_Demo.vcxproj.filters | 10 ++- AccurateMotor.cpp | 74 +++++++++++++----- AccurateMotor.h | 11 ++- AutoPathRunner.cpp | 64 +++++++++++++++- AutoPathRunner.h | 6 ++ GarageCommand.cpp | 6 ++ GarageCommand.h | 1 + Handler.cpp | 5 +- Handler.h | 1 + OpenMVOpt.cpp | 75 +++++++++---------- OpenMVOpt.h | 3 + VoiceIdentifyCommand.cpp | 29 +++++++ VoiceIdentifyCommand.h | 25 +++++++ ...RepotCommand.cpp => VoiceReportCommand.cpp | 2 +- VoiceRepotCommand.h => VoiceReportCommand.h | 4 +- ZigBeeOperator.cpp | 16 +++- ZigBeeOperator.h | 2 + __vm/Compile.vmps.xml | 2 +- __vm/Upload.vmps.xml | 2 +- 21 files changed, 267 insertions(+), 79 deletions(-) create mode 100644 VoiceIdentifyCommand.cpp create mode 100644 VoiceIdentifyCommand.h rename VoiceRepotCommand.cpp => VoiceReportCommand.cpp (98%) rename VoiceRepotCommand.h => VoiceReportCommand.h (96%) diff --git a/2021_Arduino_Demo.ino b/2021_Arduino_Demo.ino index 1b0c666..a17044f 100644 --- a/2021_Arduino_Demo.ino +++ b/2021_Arduino_Demo.ino @@ -3,9 +3,9 @@ #include "CommandEncoder.h" #include "CommandDecoder.h" #include "AccurateMotor.h" +#include "GlobalDatas.h" #include #include -#include "GlobalDatas.h" #include #include #include diff --git a/2021_Arduino_Demo.vcxproj b/2021_Arduino_Demo.vcxproj index d287c92..65effb8 100644 --- a/2021_Arduino_Demo.vcxproj +++ b/2021_Arduino_Demo.vcxproj @@ -123,7 +123,8 @@ - + + @@ -149,7 +150,8 @@ - + + diff --git a/2021_Arduino_Demo.vcxproj.filters b/2021_Arduino_Demo.vcxproj.filters index 33cce93..f056d49 100644 --- a/2021_Arduino_Demo.vcxproj.filters +++ b/2021_Arduino_Demo.vcxproj.filters @@ -57,7 +57,7 @@ Header Files - + Header Files @@ -90,6 +90,9 @@ Header Files + + Header Files + @@ -125,7 +128,7 @@ Source Files - + Source Files @@ -158,5 +161,8 @@ Source Files + + Source Files + \ No newline at end of file diff --git a/AccurateMotor.cpp b/AccurateMotor.cpp index 9546f75..d2bdbf2 100644 --- a/AccurateMotor.cpp +++ b/AccurateMotor.cpp @@ -1,18 +1,37 @@ #include "AccurateMotor.h" +void AccurateMotorClass::SetCarRunning() +{ + AccurateMotor.car_running = true; +} + +void AccurateMotorClass::SetCarNotRunning() +{ + AccurateMotor.car_running = false; +} + void AccurateMotorClass::Init() { car_running = false; } -void AccurateMotorClass::SetCarRunning() +bool AccurateMotorClass::IsCarRunning() { - AccurateMotor.car_running = true; + return car_running; } -void AccurateMotorClass::SetCarNotRunning() +void AccurateMotorClass::DelayUntilCarStop() { - AccurateMotor.car_running = false; + while (car_running) + delay(10); + delay(100); //缓冲，防止因惯性导致位置偏差 +} + +void AccurateMotorClass::ForceStop() +{ + DCMotor.Stop(); + SetCarNotRunning(); + MsTimer2::stop(); } void AccurateMotorClass::TurnLeft(uint8_t degree) @@ -32,13 +51,6 @@ void AccurateMotorClass::TurnLeft(uint8_t degree) MsTimer2::start(); } -void AccurateMotorClass::ForceStop() -{ - DCMotor.Stop(); - SetCarNotRunning(); - MsTimer2::stop(); -} - void AccurateMotorClass::TurnRight(uint8_t degree) { if (car_running) @@ -73,25 +85,47 @@ void AccurateMotorClass::RunForward(uint8_t distence) MsTimer2::start(); } -bool AccurateMotorClass::IsCarRunning() +void AccurateMotorClass::RunBackward(uint8_t distence) { - return car_running; + if (car_running) + return; + DCMotor.Back(car_speed); + SetCarRunning(); + MsTimer2::set(distence * run_multplyer, []() + { + { + DCMotor.Stop(); + SetCarNotRunning(); + MsTimer2::stop(); + } + }); + MsTimer2::start(); } -void AccurateMotorClass::DelayUntilCarStop() +void AccurateMotorClass::RunForwardSlow(uint8_t distence) { - while (car_running) - delay(10); - delay(100); //缓冲，防止因惯性导致位置偏差 + if (car_running) + return; + DCMotor.Go(car_slow_speed); + SetCarRunning(); + MsTimer2::set(distence * run_multplyer * (car_speed / car_slow_speed), []() + { + { + DCMotor.Stop(); + SetCarNotRunning(); + MsTimer2::stop(); + } + }); + MsTimer2::start(); } -void AccurateMotorClass::RunBackward(uint8_t distence) +void AccurateMotorClass::RunBackwardSlow(uint8_t distence) { if (car_running) return; - DCMotor.Back(car_speed); + DCMotor.Back(car_slow_speed); SetCarRunning(); - MsTimer2::set(distence * run_multplyer, []() + MsTimer2::set(distence * run_multplyer * (car_speed / car_slow_speed), []() { { DCMotor.Stop(); diff --git a/AccurateMotor.h b/AccurateMotor.h index 8508c1d..49cc526 100644 --- a/AccurateMotor.h +++ b/AccurateMotor.h @@ -19,10 +19,11 @@ class AccurateMotorClass { private: bool car_running; - const uint8_t car_speed = 50; - const uint8_t car_turn_speed = 100; - const uint8_t turn_multplyer = 7; - const uint8_t run_multplyer = 100; + const uint16_t car_speed = 50; + const uint16_t car_slow_speed = 10; + const uint16_t car_turn_speed = 100; + const uint16_t turn_multplyer = 7; + const uint16_t run_multplyer = 100; private: static void SetCarRunning(); @@ -39,6 +40,8 @@ public: void TurnRight(uint8_t degree); void RunForward(uint8_t distence); void RunBackward(uint8_t distence); + void RunForwardSlow(uint8_t distence); + void RunBackwardSlow(uint8_t distence); }; extern AccurateMotorClass AccurateMotor; diff --git a/AutoPathRunner.cpp b/AutoPathRunner.cpp index 62080d4..0c72ea4 100644 --- a/AutoPathRunner.cpp +++ b/AutoPathRunner.cpp @@ -216,13 +216,18 @@ AutoPathRunner::CarHeadPosition AutoPathRunner::Int2Position(int pos) bool AutoPathRunner::TrackCallback() { - AccurateMotor.RunBackward(6); + AccurateMotor.RunBackwardSlow(6); AccurateMotor.DelayUntilCarStop(); + OpenMVOpt::AdjustCarPoseInPlace(); if (IsSpecTerrainItem()) - AccurateMotor.RunForward(30); + { + OpenMVOpt::AdjustCarPoseInPlace(); + AccurateMotor.RunForwardSlow(30); + } else { - AccurateMotor.RunForward(12); + OpenMVOpt::AdjustCarPoseInPlace(); + AccurateMotor.RunForwardSlow(12); } AccurateMotor.DelayUntilCarStop(); return true; @@ -331,4 +336,57 @@ bool AutoPathRunner::IsSpecTerrainItem() AccurateMotor.DelayUntilCarStop(); delay(200); return (((dis - dis_left) >= 10) || ((dis - dis_right) >= 10)); +} + +void AutoPathRunner::ReversingIntoGarage(bool a_b_select, uint8_t target_floor) +{ + GarageCommand cmd; + uint8_t recv[8] = { 0 }; + AccurateMotor.RunForwardSlow(1); + AccurateMotor.DelayUntilCarStop(); + if (a_b_select) + cmd.UsingA(); + else + cmd.UsingB(); + ZigBeeOperator.SendCommand(cmd.CMD_PositionReset()); + do + { + delay(500); + ZigBeeOperator.SendCommand(cmd.CMD_QueryCurrentPos()); + if (ZigBeeOperator.ReciveCommand(recv, 8, 1000)) + { + if (cmd.IsPositionData(recv)) + { + if (cmd.CurrentPos(recv) == 0x01) + break; + } + } + } while (true); + AccurateMotor.TurnLeft(90); + AccurateMotor.DelayUntilCarStop(); + OpenMVOpt::OpenMV_TrackInPlace(); + do + { + delay(500); + ZigBeeOperator.SendCommand(cmd.CMD_QueryIRStatus()); + if (ZigBeeOperator.ReciveCommand(recv, 8, 1000)) + { + if (cmd.IsIRData(recv)) + { + if (!cmd.BackIRTiggered(recv)) + { + AccurateMotor.ForceStop(); + break; + } + else + { + AccurateMotor.RunBackwardSlow(1); + AccurateMotor.DelayUntilCarStop(); + } + } + } + } while (true); + AccurateMotor.RunForwardSlow(1); + AccurateMotor.DelayUntilCarStop(); + ZigBeeOperator.SendCommand(cmd.CMD_SetToFloor(target_floor)); } \ No newline at end of file diff --git a/AutoPathRunner.h b/AutoPathRunner.h index 75bc181..369b824 100644 --- a/AutoPathRunner.h +++ b/AutoPathRunner.h @@ -15,6 +15,9 @@ #include "OpenMVOpt.h" #include #include "AccurateMotor.h" +#include "GarageCommand.h" +#include "CommandDecoder.h" +#include "ZigBeeOperator.h" namespace AutoPathRunner { @@ -73,6 +76,9 @@ namespace AutoPathRunner void DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos); //区分RFID卡和特殊地形标志物 bool IsSpecTerrainItem(); + //倒车入库 + //a_b_select:车库标志物选择，true -> A, false -> B + void ReversingIntoGarage(bool a_b_select, uint8_t target_floor); }; #endif diff --git a/GarageCommand.cpp b/GarageCommand.cpp index 1173c9b..e7b4c0a 100644 --- a/GarageCommand.cpp +++ b/GarageCommand.cpp @@ -43,6 +43,12 @@ byte* GarageCommand::CMD_SetToFloor4() return GetCommandArray(); } +byte* GarageCommand::CMD_SetToFloor(uint8_t floor) +{ + SetCommand(0x01, floor); + return GetCommandArray(); +} + byte* GarageCommand::CMD_QueryCurrentPos() { SetCommand(0x02, 0x01); diff --git a/GarageCommand.h b/GarageCommand.h index 7a19313..ac87b0b 100644 --- a/GarageCommand.h +++ b/GarageCommand.h @@ -25,6 +25,7 @@ public: byte* CMD_SetToFloor2(); byte* CMD_SetToFloor3(); byte* CMD_SetToFloor4(); + byte* CMD_SetToFloor(uint8_t floor); byte* CMD_QueryCurrentPos(); byte* CMD_QueryIRStatus(); public: diff --git a/Handler.cpp b/Handler.cpp index a1a6f90..8be17f5 100644 --- a/Handler.cpp +++ b/Handler.cpp @@ -257,9 +257,8 @@ void Handler::Key_2() { OpenMVOpt::Servo_Control(-60); delay(500); - OpenMVOpt::OpenMV_Track(5, []() { return true; }, false); - delay(500); - OpenMVOpt::Servo_Control(0); + OpenMVOpt::OpenMV_Track(8, []() { return false; }, false); + AutoPathRunner::ReversingIntoGarage(false, 0x04); } void Handler::Key_3() diff --git a/Handler.h b/Handler.h index 439b39a..d1bad38 100644 --- a/Handler.h +++ b/Handler.h @@ -1,6 +1,7 @@ #ifndef __HANDLER__ #define __HANDLER__ +#include "VoiceIdentifyCommand.h" #include "AutoPathRunner.h" #include "CommandDecoder.h" #include "GlobalDatas.h" diff --git a/OpenMVOpt.cpp b/OpenMVOpt.cpp index 1f3d605..9f57dbf 100644 --- a/OpenMVOpt.cpp +++ b/OpenMVOpt.cpp @@ -1,7 +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 }; +uint8_t OpenMVOpt::ROI_Query_CMD[8] = { 0x55,0x02,0x91,0xAA,0x00,0x00,0x00,0xBB }; void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed) { @@ -56,51 +56,32 @@ void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed) AccurateMotor.ForceStop(); if (Data_OpenMVBuf[5] == 43) // + 向左调 { - AccurateMotor.TurnLeft(Data_OpenMVBuf[6] / 6); + AccurateMotor.TurnLeft(Data_OpenMVBuf[6] / 6.5); } else if (Data_OpenMVBuf[5] == 45) // - 向右调 { - AccurateMotor.TurnRight(Data_OpenMVBuf[6] / 6); + AccurateMotor.TurnRight(Data_OpenMVBuf[6] / 6.5); } AccurateMotor.DelayUntilCarStop(); DCMotor.SpeedCtr(Car_Speed, Car_Speed); } - //else if (Data_OpenMVBuf[6] <= 50) - //{ - - // if (Data_OpenMVBuf[5] == 43) // + 向左调 - // { - // DCMotor.SpeedCtr(0, 40); - // } - // else if (Data_OpenMVBuf[5] == 45) // - 向右调 - // { - // DCMotor.SpeedCtr(40, 0); - // } - //} - //else if (Data_OpenMVBuf[6] <= 70) - //{ - - // if (Data_OpenMVBuf[5] == 43) // + 向左调 - // { - // DCMotor.SpeedCtr(-20, 40); - // } - // else if (Data_OpenMVBuf[5] == 45) // - 向右调 - // { - // DCMotor.SpeedCtr(40, -20); - // } - //} - //else if (Data_OpenMVBuf[6] > 70) - //{ +} - // if (Data_OpenMVBuf[5] == 43) // + 向左调 - // { - // DCMotor.SpeedCtr(-30, 30); - // } - // else if (Data_OpenMVBuf[5] == 45) // - 向右调 - // { - // DCMotor.SpeedCtr(30, -30); - // } - //} +bool OpenMVOpt::AdjustCarPoseInPlace() +{ + if ((Data_OpenMVBuf[3] != 0x02) || (Data_OpenMVBuf[4] <= 10)) + return false; + AccurateMotor.ForceStop(); + if (Data_OpenMVBuf[5] == 43) // + 向左调 + { + AccurateMotor.TurnLeft(Data_OpenMVBuf[4] / 6.5); + } + else if (Data_OpenMVBuf[5] == 45) // - 向右调 + { + AccurateMotor.TurnRight(Data_OpenMVBuf[4] / 6.5); + } + AccurateMotor.DelayUntilCarStop(); + return true; } OpenMVOpt::ImageROIData OpenMVOpt::GetROIData() @@ -198,6 +179,24 @@ void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_wh } } +void OpenMVOpt::OpenMV_TrackInPlace() +{ + uint32_t num = 0; + // 清空串口缓存 + while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00) + ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1); + do + { + Command.Judgment(ROI_Query_CMD); + ExtSRAMInterface.ExMem_Write_Bytes(0x6008, ROI_Query_CMD, 8); + 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) || (Data_OpenMVBuf[2] != 0x80)) + continue; + } while (AdjustCarPoseInPlace()); +} + void OpenMVOpt::OpenMVQr_Disc_StartUp() { qrdi_buf[3] = 0x01; //开始识别 diff --git a/OpenMVOpt.h b/OpenMVOpt.h index fffd874..54ea1b0 100644 --- a/OpenMVOpt.h +++ b/OpenMVOpt.h @@ -29,6 +29,7 @@ namespace OpenMVOpt //车身姿态调整 void AdjustCarPosition(uint8_t Car_Speed); + bool AdjustCarPoseInPlace(); //请求ROI数据 ImageROIData GetROIData(); @@ -41,6 +42,8 @@ namespace OpenMVOpt void OpenMVTrack_Disc_CloseUp(); //循迹（到下一路口） void OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_white_detect = true); + //原地循迹（仅矫正姿态，不行进） + void OpenMV_TrackInPlace(); //启动二维码识别 void OpenMVQr_Disc_StartUp(); diff --git a/VoiceIdentifyCommand.cpp b/VoiceIdentifyCommand.cpp new file mode 100644 index 0000000..9356508 --- /dev/null +++ b/VoiceIdentifyCommand.cpp @@ -0,0 +1,29 @@ +// +// 语音识别模块 - “小创” +// + +#include "VoiceIdentifyCommand.h" + +uint8_t VoiceIdentifyCommand::RandomDisc() +{ + randomSeed(micros()); + return random(0x00, 0x07); +} + +bool VoiceIdentifyCommand::CMD_ReportAndIdentify() +{ + uint8_t source = RandomDisc(); + uint8_t result = BKRC_Voice.BKRC_Voice_Extern(source); + return (source == (result - 0x01)); +} + +void VoiceIdentifyCommand::CMD_ReportAnyText(String text) +{ + TextEncoder encoder(text); + while (encoder.HasNextChar()) + { + byte data[2] = { 0 }; + Serial2.write(data, 2); + delay(500); + } +} diff --git a/VoiceIdentifyCommand.h b/VoiceIdentifyCommand.h new file mode 100644 index 0000000..fa737db --- /dev/null +++ b/VoiceIdentifyCommand.h @@ -0,0 +1,25 @@ +// VoiceReportCommand.h + +#ifndef _VOICEIDENTIFYCOMMAND_h +#define _VOICEIDENTIFYCOMMAND_h + +#if defined(ARDUINO) && ARDUINO >= 100 + #include "arduino.h" +#else + #include "WProgram.h" +#endif + +#include +#include "TextEncoder.h" + +class VoiceIdentifyCommand +{ +private: + uint8_t RandomDisc(); +public: + bool CMD_ReportAndIdentify(); + void CMD_ReportAnyText(String text); +}; + +#endif + diff --git a/VoiceRepotCommand.cpp b/VoiceReportCommand.cpp similarity index 98% rename from VoiceRepotCommand.cpp rename to VoiceReportCommand.cpp index 3ae807f..b766176 100644 --- a/VoiceRepotCommand.cpp +++ b/VoiceReportCommand.cpp @@ -2,7 +2,7 @@ // 标志物通信 - 语音播报 - ZigBee // -#include "VoiceRepotCommand.h" +#include "VoiceReportCommand.h" VoiceReportCommand::VoiceReportCommand() { diff --git a/VoiceRepotCommand.h b/VoiceReportCommand.h similarity index 96% rename from VoiceRepotCommand.h rename to VoiceReportCommand.h index 3c3b0f2..9b14a48 100644 --- a/VoiceRepotCommand.h +++ b/VoiceReportCommand.h @@ -1,7 +1,7 @@ // VoiceRepotCommand.h -#ifndef _VOICEREPOTCOMMAND_h -#define _VOICEREPOTCOMMAND_h +#ifndef _VOICEREPORTCOMMAND_h +#define _VOICEREPORTCOMMAND_h #if defined(ARDUINO) && ARDUINO >= 100 #include "arduino.h" diff --git a/ZigBeeOperator.cpp b/ZigBeeOperator.cpp index d1f4e88..48bb711 100644 --- a/ZigBeeOperator.cpp +++ b/ZigBeeOperator.cpp @@ -1,5 +1,5 @@ // -// +// ZigBee通信 // #include "ZigBeeOperator.h" @@ -14,6 +14,20 @@ void ZigBeeOperatorClass::SendCommand(uint8_t* cmd, uint16_t siz) ExtSRAMInterface.ExMem_Write_Bytes(zigbee_address, cmd, siz); } +bool ZigBeeOperatorClass::ReciveCommand(uint8_t recv[], uint16_t siz, uint16_t timeout) +{ + uint16_t passed_time = 0; + while (ExtSRAMInterface.ExMem_Read(zigbee_read_address) == 0x00) + { + delay(1); + passed_time++; + if ((timeout != 0) && (passed_time >= timeout)) + return false; + } + ExtSRAMInterface.ExMem_Read_Bytes(zigbee_read_address, recv, siz); + return true; +} + ZigBeeOperatorClass ZigBeeOperator; diff --git a/ZigBeeOperator.h b/ZigBeeOperator.h index 29579cc..5486353 100644 --- a/ZigBeeOperator.h +++ b/ZigBeeOperator.h @@ -15,10 +15,12 @@ class ZigBeeOperatorClass { private: const uint16_t zigbee_address = 0x6008; + const uint16_t zigbee_read_address = 0x6100; public: void Initialization(); public: void SendCommand(uint8_t* cmd, uint16_t siz = 8); + bool ReciveCommand(uint8_t recv[], uint16_t siz = 8, uint16_t timeout = 0); }; extern ZigBeeOperatorClass ZigBeeOperator; diff --git a/__vm/Compile.vmps.xml b/__vm/Compile.vmps.xml index 5c28bc0..9d09372 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 5c28bc0..9d09372 100644 --- a/__vm/Upload.vmps.xml +++ b/__vm/Upload.vmps.xml @@ -2,7 +2,7 @@ - +