#include "OpenMVOpt.h"

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

void OpenMVOpt::AdjustCarPosition(uint8_t Car_Speed)
{
	if (Data_OpenMVBuf[6] <= 5)     // 车身正无需校准
	{
		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(Car_Speed, Car_Speed);
		}
	}
	else if (Data_OpenMVBuf[6] <= 15)    // 车身微偏
	{
		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(20, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 20);
		}
	}
	else if (Data_OpenMVBuf[6] <= 25)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(10, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 10);
		}
	}
	else if (Data_OpenMVBuf[6] <= 35)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(5, 50);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(50, 5);
		}
	}
	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(-5, 30);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(30, -5);
		}
	}
	else if (Data_OpenMVBuf[6] > 70)
	{

		if (Data_OpenMVBuf[5] == 43)     // +  向左调
		{
			DCMotor.SpeedCtr(-10, 20);
		}
		else if (Data_OpenMVBuf[5] == 45)     // -  向右调
		{
			DCMotor.SpeedCtr(20, -10);
		}
	}
}

void OpenMVOpt::OpenMVTrack_Disc_StartUp()
{
	trackdi_buf[3] = 0x01;                 //开始识别
	Command.Judgment(trackdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMVTrack_Disc_CloseUp()
{
	trackdi_buf[3] = 0x02;                 //关闭识别
	Command.Judgment(trackdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, trackdi_buf, 8);
}

void OpenMVOpt::OpenMV_Track(uint8_t Car_Speed)
{
	uint32_t num = 0;
	//Car_Speed = 50;
	// 清空串口缓存
	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);	//增加延迟防止卡死
		if (ExtSRAMInterface.ExMem_Read(0x6038) != 0x00)               //检测OpenMV识别结果
		{
			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)  // 路口
				{
					DCMotor.Stop();
					OpenMVTrack_Disc_CloseUp();
					break;
				}
				else  // 调整
					AdjustCarPosition(Car_Speed);
			}
		}
	}
}

void OpenMVOpt::OpenMVQr_Disc_StartUp()
{
	qrdi_buf[3] = 0x01;                 //开始识别
	Command.Judgment(qrdi_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, qrdi_buf, 8);
}

void OpenMVOpt::OpenMVQr_Disc_CloseUp()
{
	qrdi_buf[3] = 0x02;                 //关闭识别
	Command.Judgment(qrdi_buf);         //计算校验和
	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);                 //开始识别
	Command.Judgment(servo_buf);         //计算校验和
	ExtSRAMInterface.ExMem_Write_Bytes(0x6008, servo_buf, 8);
	delay(1000);
}