diff --git a/TaiChi/TaiChi.ino b/TaiChi/TaiChi.ino index 715323e..1bd5dbe 100644 --- a/TaiChi/TaiChi.ino +++ b/TaiChi/TaiChi.ino @@ -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