commit all

2021_Arduino_Demo.ino

@@ -3,9 +3,9 @@
 #include "CommandEncoder.h"
 #include "CommandDecoder.h"
 #include "AccurateMotor.h"
+#include "GlobalDatas.h"
 #include <Ultrasonic.h>
 #include <BKRC_Voice.h>
-#include "GlobalDatas.h"
 #include <CoreBeep.h>
 #include <DCMotor.h>
 #include <CoreKEY.h>

2021_Arduino_Demo.vcxproj

@@ -123,7 +123,8 @@
     <ClInclude Include="TextEncoder.h" />
     <ClInclude Include="TFTCommand.h" />
     <ClInclude Include="TrafficLightCommand.h" />
-    <ClInclude Include="VoiceRepotCommand.h" />
+    <ClInclude Include="VoiceIdentifyCommand.h" />
+    <ClInclude Include="VoiceReportCommand.h" />
     <ClInclude Include="WirelessChargerCommand.h" />
     <ClInclude Include="ZigBeeOperator.h" />
     <ClInclude Include="3DDisplayCommand.h" />
@@ -149,7 +150,8 @@
     <ClCompile Include="TextEncoder.cpp" />
     <ClCompile Include="TFTCommand.cpp" />
     <ClCompile Include="TrafficLightCommand.cpp" />
-    <ClCompile Include="VoiceRepotCommand.cpp" />
+    <ClCompile Include="VoiceIdentifyCommand.cpp" />
+    <ClCompile Include="VoiceReportCommand.cpp" />
     <ClCompile Include="WirelessChargerCommand.cpp" />
     <ClCompile Include="ZigBeeOperator.cpp" />
     <ClCompile Include="3DDisplayCommand.cpp" />

2021_Arduino_Demo.vcxproj.filters

@@ -57,7 +57,7 @@
     <ClInclude Include="GarageCommand.h">
       <Filter>Header Files</Filter>
     </ClInclude>
-    <ClInclude Include="VoiceRepotCommand.h">
+    <ClInclude Include="VoiceReportCommand.h">
       <Filter>Header Files</Filter>
     </ClInclude>
     <ClInclude Include="AlarmTowerCommand.h">
@@ -90,6 +90,9 @@
     <ClInclude Include="AutoPathRunner.h">
       <Filter>Header Files</Filter>
     </ClInclude>
+    <ClInclude Include="VoiceIdentifyCommand.h">
+      <Filter>Header Files</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <ClCompile Include="OpenMVOpt.cpp">
@@ -125,7 +128,7 @@
     <ClCompile Include="GarageCommand.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
-    <ClCompile Include="VoiceRepotCommand.cpp">
+    <ClCompile Include="VoiceReportCommand.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
     <ClCompile Include="AlarmTowerCommand.cpp">
@@ -158,5 +161,8 @@
     <ClCompile Include="AutoPathRunner.cpp">
       <Filter>Source Files</Filter>
     </ClCompile>
+    <ClCompile Include="VoiceIdentifyCommand.cpp">
+      <Filter>Source Files</Filter>
+    </ClCompile>
   </ItemGroup>
 </Project>

AccurateMotor.cpp

@@ -1,10 +1,5 @@
 #include "AccurateMotor.h"
 
-void AccurateMotorClass::Init()
-{
-	car_running = false;
-}
-
 void AccurateMotorClass::SetCarRunning()
 {
 	AccurateMotor.car_running = true;
@@ -15,6 +10,30 @@ void AccurateMotorClass::SetCarNotRunning()
 	AccurateMotor.car_running = false;
 }
 
+void AccurateMotorClass::Init()
+{
+	car_running = false;
+}
+
+bool AccurateMotorClass::IsCarRunning()
+{
+	return car_running;
+}
+
+void AccurateMotorClass::DelayUntilCarStop()
+{
+	while (car_running)
+		delay(10);
+	delay(100);	//缓冲，防止因惯性导致位置偏差
+}
+
+void AccurateMotorClass::ForceStop()
+{
+	DCMotor.Stop();
+	SetCarNotRunning();
+	MsTimer2::stop();
+}
+
 void AccurateMotorClass::TurnLeft(uint8_t degree)
 {
 	if (car_running)
@@ -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,18 +85,6 @@ void AccurateMotorClass::RunForward(uint8_t distence)
 	MsTimer2::start();
 }
 
-bool AccurateMotorClass::IsCarRunning()
-{
-	return car_running;
-}
-
-void AccurateMotorClass::DelayUntilCarStop()
-{
-	while (car_running)
-		delay(10);
-	delay(100);	//缓冲，防止因惯性导致位置偏差
-}
-
 void AccurateMotorClass::RunBackward(uint8_t distence)
 {
 	if (car_running)
@@ -102,4 +102,38 @@ void AccurateMotorClass::RunBackward(uint8_t distence)
 	MsTimer2::start();
 }
 
+void AccurateMotorClass::RunForwardSlow(uint8_t distence)
+{
+	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::RunBackwardSlow(uint8_t distence)
+{
+	if (car_running)
+		return;
+	DCMotor.Back(car_slow_speed);
+	SetCarRunning();
+	MsTimer2::set(distence * run_multplyer * (car_speed / car_slow_speed), []()
+		{
+			{
+				DCMotor.Stop();
+				SetCarNotRunning();
+				MsTimer2::stop();
+			}
+		});
+	MsTimer2::start();
+}
+
 AccurateMotorClass AccurateMotor;

AccurateMotor.h

@@ -19,10 +19,11 @@ class AccurateMotorClass
 {
 private:
 	bool car_running;
-	const uint8_t car_speed = 50;
+	const uint16_t car_speed = 50;
-	const uint8_t car_turn_speed = 100;
+	const uint16_t car_slow_speed = 10;
-	const uint8_t turn_multplyer = 7;
+	const uint16_t car_turn_speed = 100;
-	const uint8_t run_multplyer = 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;

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));
 }

AutoPathRunner.h

@@ -15,6 +15,9 @@
 #include "OpenMVOpt.h"
 #include <Ultrasonic.h>
 #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

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);

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:

Handler.cpp

@@ -257,9 +257,8 @@ void Handler::Key_2()
 {
 	OpenMVOpt::Servo_Control(-60);
 	delay(500);
-	OpenMVOpt::OpenMV_Track(5, []() { return true; }, false);
+	OpenMVOpt::OpenMV_Track(8, []() { return false; }, false);
-	delay(500);
+	AutoPathRunner::ReversingIntoGarage(false, 0x04);
-	OpenMVOpt::Servo_Control(0);
 }
 
 void Handler::Key_3()

Handler.h

@@ -1,6 +1,7 @@
 #ifndef __HANDLER__
 #define __HANDLER__
 
+#include "VoiceIdentifyCommand.h"
 #include "AutoPathRunner.h"
 #include "CommandDecoder.h"
 #include "GlobalDatas.h"

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)     // +  向左调
+bool OpenMVOpt::AdjustCarPoseInPlace()
-	//	{
+{
-	//		DCMotor.SpeedCtr(0, 40);
+	if ((Data_OpenMVBuf[3] != 0x02) || (Data_OpenMVBuf[4] <= 10))
-	//	}
+		return false;
-	//	else if (Data_OpenMVBuf[5] == 45)     // -  向右调
+	AccurateMotor.ForceStop();
-	//	{
+	if (Data_OpenMVBuf[5] == 43)     // +  向左调
-	//		DCMotor.SpeedCtr(40, 0);
+	{
-	//	}
+		AccurateMotor.TurnLeft(Data_OpenMVBuf[4] / 6.5);
-	//}
+	}
-	//else if (Data_OpenMVBuf[6] <= 70)
+	else if (Data_OpenMVBuf[5] == 45)     // -  向右调
-	//{
+	{
-
+		AccurateMotor.TurnRight(Data_OpenMVBuf[4] / 6.5);
-	//	if (Data_OpenMVBuf[5] == 43)     // +  向左调
+	}
-	//	{
+	AccurateMotor.DelayUntilCarStop();
-	//		DCMotor.SpeedCtr(-20, 40);
+	return true;
-	//	}
-	//	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);
-	//	}
-	//}
 }
 
 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;                 //开始识别

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();

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);
+	}
+}

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 <BKRC_Voice.h>
+#include "TextEncoder.h"
+
+class VoiceIdentifyCommand
+{
+private:
+	uint8_t RandomDisc();
+public:
+	bool CMD_ReportAndIdentify();
+	void CMD_ReportAnyText(String text);
+};
+
+#endif
+

VoiceReportCommand.cpp

@@ -2,7 +2,7 @@
 // 标志物通信 - 语音播报 - ZigBee
 // 
 
-#include "VoiceRepotCommand.h"
+#include "VoiceReportCommand.h"
 
 VoiceReportCommand::VoiceReportCommand()
 {

VoiceReportCommand.h

@@ -1,7 +1,7 @@
 // VoiceRepotCommand.h
 
-#ifndef _VOICEREPOTCOMMAND_h
+#ifndef _VOICEREPORTCOMMAND_h
-#define _VOICEREPOTCOMMAND_h
+#define _VOICEREPORTCOMMAND_h
 
 #if defined(ARDUINO) && ARDUINO >= 100
 	#include "arduino.h"

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;
 

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;