RaceSubCar/OpenMVOpt.cpp

#include "OpenMVOpt.h"

uint8_t OpenMVOpt::Data_OpenMVBuf[8] = { 0 };
uint8_t OpenMVOpt::ROI_Query_CMD[8] = { 0x55,0x02,0x91,0xAA,0x00,0x00,0x00,0xBB };

void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
{
	if (Data_OpenMVBuf[6] <= 5)     // <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>У׼
	{
		if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
	}
	else if (Data_OpenMVBuf[6] <= 15)    // <20><><EFBFBD><EFBFBD>΢ƫ
	{
		if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			DCMotor.SpeedCtr(20, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
		{
			DCMotor.SpeedCtr(50, 20);
		}
	}
	else if (Data_OpenMVBuf[6] <= 25)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			DCMotor.SpeedCtr(10, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
		{
			DCMotor.SpeedCtr(50, 10);
		}
	}
	else if (Data_OpenMVBuf[6] <= 35)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			DCMotor.SpeedCtr(5, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
		{
			DCMotor.SpeedCtr(50, 5);
		}
	}
	else
	{
		AccurateMotor.ForceStop();
		if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			AccurateMotor.TurnLeft(Data_OpenMVBuf[6] / 6.5);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
		{
			AccurateMotor.TurnRight(Data_OpenMVBuf[6] / 6.5);
		}
		AccurateMotor.DelayUntilCarStop();
		DCMotor.SpeedCtr(Car_Speed, Car_Speed);
	}
}

bool OpenMVOpt::AdjustCarPoseInPlace()
{
	if ((Data_OpenMVBuf[3] != 0x02) || (Data_OpenMVBuf[4] <= 10))
		return false;
	AccurateMotor.ForceStop();
	if (Data_OpenMVBuf[5] == 43)     // +  <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
	{
		AccurateMotor.TurnLeft(Data_OpenMVBuf[4] / 6.5);
	}
	else if (Data_OpenMVBuf[5] == 45)     // -  <20><><EFBFBD>ҵ<EFBFBD>
	{
		AccurateMotor.TurnRight(Data_OpenMVBuf[4] / 6.5);
	}
	AccurateMotor.DelayUntilCarStop();
	return true;
}

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;                 //<2F><>ʼʶ<CABC><CAB6>
	Command.Judgment(trackdi_buf);         //<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMVTrack_Disc_CloseUp()
{
	trackdi_buf[3] = 0x02;                 //<2F>ر<EFBFBD>ʶ<EFBFBD><CAB6>
	Command.Judgment(trackdi_buf);         //<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed, TrackCallback cb, bool enable_white_detect)
{
	uint32_t num = 0;
	// <20><><EFBFBD>մ<EFBFBD><D5B4>ڻ<EFBFBD><DABB><EFBFBD>
	if (cb == nullptr)
		return;
	while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1);
	delay(800);
	OpenMVTrack_Disc_StartUp();
	DCMotor.SpeedCtr(Car_Speed, Car_Speed);
	while (1)
	{
		if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //<2F><><EFBFBD><EFBFBD>OpenMVʶ<56><CAB6><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
		{
			ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 8);
			if ((Data_OpenMVBuf[0] == 0x55) && (Data_OpenMVBuf[1] == 0x02))
			{
				if(Data_OpenMVBuf[2] == 0x91)
				{
					num++;
					//Serial.println(num);
					if (Data_OpenMVBuf[4] == 1)  // ·<><C2B7>
					{
						DCMotor.Stop();
						OpenMVTrack_Disc_CloseUp();
						break;
					}
					else  // <20><><EFBFBD><EFBFBD>
						AdjustCarPosition(Car_Speed);
				}
				else if ((Data_OpenMVBuf[2] == 0x80) && enable_white_detect)
				{
					DCMotor.Stop();
					OpenMVTrack_Disc_CloseUp();
					if (cb())
					{
						AccurateMotor.RunBackward(5);
						AccurateMotor.DelayUntilCarStop();
						OpenMVTrack_Disc_StartUp();
					}
				}
			}
		}
	}
}

void OpenMVOpt::OpenMV_TrackInPlace()
{
	uint32_t num = 0;
	// <20><><EFBFBD>մ<EFBFBD><D5B4>ڻ<EFBFBD><DABB><EFBFBD>
	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;                 //<2F><>ʼʶ<CABC><CAB6>
	Command.Judgment(qrdi_buf);         //<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

void OpenMVOpt::OpenMVQr_Disc_CloseUp()
{
	qrdi_buf[3] = 0x02;                 //<2F>ر<EFBFBD>ʶ<EFBFBD><CAB6>
	Command.Judgment(qrdi_buf);         //<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

void OpenMVOpt::Servo_Control(int8_t angle)
{
	if (angle >= 0)
	{
		servo_buf[4] = 0x2B;
	}
	else
	{
		servo_buf[4] = 0x2D;
	}
	servo_buf[5] = abs(angle);                 //<2F><>ʼʶ<CABC><CAB6>
	Command.Judgment(servo_buf);         //<2F><><EFBFBD><EFBFBD>У<EFBFBD><D0A3><EFBFBD><EFBFBD>
	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 "";
}