You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
278 lines
6.6 KiB
C++
278 lines
6.6 KiB
C++
#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) // ³µÉíÕýÎÞÐèУ׼
|
|
{
|
|
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
|
|
{
|
|
AccurateMotor.ForceStop();
|
|
if (Data_OpenMVBuf[5] == 43) // + Ïò×óµ÷
|
|
{
|
|
AccurateMotor.TurnLeft(Data_OpenMVBuf[6] / 6.5);
|
|
}
|
|
else if (Data_OpenMVBuf[5] == 45) // - ÏòÓÒµ÷
|
|
{
|
|
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) // + Ïò×óµ÷
|
|
{
|
|
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()
|
|
{
|
|
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; //¿ªÊ¼Ê¶±ð
|
|
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, TrackCallback cb, bool enable_white_detect)
|
|
{
|
|
uint32_t num = 0;
|
|
// Çå¿Õ´®¿Ú»º´æ
|
|
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) //¼ì²âOpenMVʶ±ð½á¹û
|
|
{
|
|
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) // ·¿Ú
|
|
{
|
|
DCMotor.Stop();
|
|
OpenMVTrack_Disc_CloseUp();
|
|
break;
|
|
}
|
|
else // µ÷Õû
|
|
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;
|
|
// Çå¿Õ´®¿Ú»º´æ
|
|
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; //¿ªÊ¼Ê¶±ð
|
|
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);
|
|
}
|
|
|
|
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 "";
|
|
}
|