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-18 14:31:51 +08:00
parent 922006a42c
commit d373813a81
1 changed files with 67 additions and 37 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

94
TaiChi/TaiChi.ino
View File

@ -101,6 +101,9 @@ int max_flag;
//下一点朝向 //下一点朝向
uint8_t next_position = FRONT_NEXT; uint8_t next_position = FRONT_NEXT;
//爪子是否抓有环
bool is_claw_catch = false;
//爪子是否正常 //爪子是否正常
bool is_claw_ok = true; bool is_claw_ok = true;
//*************************************************************************************** //***************************************************************************************
@ -114,17 +117,17 @@ uint8_t CalcDirection(void);
#define BACK_END 1 #define BACK_END 1
#define CATCH_END 2 #define CATCH_END 2
#define RELEASE_END 3 #define RELEASE_END 3
//沿线直行，在触发条件后停止 //沿线直行，在触发条件后离开函数但不停止
void LineForward(uint8_t end_position, float speed_rate = 1.0); void LineForward(uint8_t end_position, float speed_rate = 1.0);
//沿线后退，在触发条件后停止 //沿线后退，在触发条件后离开函数但不停止
void LineBackward(uint8_t end_position, float speed_rate = 1.0); void LineBackward(uint8_t end_position, float speed_rate = 1.0);
//直行或后退或转向 //直行或后退或转向，完成后离开函数但不停止
void TurnDirection(float speed_rate = 1.0); void TurnDirection(float speed_rate = 1.0);
//抓取环，并判定是否抓取成功 //抓取环，并判定是否抓取成功
bool CatchAndCheckIsDone(float speed = 1.0); bool CatchAndCheck(float speed = 1.0);
//打开爪子，并判断爪子是否能正常工作 //打开爪子，并判断爪子是否能正常工作
bool OpenClawAndCheck(void); bool OpenClawAndCheck(void);
@ -146,7 +149,7 @@ void setup()
max_flag = sizeof(route) / sizeof(route[0]) - 1; max_flag = sizeof(route) / sizeof(route[0]) - 1;
//前往 0, 0 //前往 0, 0
//沿线直行，到后端传感器接触下一条线为止 //沿线直行，到后端传感器接触下一条线离开函数
LineForward(BACK_END); LineForward(BACK_END);
//已越过 0, 0 正式进入循环 //已越过 0, 0 正式进入循环
@ -166,19 +169,20 @@ void loop()
Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]"); Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]");
Serial.print(" next_position: "); Serial.print((int)next_position); Serial.print(" next_position: "); Serial.print((int)next_position);
Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]); Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]);
Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok);
#endif #endif
//情况一：刚完整经过普通点，下一个点为普通点 //情况一：刚完整经过普通点，下一个点为普通点或携带点
if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == NORMAL_POINT) if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == NORMAL_POINT)
{ {
if (next_position == FRONT_NEXT) if (next_position == FRONT_NEXT)
{ {
//沿线直行，到前端传感器接触下一条线为止 //沿线直行，到前端传感器接触下一条线离开函数
LineForward(FRONT_END); LineForward(FRONT_END);
} }
else else
{ {
//沿线后退，到后端传感器接触下一条线为止 //沿线后退，到后端传感器接触下一条线离开函数
LineBackward(BACK_END); LineBackward(BACK_END);
} }
@ -188,40 +192,62 @@ void loop()
//情况二：刚完整经过普通点，下一个点为抓取点 //情况二：刚完整经过普通点，下一个点为抓取点
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == CATCH_POINT) else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == CATCH_POINT)
{ {
//沿线直行，在抓取位置停止 if (!is_claw_catch && !is_claw_ok) //爪子上无环且状态不正常
{
move.Stop(); //停止前进
OpenClawAndCheck(); //再次检测爪子是否正常
}
if (!is_claw_catch && is_claw_ok) //爪子上无环且状态正常
{
//沿线直行，在抓取位置离开函数
LineForward(CATCH_END); LineForward(CATCH_END);
//抓取 //停止前进
CatchAndCheckIsDone(); move.Stop();
//继续沿线直行，到前端传感器接触下一条线为止 //抓取
CatchAndCheck();
}
//继续沿线直行，到前端传感器接触下一条线离开函数
LineForward(FRONT_END); LineForward(FRONT_END);
//继续直行或后退或转向 //继续直行或后退或转向
TurnDirection(); TurnDirection();
//抓取完成后，将越过的点视为普通点 //将越过的点视为普通点。因为即使抓取失败，环也会被携带在底盘内
route[next_flag][TYPE] = NORMAL_POINT; route[next_flag][TYPE] = NORMAL_POINT;
} }
//情况三：刚完整经过普通点，下一个点为释放点（使用机械臂） //情况三：刚完整经过普通点，下一个点为释放点（使用机械臂）
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == RELEASE_POINT) else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == RELEASE_POINT)
{ {
//沿线直行，在释放位置停止 //沿线直行，在释放位置离开函数
LineForward(RELEASE_END); LineForward(RELEASE_END);
//停止前进
move.Stop();
if (is_claw_catch) //爪子有环
{
//释放 //释放
servo.Release(); servo.Release();
delay(RELEASE_DELAY_TIME); //释放留时 delay(RELEASE_DELAY_TIME); //释放留时
is_claw_catch = false;
}
//释放完成后，下一点朝向为后 //下一点朝向为后
next_position = BACK_NEXT; next_position = BACK_NEXT;
} }
//情况四：刚完整经过释放点（使用机械臂），下一个点为普通点 //情况四：刚完整经过释放点（使用机械臂），下一个点为普通点
else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT) else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT)
{ {
//沿线后退，到后端传感器接触下一条线为止 //沿线后退，到后端传感器接触下一条线离开函数
LineBackward(BACK_END); LineBackward(BACK_END);
//停止前进
move.Stop();
//机械臂复原 //机械臂复原
servo.Reset(); servo.Reset();
delay(RESET_DELAY_TIME); //复原留时 delay(RESET_DELAY_TIME); //复原留时
@ -232,16 +258,19 @@ void loop()
//情况五：刚完整经过普通点，下一个点为释放点（从底盘） //情况五：刚完整经过普通点，下一个点为释放点（从底盘）
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == GETOUT_POINT) else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == GETOUT_POINT)
{ {
//沿线直行，到后端传感器接触下一条线为止 //沿线直行，到后端传感器接触下一条线离开函数
LineForward(BACK_END); LineForward(BACK_END);
//停止前进
move.Stop();
//下一点朝向为后 //下一点朝向为后
next_position = BACK_NEXT; next_position = BACK_NEXT;
} }
//情况六：刚完整经过释放点（从底盘），下一个点为普通点 //情况六：刚完整经过释放点（从底盘），下一个点为普通点
else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT) else if (route[passed_flag][TYPE] == GETOUT_POINT && route[next_flag][TYPE] == NORMAL_POINT)
{ {
//沿线后退，到后端传感器接触下一条线为止，以完成释放 //沿线后退，到后端传感器接触下一条线离开函数，以完成释放
LineBackward(BACK_END); LineBackward(BACK_END);
//继续后退或转向 //继续后退或转向
@ -253,7 +282,6 @@ void loop()
move.Stop(); move.Stop();
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
Serial.println("#TAICHI: **********FAIL TO RUN NEW LOOP!**********");
Serial.print("#TAICHI: loop_time: "); Serial.println(loop_time); Serial.print("#TAICHI: loop_time: "); Serial.println(loop_time);
Serial.print("#TAICHI: pass: ["); Serial.print(route[passed_flag][X]); Serial.print(", "); Serial.print(route[passed_flag][Y]); Serial.print("]"); Serial.print("#TAICHI: pass: ["); Serial.print(route[passed_flag][X]); Serial.print(", "); Serial.print(route[passed_flag][Y]); Serial.print("]");
Serial.print(" flag: "); Serial.print(passed_flag); Serial.print(" flag: "); Serial.print(passed_flag);
@ -261,6 +289,7 @@ void loop()
Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]"); Serial.print("#TAICHI: next: ["); Serial.print(route[next_flag][X]); Serial.print(", "); Serial.print(route[next_flag][Y]); Serial.print("]");
Serial.print(" next_position: "); Serial.print((int)next_position); Serial.print(" next_position: "); Serial.print((int)next_position);
Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]); Serial.print(" TYPE: "); Serial.println((int)route[next_flag][TYPE]);
Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok);
#endif #endif
while (1) {} while (1) {}
@ -370,7 +399,7 @@ uint8_t CalcDirection(void)
} }
//沿线直行，在触发条件后停止 //沿线直行，在触发条件后离开函数但不停止
void LineForward(uint8_t end_position, float speed_rate) void LineForward(uint8_t end_position, float speed_rate)
{ {
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
@ -402,22 +431,22 @@ void LineForward(uint8_t end_position, float speed_rate)
move.Backward(LINE_FIND_SPEED_RATE); move.Backward(LINE_FIND_SPEED_RATE);
} }
if (end_position == FRONT_END) //前端接触线为止 if (end_position == FRONT_END) //前端接触线离开函数
{ {
if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2)) if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2))
break; break;
} }
else if (end_position == BACK_END) //后端接触线为止 else if (end_position == BACK_END) //后端接触线离开函数
{ {
if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6)) if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6))
break; break;
} }
else if (end_position == CATCH_END) //到达抓取位置为止 else if (end_position == CATCH_END) //到达抓取位置离开函数
{ {
if (millis() - begin_time > CATCH_MOVE_DELAY_TIME) if (millis() - begin_time > CATCH_MOVE_DELAY_TIME)
break; break;
} }
else //到达释放位置为止 else //到达释放位置离开函数
{ {
if (millis() - begin_time > RELEASE_MOVE_DELAY_TIME) if (millis() - begin_time > RELEASE_MOVE_DELAY_TIME)
break; break;
@ -431,7 +460,7 @@ void LineForward(uint8_t end_position, float speed_rate)
} }
//沿线后退，在触发条件后停止 //沿线后退，在触发条件后离开函数但不停止
void LineBackward(uint8_t end_position, float speed_rate) void LineBackward(uint8_t end_position, float speed_rate)
{ {
#ifdef TAICHI_DEBUG #ifdef TAICHI_DEBUG
@ -461,12 +490,12 @@ void LineBackward(uint8_t end_position, float speed_rate)
} }
if (end_position == FRONT_END) //前端接触线为止 if (end_position == FRONT_END) //前端接触线离开函数
{ {
if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2)) if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2))
break; break;
} }
else //后端接触线为止 else //后端接触线离开函数
{ {
if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6)) if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6))
break; break;
@ -480,7 +509,7 @@ void LineBackward(uint8_t end_position, float speed_rate)
} }
//直行或后退或转向 //直行或后退或转向，完成后离开函数但不停止
void TurnDirection(float speed_rate) void TurnDirection(float speed_rate)
{ {
uint8_t direction = CalcDirection(); uint8_t direction = CalcDirection();
@ -494,12 +523,12 @@ void TurnDirection(float speed_rate)
if (direction == FORWARD) //继续直行 if (direction == FORWARD) //继续直行
{ {
//沿线直行，到后端传感器接触线为止 //沿线直行，到后端传感器接触线离开函数
LineForward(BACK_END, speed_rate); LineForward(BACK_END, speed_rate);
} }
else if (direction == BACKWARD) //继续后退 else if (direction == BACKWARD) //继续后退
{ {
//沿线后退，到前端传感器接触线为止 //沿线后退，到前端传感器接触线离开函数
LineBackward(FRONT_END, speed_rate); LineBackward(FRONT_END, speed_rate);
} }
else //继续转向 else //继续转向
@ -519,7 +548,7 @@ void TurnDirection(float speed_rate)
//抓取环，并判定是否抓取成功 //抓取环，并判定是否抓取成功
bool CatchAndCheckIsDone(float speed) bool CatchAndCheck(float speed)
{ {
//记录开始时间 //记录开始时间
unsigned long begin_time = millis(); unsigned long begin_time = millis();
@ -559,6 +588,7 @@ bool CatchAndCheckIsDone(float speed)
Serial.println("#TAICHI: SUCCUESS CATCH!"); Serial.println("#TAICHI: SUCCUESS CATCH!");
#endif #endif
is_claw_catch = true;
return true; return true;
} }