MYGL/Car
4
0
Fork 0
forked from StopPointTeam/TaiChi
Code Issues Pull Requests Projects Releases Wiki Activity

修正方向计算函数

Browse Source
This commit is contained in:
lxbpxylps@126.com 2021-02-14 13:36:38 +08:00
parent 1c74d8a509
commit 4f54956f09
1 changed files with 21 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
TaiChi/TaiChi.ino
View File

@ -203,29 +203,42 @@ uint8_t CalcDirection(void)
rx = -ry0; rx = -ry0;
ry = rx0; ry = rx0;
} }
else return 254; //DEBUG
//判断行进方向 //判断行进方向
if (rx == 0 && ry == 2) if (rx == 0 && ry == 2)
{ {
if (next_position == FRONT_NEXT) if (next_position == FRONT_NEXT)
{ {
return FORWARD; //正对下一点 return FORWARD;
} }
else else
{ {
return BACKWARD; //后退，背对下一点 return BACKWARD;
}
}
else if (rx == 0 && ry == 0)
{
if (next_position == FRONT_NEXT)
{
next_position = BACK_NEXT;
return BACKWARD;
}
else
{
next_position = FRONT_NEXT;
return FORWARD;
} }
} }
else if (rx == -1 && ry == 1) else if (rx == -1 && ry == 1)
{ {
if (next_position == FRONT_NEXT) if (next_position == FRONT_NEXT)
{ {
return FORLEFTWARD; //正对下一点 return FORLEFTWARD;
} }
else else
{ {
next_position = FRONT_NEXT; //向左后退，正对下一点 next_position = FRONT_NEXT;
return BACKLEFTWARD; return BACKLEFTWARD;
} }
} }
@ -233,11 +246,11 @@ uint8_t CalcDirection(void)
{ {
if (next_position == FRONT_NEXT) if (next_position == FRONT_NEXT)
{ {
return FORRIGHTWARD; //正对下一点 return FORRIGHTWARD;
} }
else else
{ {
next_position = FRONT_NEXT; //向右后退，正对下一点 next_position = FRONT_NEXT;
return BACKRIGHTWARD; return BACKRIGHTWARD;
} }
} }