StopPointTeam/TaiChi
3
0
Fork 1
Code Issues Pull Requests Projects Releases Wiki Activity

新增从底盘释放的情况

Browse Source
This commit is contained in:
lxbpxylps@126.com 2021-02-17 19:52:59 +08:00
parent 8180cfc864
commit 20b548ab12
1 changed files with 42 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
TaiChi/TaiChi.ino
View File

@ -28,9 +28,11 @@ Servo servo; //舵机实例
//****************************************运动路径**************************************** //****************************************运动路径****************************************
//坐标点操作定义 //坐标点操作定义
#define NORMAL_POINT 0 #define NORMAL_POINT 0 //普通点
#define CATCH_POINT 1 #define CATCH_POINT 1 //抓取点
#define RELEASE_POINT 2 #define RELEASE_POINT 2 //释放点（使用机械臂）
#define CARRY_POINT 0 //携带点，在算法中可视为普通点
#define GETOUT_POINT 4 //释放点（从底盘）
//坐标点数组定义 //坐标点数组定义
#define X 0 #define X 0
@ -190,7 +192,7 @@ void loop()
//抓取完成后，将越过的点视为普通点 //抓取完成后，将越过的点视为普通点
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)
{ {
//沿线直行，在释放位置停止 //沿线直行，在释放位置停止
@ -203,7 +205,7 @@ void loop()
//释放完成后，下一点朝向为后 //释放完成后，下一点朝向为后
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)
{ {
//沿线后退，到后端传感器接触下一条线为止 //沿线后退，到后端传感器接触下一条线为止
@ -216,6 +218,24 @@ void loop()
//继续后退或转向 //继续后退或转向
TurnDirection(); TurnDirection();
} }
//情况五：刚完整经过普通点，下一个点为释放点（从底盘）
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == GETOUT_POINT)
{
//沿线直行，到后端传感器接触下一条线为止
LineForward(BACK_END);
//下一点朝向为后
next_position = BACK_NEXT;
}
//情况六：刚完整经过释放点（从底盘），下一个点为普通点
else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT)
{
//沿线后退，到后端传感器接触下一条线为止，以完成释放
LineBackward(BACK_END);
//继续后退或转向
TurnDirection();
}
//出现错误 //出现错误
else else
{ {
@ -495,6 +515,11 @@ bool CatchAndCheckIsDone(float speed)
if (sensor.IsPushed(BUTTON_1)) //开关 1 闭合，即爪子两端接触，说明抓取失败 if (sensor.IsPushed(BUTTON_1)) //开关 1 闭合，即爪子两端接触，说明抓取失败
{ {
#ifdef TAICHI_DEBUG
//调试输出错误信息
Serial.println("#TAICHI: FAIL CATCH!");
#endif
servo.Catch(speed); //重试，此次检测但不重试 servo.Catch(speed); //重试，此次检测但不重试
//等待完成动作 //等待完成动作
@ -502,11 +527,19 @@ bool CatchAndCheckIsDone(float speed)
if (sensor.IsPushed(BUTTON_1)) if (sensor.IsPushed(BUTTON_1))
{ {
#ifdef TAICHI_DEBUG
//调试输出错误信息
Serial.println("#TAICHI: FAIL CATCH AGAIN!");
#endif
return false; return false;
} }
} }
else
{ #ifdef TAICHI_DEBUG
//调试输出错误信息
Serial.println("#TAICHI: SUCCUESS CATCH!");
#endif
return true; return true;
} }
}