From 562d317b13b410993f9bc7f45def09f741311e21 Mon Sep 17 00:00:00 2001 From: "lxbpxylps@126.com" Date: Wed, 10 Mar 2021 23:41:12 +0800 Subject: [PATCH] =?UTF-8?q?=E9=A6=96=E4=B8=AA=E8=83=BD=E5=AE=8C=E6=95=B4?= =?UTF-8?q?=E6=AD=A3=E7=A1=AE=E8=BF=90=E8=A1=8C=E7=9A=84=E7=89=88=E6=9C=AC?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- TaiChi/TaiChi.ino | 298 ++++++++++++++++++++++++++++++++++------------ 1 file changed, 225 insertions(+), 73 deletions(-) diff --git a/TaiChi/TaiChi.ino b/TaiChi/TaiChi.ino index e7656a5..8bbd496 100644 --- a/TaiChi/TaiChi.ino +++ b/TaiChi/TaiChi.ino @@ -15,6 +15,7 @@ Servo servo; //舵机实例 #define RELEASE_POINT 2 //释放点（使用机械臂） #define CARRY_POINT 3 //携带点（从底盘） #define GETOUT_POINT 4 //释放点（从底盘） +#define GAIN_POINT 5 //增益点 //坐标点数组定义 #define X 0 @@ -25,63 +26,61 @@ int8_t route[][3] = { {0, 0, NORMAL_POINT}, {0, 1, NORMAL_POINT}, - {1, 1, CARRY_POINT}, - {2, 1, NORMAL_POINT}, - {2, 2, CATCH_POINT}, - {2, 3, NORMAL_POINT}, - {3, 3, GETOUT_POINT}, - {2, 3, NORMAL_POINT}, - {2, 4, NORMAL_POINT}, - {3, 4, RELEASE_POINT}, - {2, 4, NORMAL_POINT}, - {2, 3, NORMAL_POINT}, - {1, 3, NORMAL_POINT}, - {0, 3, NORMAL_POINT}, - {0, 2, CATCH_POINT}, - {1, 2, NORMAL_POINT}, - {2, 2, NORMAL_POINT}, - {3, 2, RELEASE_POINT}, - {2, 2, NORMAL_POINT}, - {2, 3, NORMAL_POINT}, - {3, 3, GETOUT_POINT}, - {2, 3, NORMAL_POINT} + {0, 2, NORMAL_POINT}, + {-1, 2, NORMAL_POINT}, + {-1, 1, CATCH_POINT}, + {0, 1, NORMAL_POINT}, + {0, 0, NORMAL_POINT}, + {-1, 0, RELEASE_POINT} }; //*************************************************************************************** //****************************************可调参数**************************************** +//是否关闭爪子状态检测功能 +#define DISABLE_CLAW_CHECK + //抓取点移动用时 -#define CATCH_MOVE_DELAY_TIME 700 -//释放点移动用时 -#define RELEASE_MOVE_DELAY_TIME 700 +#define CATCH_MOVE_DELAY_TIME 500 +//释放点向前移动用时 +#define RELEASE_MOVE_FORWARD_DELAY_TIME 1000 +//释放点暂停用时 +#define RELEASE_STOP_DELAY_TIME 500 +//释放点向后向前移动用时 +#define RELEASE_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME 460 +//增益点向后向前移动用时 +#define GAIN_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME 460 +//增益点稍稍向前移动用时 +#define GAIN_MOVE_LITTLEFORWARD_DELAY_TIME 200 //重置留时 -#define RESET_DELAY_TIME 5000 +#define RESET_DELAY_TIME 1600 +//放下爪子用时 +#define DOWN_DELAY_TIME 1600 //抓取留时 -#define CATCH_DELAY_TIME 5000 +#define CATCH_DELAY_TIME 2100 //释放留时 -#define RELEASE_DELAY_TIME 5000 +#define RELEASE_DELAY_TIME 3100 +//增益放下爪子用时 +#define GAINDOWN_DELAY_TIME 1600 +//增益抓取用时 +#define GAINCATCH_DELAY_TIME 3100 //最大抓取尝试次数 #define MAX_CATCH_TIMES 2 //（前进转）从触碰白线到开始转向延时 -#define BEFORE_FORTURN_DELAY 700 +#define BEFORE_FORTURN_DELAY 670 //（后退转）从触碰白线到开始转向延时 -#define BEFORE_BACKTURN_DELAY 700 +#define BEFORE_BACKTURN_DELAY 500 //开始转到开始检测是否摆正的延时 #define BEFORE_CHECK_STRAIGHT_DELAY 700 - -//修正循迹时单侧减速比率 -#define LINE_FIX_SPEED_RATE 0.4 -//低速重新寻线时减速比率 -#define LINE_FIND_SPEED_RATE 0.3 //*************************************************************************************** //****************************************全局变量**************************************** -// GND Pins +//GND Pins uint8_t gnd_pins[4] = {14, 15, 16, 17}; //数组位置标记 @@ -123,6 +122,9 @@ void UpdateFlag(uint8_t jump_choice = NO_JUMP); //计算方向，同时更改完成转向后相对下一点的朝向 uint8_t CalcDirection(void); +//由灰度比计算修正减速比 +float CalcFixSpeedRate(float gray_deviation_rate); + #define FRONT_END 0 #define BACK_END 1 #define CATCH_END 2 @@ -136,8 +138,10 @@ void LineBackward(uint8_t end_position, float speed_rate = 1.0); //直行或后退或转向，完成后离开函数但不停止。会自动跳过无需前往的释放点 void TurnDirection(float speed_rate = 1.0); +#define CATCH_TYPE_CATCH 0 +#define CATCH_TYPE_GAIN 1 //抓取环，并判定是否抓取成功 -bool CatchAndCheck(float speed = 1.0); +bool CatchAndCheck(uint8_t type = CATCH_TYPE_CATCH, float speed = 1.0); //打开爪子，并判断爪子是否能正常工作 bool OpenClawAndCheck(void); @@ -159,7 +163,19 @@ int loop_time = 0; //中断函数，用于调试时暂停程序 void DebugPause(void) { + //记录之前的运动状态 + uint8_t current_direction = move.GetCurrentMove(); //当前运动状态 + float current_speed_rate = move.GetCurrentSpeedRate(); //当前运动速度比率 + float current_turn_speed_rate = move.GetCurrentTurnSpeedRate(); //当前转向时一侧减速的比率 + + //停止运动 + move.Stop(); + + //等待低电平结束 while (digitalRead(DEBUG_PAUSE_PIN) == LOW) {} + + //恢复之前的运动 + move.MoveDirection(current_direction, current_speed_rate, current_turn_speed_rate); } //错误消息函数，用于在出现致命错误后结束程序 @@ -266,6 +282,11 @@ void loop() if (!is_claw_catch && is_claw_ok) //爪子上无环且状态正常 { + move.Stop(); //停止前进 + + servo.Down(); //放下爪子 + delay(DOWN_DELAY_TIME); + //沿线直行，在抓取位置离开函数 LineForward(CATCH_END); @@ -274,7 +295,7 @@ void loop() //抓取 if (!CatchAndCheck()) - is_carry = true; //抓取失败 + is_carry = true; //抓取失败，视为底盘携带 } else //未进行抓取 { @@ -293,32 +314,51 @@ void loop() //情况三：刚完整经过普通点，下一个点为释放点（使用机械臂） else if (passed_flag_type == NORMAL_POINT && next_flag_type == RELEASE_POINT) { - //沿线直行，在释放位置离开函数 + //沿线直行，在释放柱离开函数 LineForward(RELEASE_END); //停止前进 move.Stop(); + delay(RELEASE_STOP_DELAY_TIME); + + //无循迹后退到真实释放位置 + move.Backward(); + delay(RELEASE_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME); + + //停止后退 + move.Stop(); //释放 servo.Release(); delay(RELEASE_DELAY_TIME); //释放留时 is_claw_catch = false; - //下一点朝向为后 - next_position = BACK_NEXT; - } - //情况四：刚完整经过释放点（使用机械臂），下一个点为普通点 - else if (passed_flag_type == RELEASE_POINT && next_flag_type == NORMAL_POINT) - { - //沿线后退，到后端传感器接触下一条线离开函数 - LineBackward(BACK_END); - - //停止后退 - move.Stop(); - //机械臂复原 servo.Reset(); - delay(RESET_DELAY_TIME); //复原留时 + delay(RESET_DELAY_TIME); + + //无循迹前进到释放柱 + move.Forward(); + delay(RELEASE_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME); + + //下一点朝向为后 + next_position = BACK_NEXT; + + //停止前进 + move.Stop(); + delay(RELEASE_STOP_DELAY_TIME); + + //!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! + //route[4][2] = CATCH_POINT; + } + //情况四：刚完整经过释放点（使用机械臂）或增益抓取点，下一个点为普通点 + else if ((passed_flag_type == RELEASE_POINT || passed_flag_type == GAIN_POINT) && next_flag_type == NORMAL_POINT) + { + //底盘携带清空 + is_carry = false; + + //沿线后退，到后端传感器接触下一条线离开函数 + LineBackward(BACK_END); //继续后退或转向 TurnDirection(); @@ -341,6 +381,7 @@ void loop() //沿线后退，到前端传感器接触线离开函数 LineBackward(FRONT_END); + //底盘携带清空 is_carry = false; //沿线后退，到后端传感器接触线离开函数 @@ -349,6 +390,57 @@ void loop() //继续后退或转向 TurnDirection(); } + //情况七：刚完整经过普通点，下一个点为增益抓取点 + else if (passed_flag_type == NORMAL_POINT && next_flag_type == GAIN_POINT) + { + if (!is_claw_ok) //爪子状态不正常 + { + move.Stop(); //停止前进 + OpenClawAndCheck(); //再次检测爪子是否正常 + } + + if (is_claw_ok) //爪子状态正常 + { + //沿线直行，在释放柱（增益柱）离开函数 + LineForward(RELEASE_END); + + //停止前进 + move.Stop(); + delay(RELEASE_STOP_DELAY_TIME); + + //无循迹后退到真实抓取位置 + move.Backward(); + delay(GAIN_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME); + + //停止后退 + move.Stop(); + + //放下爪子 + servo.GainDown(); + delay(GAINDOWN_DELAY_TIME); + + //稍稍无循迹前进 + move.Forward(); + delay(GAIN_MOVE_LITTLEFORWARD_DELAY_TIME); + + //停止前进 + move.Stop(); + + //抓取 + CatchAndCheck(CATCH_TYPE_GAIN); + + //无循迹前进到释放柱（增益柱） + move.Forward(); + delay(GAIN_MOVE_BACKWARD_AND_FORWARD_DELAY_TIME); + } + + //下一点朝向为后 + next_position = BACK_NEXT; + + //停止前进 + move.Stop(); + delay(RELEASE_STOP_DELAY_TIME); + } //出现错误 else { @@ -526,6 +618,13 @@ uint8_t CalcDirection(void) } +//由灰度比计算修正减速比 +float CalcFixSpeedRate(float gray_deviation_rate) +{ + return -50.0 * pow((gray_deviation_rate - 1.0), 2.0) + 1.0; +} + + //沿线直行，在触发条件后离开函数但不停止 void LineForward(uint8_t end_position, float speed_rate) { @@ -538,16 +637,18 @@ void LineForward(uint8_t end_position, float speed_rate) //记录开始时间 unsigned long begin_time = millis(); + //记录灰度传感器匹配次数 + uint8_t gray_match_time = 0; while(1) { if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线 { - move.ForRightward(speed_rate, LINE_FIX_SPEED_RATE); + move.ForRightward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_3))); } else if (sensor.IsWhite(GRAY_3) && !sensor.IsWhite(GRAY_4)) //右侧越线 { - move.ForLeftward(speed_rate, LINE_FIX_SPEED_RATE); + move.ForLeftward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_4))); } else if (sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //在白线上 { @@ -555,18 +656,18 @@ void LineForward(uint8_t end_position, float speed_rate) } else //均不符合，则低速后退，尝试回到白线上 { - move.Backward(LINE_FIND_SPEED_RATE); + //move.Backward(LINE_FIND_SPEED_RATE); } if (end_position == FRONT_END) //前端接触线离开函数 { - if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2)) - break; + if (sensor.IsWhite(GRAY_1) || sensor.IsWhite(GRAY_2)) + gray_match_time++; } else if (end_position == BACK_END) //后端接触线离开函数 { - if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6)) - break; + if (sensor.IsWhite(GRAY_5) || sensor.IsWhite(GRAY_6)) + gray_match_time++; } else if (end_position == CATCH_END) //到达抓取位置离开函数 { @@ -575,9 +676,13 @@ void LineForward(uint8_t end_position, float speed_rate) } else //到达释放位置离开函数 { - if (millis() - begin_time > RELEASE_MOVE_DELAY_TIME) + if (millis() - begin_time > RELEASE_MOVE_FORWARD_DELAY_TIME) break; } + + //对应前端接触线离开函数和后端接触线离开函数的情况 + if (gray_match_time == 2) + break; } #ifdef TAICHI_DEBUG @@ -597,15 +702,18 @@ void LineBackward(uint8_t end_position, float speed_rate) Serial.println((int)end_position); #endif + //记录灰度传感器匹配次数 + uint8_t gray_match_time = 0; + while(1) { if (!sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //左侧越线 { - move.BackRightward(speed_rate, LINE_FIX_SPEED_RATE); + move.BackRightward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_3))); } else if (sensor.IsWhite(GRAY_3) && !sensor.IsWhite(GRAY_4)) //右侧越线 { - move.BackLeftward(speed_rate, LINE_FIX_SPEED_RATE); + move.BackLeftward(speed_rate, CalcFixSpeedRate(sensor.GrayDeviationRate(GRAY_4))); } else if (sensor.IsWhite(GRAY_3) && sensor.IsWhite(GRAY_4)) //在白线上 { @@ -613,19 +721,23 @@ void LineBackward(uint8_t end_position, float speed_rate) } else //均不符合，则低速前进，尝试回到白线上 { - move.Forward(LINE_FIND_SPEED_RATE); + //move.Forward(LINE_FIND_SPEED_RATE); } if (end_position == FRONT_END) //前端接触线离开函数 { - if (sensor.IsWhite(GRAY_1) && sensor.IsWhite(GRAY_2)) - break; + if (sensor.IsWhite(GRAY_1) || sensor.IsWhite(GRAY_2)) + gray_match_time++; } else //后端接触线离开函数 { - if (sensor.IsWhite(GRAY_5) && sensor.IsWhite(GRAY_6)) - break; + if (sensor.IsWhite(GRAY_5) || sensor.IsWhite(GRAY_6)) + gray_match_time++; } + + //对应前端接触线离开函数和后端接触线离开函数的情况 + if (gray_match_time == 2) + break; } #ifdef TAICHI_DEBUG @@ -638,15 +750,18 @@ 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)) + //若下下点为释放点或增益抓取点，判断是否需要跳过 + if ((route[next_next_flag][TYPE] == RELEASE_POINT && (!is_claw_catch || is_carry)) + || (route[next_next_flag][TYPE] == GETOUT_POINT && !is_carry) + || (route[next_next_flag][TYPE] == GAIN_POINT && is_claw_catch) + ) { UpdateFlag(JUMP_DEAD_ROAD); #ifdef TAICHI_DEBUG //调试输出直行或后退或转向状态 - Serial.println("#TAICHI: JUMP THE RELEASE/GETOUT POINT FOR NOTHING CARRIED"); - #endif + Serial.println("#TAICHI: JUMP THE RELEASE/GETOUT/GAIN POINT FOR STATUS REASON"); + #endif } uint8_t direction = CalcDirection(); @@ -670,10 +785,12 @@ void TurnDirection(float speed_rate) } else //继续转向 { + //等待小车旋转中心与十字中心重合 if (direction == FORLEFTWARD || direction == FORRIGHTWARD) delay(BEFORE_FORTURN_DELAY); else delay(BEFORE_BACKTURN_DELAY); + //旋转 move.MoveDirection(direction, speed_rate); //等待一定时间后检测是否摆正 @@ -689,21 +806,31 @@ void TurnDirection(float speed_rate) //抓取环，并判定是否抓取成功 -bool CatchAndCheck(float speed) +bool CatchAndCheck(uint8_t type, float speed) { //记录开始时间 unsigned long begin_time = millis(); + //抓取延时 + int catch_delay_time; + //抓取次数 uint8_t catch_times = 0; + + if (type == CATCH_TYPE_CATCH) + catch_delay_time = CATCH_DELAY_TIME; + else catch_delay_time = GAINCATCH_DELAY_TIME; //执行抓取动作 - servo.Catch(speed); + if (type == CATCH_TYPE_CATCH) + servo.Catch(speed); + else servo.GainCatch(speed); catch_times++; //等待完成动作 - while (millis() - begin_time < CATCH_DELAY_TIME) + while (millis() - begin_time < catch_delay_time) { + #ifndef DISABLE_CLAW_CHECK if (sensor.IsPushed(BUTTON_1)) //开关 1 闭合，即爪子两端接触，说明抓取失败 { #ifdef TAICHI_DEBUG @@ -712,6 +839,9 @@ bool CatchAndCheck(float speed) Serial.print(" catch_times: "); Serial.println((int)catch_times); #endif + if (type == CATCH_TYPE_GAIN) + delay(millis() - begin_time - catch_delay_time); //等待运行完成，防止卡住 + //停止动作组运行 servo.StopActionGroup(); @@ -729,15 +859,27 @@ bool CatchAndCheck(float speed) begin_time = millis(); //执行抓取动作 - servo.Catch(speed); + if (type == CATCH_TYPE_CATCH) + servo.Catch(speed); + else servo.GainCatch(speed); catch_times++; } + #endif } + #ifndef DISABLE_CLAW_CHECK #ifdef TAICHI_DEBUG //调试输出成功信息 Serial.println("#TAICHI: SUCCUESS CATCH!"); #endif + #endif + + #ifdef DISABLE_CLAW_CHECK + #ifdef TAICHI_DEBUG + //调试输出结束信息 + Serial.println("#TAICHI: CATCH WITHOUT CHECK!"); + #endif + #endif is_claw_catch = true; return true; @@ -747,6 +889,16 @@ bool CatchAndCheck(float speed) //打开爪子，并判断爪子是否能正常工作 bool OpenClawAndCheck(void) { + #ifdef DISABLE_CLAW_CHECK + #ifdef TAICHI_DEBUG + //调试输出信息 + Serial.println("#TAICHI: DISABLE CLAW CHECK!"); + #endif + + is_claw_ok = true; + return true; + #endif + //记录开始时间 unsigned long begin_time = millis();