diff --git a/src/strategy_roles/corner_kick_1.cpp b/src/strategy_roles/corner_kick_1.cpp index 1893029..04bb887 100644 --- a/src/strategy_roles/corner_kick_1.cpp +++ b/src/strategy_roles/corner_kick_1.cpp @@ -33,16 +33,15 @@ void CornerKick::kick(){ kick_flag = false; } else if(kick_state == 1){ drive->prepareDrive(0, 0, CURRENT_DATA_READ.ballAngleFix); - if((CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) && millis() - kicktimer > 250) kick_state ++; + if((CURRENT_DATA_READ.ballAngle >= 350 || CURRENT_DATA_READ.ballAngle <= 10) && millis() - kicktimer > 1000) kick_state ++; } else if(kick_state==2){ - drive->prepareDrive(0, 60, CURRENT_DATA_READ.ballAngleFix); + drive->prepareDrive(0, 100, 350); if(ball->isInMouth()){ - kick_state++; if(!kick_flag) { kick_flag = true; kicktimer = millis(); }else{ - if(kick_flag && millis() - kicktimer > 700){ + if(kick_flag && millis() - kicktimer > 425 ){ kick_flag = false; kick_state++; kicktimer = millis(); @@ -50,15 +49,15 @@ void CornerKick::kick(){ } } } else if(kick_state==3){ - drive->prepareDrive(0, 150, CURRENT_DATA_READ.ballAngleFix); - if(millis()-kicktimer > 400){ + drive->prepareDrive(270, 60,0); + if(millis()-kicktimer > 300){ kick_state++; } } else if(kick_state==4){ - if(((PositionSysCamera*)ps)->isAtDistanceFrom(0, -28, 5)) { + if(((PositionSysCamera*)ps)->isAtDistanceFrom(1, -28, 2)) { kick_state++; kicktimer = millis(); - }else (((PositionSysCamera*)ps)->setMoveSetpoints(0, -28)); + }else (((PositionSysCamera*)ps)->setMoveSetpoints(1, -28)); } else if(kick_state == 5){ drive->prepareDrive(0,0,0); bt->can_send = true; diff --git a/src/strategy_roles/corner_kick_2.cpp b/src/strategy_roles/corner_kick_2.cpp index 8558f5c..8a5cc20 100644 --- a/src/strategy_roles/corner_kick_2.cpp +++ b/src/strategy_roles/corner_kick_2.cpp @@ -17,22 +17,29 @@ void CornerKick2::realPlay() { tone(BUZZER, 320, 250); timer = millis(); state=1; + + ball_catch_state = 0; + ball_catch_tilt = 0; + spinner_state = 0; + spinner_tilt = 0; + ball_catch_flag = false; + spinner_flag = false; } if(state == 1){ drive->prepareDrive(265, 50, 0); - if(CURRENT_DATA_READ.bSeen && millis() - timer > 500) { + if(CURRENT_DATA_READ.bSeen && millis() - timer > 1000) { state ++; timer = millis(); } }else if(state == 2){ drive->prepareDrive(315, 50, 0); - if(millis() - timer > 200) state++; + if(millis() - timer > 400) state++; }else if(state == 3){ catchBall(); }else if(state == 4){ - spinner(0); + spinner(-5); }else if(state == 5){ - drive->stopAll(); + drive->prepareDrive(0,0,0); } } @@ -59,8 +66,8 @@ void CornerKick2::catchBall(){ ball_catch_tilt = CURRENT_DATA_READ.IMUAngle; } }else if(ball_catch_state == 1){ - if(ball_catch_tilt > 180) ball_catch_tilt += 0.01; - else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.01; + if(ball_catch_tilt > 180) ball_catch_tilt += 0.0075; + else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.0075; drive->prepareDrive(0,0,ball_catch_tilt); @@ -90,7 +97,7 @@ void CornerKick2::spinner(int targetx){ // if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD; // else spotx = targetx+CK2_SPINNER_OVERHEAD; - if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)) { + if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, -1)) { if( !spinner_flag){ spinner_flag = true; @@ -103,19 +110,19 @@ void CornerKick2::spinner(int targetx){ spinner_tilt = CURRENT_DATA_READ.IMUAngle; } - if(targetx >= 0) { - tilt1 = -0.01; + // if(targetx >= 0) { + tilt1 = -0.0075; tilt2 = 0.55; limitx = 360-CK2_KICK_LIMIT_TILT1; - }else{ - tilt1 = 0.01; - tilt2 = -0.55; + // }else{ + // tilt1 = 0.01; + // tilt2 = -0.55; - limitx = CK2_KICK_LIMIT_TILT1; - } + // limitx = CK2_KICK_LIMIT_TILT1; + // } - }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0); + }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, -1); }else if(spinner_state == 2){ roller->speed(roller->MAX);