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-21 19:28:17 +08:00
parent e1dd5db1ab
commit cf2a4f69f0
1 changed files with 81 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

108
TaiChi/TaiChi.ino
View File

@ -13,7 +13,7 @@ Servo servo; //舵机实例
#define NORMAL_POINT 0 //普通点
#define CATCH_POINT 1 //抓取点
#define RELEASE_POINT 2 //释放点（使用机械臂）
#define CARRY_POINT 0 //携带点，在算法中可视为普通点
#define CARRY_POINT 3 //携带点（从底盘）
#define GETOUT_POINT 4 //释放点（从底盘）
//坐标点数组定义
@ -94,10 +94,18 @@ bool is_claw_catch = false;
//爪子是否正常
bool is_claw_ok = true;
//底盘是否携带环
bool is_carry = false;
//***************************************************************************************
//****************************************自定函数****************************************
#define NO_JUMP 0
#define JUMP_DEAD_ROAD 1
//更新标记
void UpdateFlag(uint8_t jump_choice = NO_JUMP);
//计算方向，同时更改完成转向后相对下一点的朝向
uint8_t CalcDirection(void);
@ -111,7 +119,7 @@ void LineForward(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);
//抓取环，并判定是否抓取成功
@ -196,13 +204,27 @@ void loop()
Serial.print("#TAICHI: is_claw_catch: "); Serial.print((int)is_claw_catch); Serial.print(" is_claw_ok: "); Serial.println((int)is_claw_ok);
#endif
//情况一：刚完整经过普通点，下一个点为普通点
if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == NORMAL_POINT)
int8_t& passed_flag_type = route[passed_flag][TYPE];
int8_t& next_flag_type = route[next_flag][TYPE];
int8_t& next_next_flag_type = route[next_next_flag][TYPE];
//情况一：刚完整经过普通点，下一个点为普通点或携带点
if (passed_flag_type == NORMAL_POINT && (next_flag_type == NORMAL_POINT || next_flag_type == CARRY_POINT))
{
if (next_position == FRONT_NEXT)
{
//沿线直行，到前端传感器接触下一条线离开函数
LineForward(FRONT_END);
//若下一点为携带点
if (next_flag_type == CARRY_POINT)
{
//底盘携带
is_carry = true;
//越过点成为普通点
next_flag_type = NORMAL_POINT;
}
}
else
{
@ -214,7 +236,7 @@ void loop()
TurnDirection();
}
//情况二：刚完整经过普通点，下一个点为抓取点
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == CATCH_POINT)
else if (passed_flag_type == NORMAL_POINT && next_flag_type == CATCH_POINT)
{
if (!is_claw_catch && !is_claw_ok) //爪子上无环且状态不正常
{
@ -231,7 +253,12 @@ void loop()
move.Stop();
//抓取
CatchAndCheck();
if (!CatchAndCheck())
is_carry = true; //抓取失败
}
else //未进行抓取
{
is_carry = true;
}
//继续沿线直行，到前端传感器接触下一条线离开函数
@ -241,10 +268,10 @@ void loop()
TurnDirection();
//将越过的点视为普通点。因为即使抓取失败，环也会被携带在底盘内
route[next_flag][TYPE] = NORMAL_POINT;
next_flag_type = NORMAL_POINT;
}
//情况三：刚完整经过普通点，下一个点为释放点（使用机械臂）
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == RELEASE_POINT)
else if (passed_flag_type == NORMAL_POINT && next_flag_type == RELEASE_POINT)
{
//沿线直行，在释放位置离开函数
LineForward(RELEASE_END);
@ -252,19 +279,16 @@ void loop()
//停止前进
move.Stop();
if (is_claw_catch) //爪子有环
{
//释放
servo.Release();
delay(RELEASE_DELAY_TIME); //释放留时
is_claw_catch = false;
}
//释放
servo.Release();
delay(RELEASE_DELAY_TIME); //释放留时
is_claw_catch = false;
//下一点朝向为后
next_position = BACK_NEXT;
}
//情况四：刚完整经过释放点（使用机械臂），下一个点为普通点
else if (route[passed_flag][TYPE] == RELEASE_POINT && route[next_flag][TYPE] == NORMAL_POINT)
else if (passed_flag_type == RELEASE_POINT && next_flag_type == NORMAL_POINT)
{
//沿线后退，到后端传感器接触下一条线离开函数
LineBackward(BACK_END);
@ -280,7 +304,7 @@ void loop()
TurnDirection();
}
//情况五：刚完整经过普通点，下一个点为释放点（从底盘）
else if (route[passed_flag][TYPE] == NORMAL_POINT && route[next_flag][TYPE] == GETOUT_POINT)
else if (passed_flag_type == NORMAL_POINT && next_flag_type == GETOUT_POINT)
{
//沿线直行，到后端传感器接触下一条线离开函数
LineForward(BACK_END);
@ -292,10 +316,12 @@ void loop()
next_position = BACK_NEXT;
}
//情况六：刚完整经过释放点（从底盘），下一个点为普通点
else if (route[passed_flag][TYPE] == GETOUT_POINT && route[next_flag][TYPE] == NORMAL_POINT)
else if (passed_flag_type == GETOUT_POINT && next_flag_type == NORMAL_POINT)
{
//沿线后退，到前端传感器接触线离开函数
LineBackward(FRONT_END);
is_carry = false;
//沿线后退，到后端传感器接触线离开函数
LineBackward(BACK_END);
@ -316,14 +342,7 @@ void loop()
}
//更新标记，继续循环
if (++passed_flag > max_flag)
passed_flag = 0;
if (++next_flag > max_flag)
next_flag = 0;
if (++next_next_flag > max_flag)
next_next_flag = 0;
UpdateFlag();
#ifdef TAICHI_DEBUG
Serial.println("#TAICHI: ====================End loop()====================");
@ -331,6 +350,30 @@ void loop()
}
//更新标记
void UpdateFlag(uint8_t jump_choice)
{
if (jump_choice == NO_JUMP)
{
passed_flag = next_flag;
next_flag = next_next_flag;
if (++next_next_flag > max_flag)
next_next_flag = 0;
}
else
{
int current_passed_flag = passed_flag;
UpdateFlag(NO_JUMP);
UpdateFlag(NO_JUMP);
passed_flag = current_passed_flag;
}
}
//计算方向，同时更改完成转向后相对下一点的朝向
uint8_t CalcDirection(void)
{
@ -529,9 +572,20 @@ void LineBackward(uint8_t end_position, float speed_rate)
}
//直行或后退或转向，完成后离开函数但不停止
//直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点
void TurnDirection(float speed_rate)
{
//若下下点为释放点，判断是否需要跳过
if ((route[next_next_flag][TYPE] == RELEASE_POINT && (!is_claw_catch || is_carry)) || (route[next_next_flag][TYPE] == GETOUT_POINT && !is_carry))
{
UpdateFlag(JUMP_DEAD_ROAD);
#ifdef TAICHI_DEBUG
//调试输出直行或后退或转向状态
Serial.println("#TAICHI: JUMP THE RELEASE/GETOUT POINT FOR NOTHING CARRIED");
#endif
}
uint8_t direction = CalcDirection();
#ifdef TAICHI_DEBUG