|
|
|
|
@ -0,0 +1,334 @@
|
|
|
|
|
//
|
|
|
|
|
// 自动路径运行程序 - 修改自原MCR，去除了对STL库的依赖
|
|
|
|
|
//
|
|
|
|
|
|
|
|
|
|
#include "AutoPathRunner.h"
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::MapPoint::MapPoint()
|
|
|
|
|
{
|
|
|
|
|
x = 0;
|
|
|
|
|
y = 0;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::MapPoint::MapPoint(int x, int y)
|
|
|
|
|
{
|
|
|
|
|
this->x = x;
|
|
|
|
|
this->y = y;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int AutoPathRunner::MapPoint::GetX()
|
|
|
|
|
{
|
|
|
|
|
return x;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
int AutoPathRunner::MapPoint::GetY()
|
|
|
|
|
{
|
|
|
|
|
return y;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AutoPathRunner::MapPoint::SetX(int x)
|
|
|
|
|
{
|
|
|
|
|
this->x = x;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AutoPathRunner::MapPoint::SetY(int y)
|
|
|
|
|
{
|
|
|
|
|
this->y = y;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool AutoPathRunner::MapPoint::NeedTurn(MapPoint next, CarHeadPosition pos)
|
|
|
|
|
{
|
|
|
|
|
if ((pos == CarHeadPosition::X_Positive) || (pos == CarHeadPosition::X_Negitive))
|
|
|
|
|
return (x != next.GetX());
|
|
|
|
|
else
|
|
|
|
|
return (y != next.GetY());
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::TurnDirection AutoPathRunner::MapPoint::CalcTurnDirection(MapPoint next, CarHeadPosition pos)
|
|
|
|
|
{
|
|
|
|
|
TurnDirection direction;
|
|
|
|
|
if (x == next.GetX())
|
|
|
|
|
{
|
|
|
|
|
if (y < next.GetY())
|
|
|
|
|
direction = TurnDirection::Right;
|
|
|
|
|
else if (y > next.GetY())
|
|
|
|
|
direction = TurnDirection::Left;
|
|
|
|
|
else
|
|
|
|
|
direction = TurnDirection::Null;
|
|
|
|
|
}
|
|
|
|
|
else if (y == next.GetY())
|
|
|
|
|
{
|
|
|
|
|
if (x < next.GetX())
|
|
|
|
|
direction = TurnDirection::Right;
|
|
|
|
|
else if (x > next.GetX())
|
|
|
|
|
direction = TurnDirection::Left;
|
|
|
|
|
else
|
|
|
|
|
direction = TurnDirection::Null;
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
direction = TurnDirection::Null;
|
|
|
|
|
if ((pos == CarHeadPosition::X_Negitive) || (pos == CarHeadPosition::Y_Negitive))
|
|
|
|
|
{
|
|
|
|
|
if (direction == TurnDirection::Left)
|
|
|
|
|
direction = TurnDirection::Right;
|
|
|
|
|
else if (direction == TurnDirection::Right)
|
|
|
|
|
direction = TurnDirection::Left;
|
|
|
|
|
}
|
|
|
|
|
return direction;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::MapPoint AutoPathRunner::MapPoint::operator=(const MapPoint& obj)
|
|
|
|
|
{
|
|
|
|
|
this->x = obj.x;
|
|
|
|
|
this->y = obj.y;
|
|
|
|
|
return (*this);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool AutoPathRunner::MapPoint::operator==(const MapPoint& obj)
|
|
|
|
|
{
|
|
|
|
|
return ((x == obj.x) && (y == obj.y));
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::PointMapper::PointMapper()
|
|
|
|
|
{
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::PointMapper::PointMapper(const char* n, const MapPoint& p)
|
|
|
|
|
{
|
|
|
|
|
strncpy(name, n, 3);
|
|
|
|
|
point = p;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::PointMapper std_points[49] =
|
|
|
|
|
{
|
|
|
|
|
{"A1", AutoPathRunner::MapPoint(0,0)},
|
|
|
|
|
{"A2", AutoPathRunner::MapPoint(0,1)},
|
|
|
|
|
{"A3", AutoPathRunner::MapPoint(0,2)},
|
|
|
|
|
{"A4", AutoPathRunner::MapPoint(0,3)},
|
|
|
|
|
{"A5", AutoPathRunner::MapPoint(0,4)},
|
|
|
|
|
{"A6", AutoPathRunner::MapPoint(0,5)},
|
|
|
|
|
{"A7", AutoPathRunner::MapPoint(0,6)},
|
|
|
|
|
{"B1", AutoPathRunner::MapPoint(1,0)},
|
|
|
|
|
{"B2", AutoPathRunner::MapPoint(1,1)},
|
|
|
|
|
{"B3", AutoPathRunner::MapPoint(1,2)},
|
|
|
|
|
{"B4", AutoPathRunner::MapPoint(1,3)},
|
|
|
|
|
{"B5", AutoPathRunner::MapPoint(1,4)},
|
|
|
|
|
{"B6", AutoPathRunner::MapPoint(1,5)},
|
|
|
|
|
{"B7", AutoPathRunner::MapPoint(1,6)},
|
|
|
|
|
{"C1", AutoPathRunner::MapPoint(2,0)},
|
|
|
|
|
{"C2", AutoPathRunner::MapPoint(2,1)},
|
|
|
|
|
{"C3", AutoPathRunner::MapPoint(2,2)},
|
|
|
|
|
{"C4", AutoPathRunner::MapPoint(2,3)},
|
|
|
|
|
{"C5", AutoPathRunner::MapPoint(2,4)},
|
|
|
|
|
{"C6", AutoPathRunner::MapPoint(2,5)},
|
|
|
|
|
{"C7", AutoPathRunner::MapPoint(2,6)},
|
|
|
|
|
{"D1", AutoPathRunner::MapPoint(3,0)},
|
|
|
|
|
{"D2", AutoPathRunner::MapPoint(3,1)},
|
|
|
|
|
{"D3", AutoPathRunner::MapPoint(3,2)},
|
|
|
|
|
{"D4", AutoPathRunner::MapPoint(3,3)},
|
|
|
|
|
{"D5", AutoPathRunner::MapPoint(3,4)},
|
|
|
|
|
{"D6", AutoPathRunner::MapPoint(3,5)},
|
|
|
|
|
{"D7", AutoPathRunner::MapPoint(3,6)},
|
|
|
|
|
{"E1", AutoPathRunner::MapPoint(4,0)},
|
|
|
|
|
{"E2", AutoPathRunner::MapPoint(4,1)},
|
|
|
|
|
{"E3", AutoPathRunner::MapPoint(4,2)},
|
|
|
|
|
{"E4", AutoPathRunner::MapPoint(4,3)},
|
|
|
|
|
{"E5", AutoPathRunner::MapPoint(4,4)},
|
|
|
|
|
{"E6", AutoPathRunner::MapPoint(4,5)},
|
|
|
|
|
{"E7", AutoPathRunner::MapPoint(4,6)},
|
|
|
|
|
{"F1", AutoPathRunner::MapPoint(5,0)},
|
|
|
|
|
{"F2", AutoPathRunner::MapPoint(5,1)},
|
|
|
|
|
{"F3", AutoPathRunner::MapPoint(5,2)},
|
|
|
|
|
{"F4", AutoPathRunner::MapPoint(5,3)},
|
|
|
|
|
{"F5", AutoPathRunner::MapPoint(5,4)},
|
|
|
|
|
{"F6", AutoPathRunner::MapPoint(5,5)},
|
|
|
|
|
{"F7", AutoPathRunner::MapPoint(5,6)},
|
|
|
|
|
{"G1", AutoPathRunner::MapPoint(6,0)},
|
|
|
|
|
{"G2", AutoPathRunner::MapPoint(6,1)},
|
|
|
|
|
{"G3", AutoPathRunner::MapPoint(6,2)},
|
|
|
|
|
{"G4", AutoPathRunner::MapPoint(6,3)},
|
|
|
|
|
{"G5", AutoPathRunner::MapPoint(6,4)},
|
|
|
|
|
{"G6", AutoPathRunner::MapPoint(6,5)},
|
|
|
|
|
{"G7", AutoPathRunner::MapPoint(6,6)}
|
|
|
|
|
};
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::MapPoint AutoPathRunner::Mapping(const char* name)
|
|
|
|
|
{
|
|
|
|
|
for (auto& [key, value] : std_points)
|
|
|
|
|
if (strcmp(key, name) == 0)
|
|
|
|
|
return value;
|
|
|
|
|
return MapPoint();
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
const char* AutoPathRunner::ReverseMapping(MapPoint point)
|
|
|
|
|
{
|
|
|
|
|
for (auto& [key, value] : std_points)
|
|
|
|
|
if (value == point)
|
|
|
|
|
return key;
|
|
|
|
|
return "";
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AutoPathRunner::ChangePositon(CarHeadPosition& pos, TurnDirection turn)
|
|
|
|
|
{
|
|
|
|
|
switch (pos)
|
|
|
|
|
{
|
|
|
|
|
case CarHeadPosition::X_Positive:
|
|
|
|
|
if (turn == TurnDirection::Left)
|
|
|
|
|
pos = CarHeadPosition::Y_Negitive;
|
|
|
|
|
else if (turn == TurnDirection::Right)
|
|
|
|
|
pos = CarHeadPosition::Y_Positive;
|
|
|
|
|
break;
|
|
|
|
|
case CarHeadPosition::X_Negitive:
|
|
|
|
|
if (turn == TurnDirection::Left)
|
|
|
|
|
pos = CarHeadPosition::Y_Positive;
|
|
|
|
|
else if (turn == TurnDirection::Right)
|
|
|
|
|
pos = CarHeadPosition::Y_Negitive;
|
|
|
|
|
break;
|
|
|
|
|
case CarHeadPosition::Y_Positive:
|
|
|
|
|
if (turn == TurnDirection::Left)
|
|
|
|
|
pos = CarHeadPosition::X_Positive;
|
|
|
|
|
else if (turn == TurnDirection::Right)
|
|
|
|
|
pos = CarHeadPosition::X_Negitive;
|
|
|
|
|
break;
|
|
|
|
|
case CarHeadPosition::Y_Negitive:
|
|
|
|
|
if (turn == TurnDirection::Left)
|
|
|
|
|
pos = CarHeadPosition::X_Negitive;
|
|
|
|
|
else if (turn == TurnDirection::Right)
|
|
|
|
|
pos = CarHeadPosition::X_Positive;
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::CarHeadPosition AutoPathRunner::Int2Position(int pos)
|
|
|
|
|
{
|
|
|
|
|
switch (pos)
|
|
|
|
|
{
|
|
|
|
|
case 0:
|
|
|
|
|
return CarHeadPosition::X_Positive;
|
|
|
|
|
case 1:
|
|
|
|
|
return CarHeadPosition::X_Negitive;
|
|
|
|
|
case 2:
|
|
|
|
|
return CarHeadPosition::Y_Positive;
|
|
|
|
|
default:
|
|
|
|
|
return CarHeadPosition::Y_Negitive;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool AutoPathRunner::TrackCallback()
|
|
|
|
|
{
|
|
|
|
|
AccurateMotor.RunBackward(6);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
if (IsSpecTerrainItem())
|
|
|
|
|
AccurateMotor.RunForward(30);
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
AccurateMotor.RunForward(12);
|
|
|
|
|
}
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
return true;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
AutoPathRunner::MapPoint* AutoPathRunner::DecodeRoute(const char* route, int& point_cnt)
|
|
|
|
|
{
|
|
|
|
|
char temp[3] = { 0 };
|
|
|
|
|
MapPoint* list = new MapPoint[(strlen(route) + 1) / 3];
|
|
|
|
|
int str_index = 0, point_index = 0;
|
|
|
|
|
char* route_cpy = new char[strlen(route) + 1];
|
|
|
|
|
memset(route_cpy, 0, strlen(route) + 1);
|
|
|
|
|
strncpy(route_cpy, route, strlen(route));
|
|
|
|
|
memset(list, 0, sizeof(list));
|
|
|
|
|
while (sscanf(route_cpy, "%c%c-", &temp[0], &temp[1]) != 0)
|
|
|
|
|
{
|
|
|
|
|
str_index += 3;
|
|
|
|
|
if (str_index >= strlen(route))
|
|
|
|
|
break;
|
|
|
|
|
if (point_index < ((strlen(route) + 1) / 3))
|
|
|
|
|
list[point_index++] = Mapping(temp);
|
|
|
|
|
memset(temp, 0, sizeof(temp));
|
|
|
|
|
memset(route_cpy, 0, strlen(route) + 1);
|
|
|
|
|
strncpy(route_cpy, route + str_index, strlen(route) - str_index);
|
|
|
|
|
}
|
|
|
|
|
point_cnt = point_index;
|
|
|
|
|
delete[] route_cpy;
|
|
|
|
|
return list;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
void AutoPathRunner::DoRun(MapPoint* points, int point_cnt, CarHeadPosition pos)
|
|
|
|
|
{
|
|
|
|
|
//调整K210舵机至循迹角度
|
|
|
|
|
OpenMVOpt::Servo_Control(-60);
|
|
|
|
|
MapPoint last_point = points[0];
|
|
|
|
|
for (int i = 1; i < point_cnt; i++)
|
|
|
|
|
{
|
|
|
|
|
if (i != 1)
|
|
|
|
|
{
|
|
|
|
|
//向前一定距离
|
|
|
|
|
AccurateMotor.RunForward(3);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
}
|
|
|
|
|
if (last_point.NeedTurn(points[i], pos))
|
|
|
|
|
{
|
|
|
|
|
//TurnFlag = true;
|
|
|
|
|
switch (last_point.CalcTurnDirection(points[i], pos))
|
|
|
|
|
{
|
|
|
|
|
case TurnDirection::Left:
|
|
|
|
|
//左转
|
|
|
|
|
AccurateMotor.TurnLeft(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
ChangePositon(pos, TurnDirection::Left);
|
|
|
|
|
break;
|
|
|
|
|
case TurnDirection::Right:
|
|
|
|
|
//右转
|
|
|
|
|
AccurateMotor.TurnRight(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
ChangePositon(pos, TurnDirection::Right);
|
|
|
|
|
break;
|
|
|
|
|
default:
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
//向前循迹
|
|
|
|
|
OpenMVOpt::OpenMV_Track(8, TrackCallback);
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
//向前循迹
|
|
|
|
|
OpenMVOpt::OpenMV_Track(8, TrackCallback);
|
|
|
|
|
}
|
|
|
|
|
last_point = points[i];
|
|
|
|
|
}
|
|
|
|
|
delete[] points;
|
|
|
|
|
//恢复K210舵机角度
|
|
|
|
|
delay(1000);
|
|
|
|
|
OpenMVOpt::OpenMVTrack_Disc_CloseUp();
|
|
|
|
|
delay(500);
|
|
|
|
|
OpenMVOpt::Servo_Control(0);
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
bool AutoPathRunner::IsSpecTerrainItem()
|
|
|
|
|
{
|
|
|
|
|
double dis = Ultrasonic.Ranging(CM);
|
|
|
|
|
AccurateMotor.TurnLeft(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.RunForward(5);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.TurnRight(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
delay(200);
|
|
|
|
|
double dis_left = Ultrasonic.Ranging(CM);
|
|
|
|
|
AccurateMotor.TurnRight(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.RunForward(10);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.TurnLeft(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
delay(200);
|
|
|
|
|
double dis_right = Ultrasonic.Ranging(CM);
|
|
|
|
|
AccurateMotor.TurnRight(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.RunBackward(5);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
AccurateMotor.TurnLeft(90);
|
|
|
|
|
AccurateMotor.DelayUntilCarStop();
|
|
|
|
|
delay(200);
|
|
|
|
|
return (((dis - dis_left) >= 10) || ((dis - dis_right) >= 10));
|
|
|
|
|
}
|
UnknownObject
2 years ago