RaceSubCar/OpenMVOpt.cpp

#include "OpenMVOpt.h"

uint8_t OpenMVOpt::Data_OpenMVBuf[8] = { 0 };

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 if (Data_OpenMVBuf[6] <= 50)
	{

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

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

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

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)
{
	uint32_t num = 0;
	Car_Speed = 50;
	// <20><><EFBFBD>մ<EFBFBD><D5B4>ڻ<EFBFBD><DABB><EFBFBD>
	while (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)
		ExtSRAMInterface.ExMem_Read_Bytes(0x6038, Data_OpenMVBuf, 1);
	OpenMVTrack_Disc_StartUp();
	delay(500);
	DCMotor.SpeedCtr(Car_Speed, Car_Speed);
	Serial.println("Video Track Start.");
	while (1)
	{
		delay(10);	//<2F><><EFBFBD><EFBFBD><EFBFBD>ӳٷ<D3B3>ֹ<EFBFBD><D6B9><EFBFBD><EFBFBD>
		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) && (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);
			}
		}
	}
}

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