end of rcj 2021

Co-authored-by: u-siri-ous  <sanninosiria@gmail.com>
pull/1/head
EmaMaker 2021-06-27 13:51:58 +02:00
parent 39a3a26249
commit 788684ef0a
2 changed files with 29 additions and 23 deletions

View File

@ -33,16 +33,15 @@ void CornerKick::kick(){
kick_flag = false; kick_flag = false;
} else if(kick_state == 1){ } else if(kick_state == 1){
drive->prepareDrive(0, 0, CURRENT_DATA_READ.ballAngleFix); 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){ } else if(kick_state==2){
drive->prepareDrive(0, 60, CURRENT_DATA_READ.ballAngleFix); drive->prepareDrive(0, 100, 350);
if(ball->isInMouth()){ if(ball->isInMouth()){
kick_state++;
if(!kick_flag) { if(!kick_flag) {
kick_flag = true; kick_flag = true;
kicktimer = millis(); kicktimer = millis();
}else{ }else{
if(kick_flag && millis() - kicktimer > 700){ if(kick_flag && millis() - kicktimer > 425 ){
kick_flag = false; kick_flag = false;
kick_state++; kick_state++;
kicktimer = millis(); kicktimer = millis();
@ -50,15 +49,15 @@ void CornerKick::kick(){
} }
} }
} else if(kick_state==3){ } else if(kick_state==3){
drive->prepareDrive(0, 150, CURRENT_DATA_READ.ballAngleFix); drive->prepareDrive(270, 60,0);
if(millis()-kicktimer > 400){ if(millis()-kicktimer > 300){
kick_state++; kick_state++;
} }
} else if(kick_state==4){ } else if(kick_state==4){
if(((PositionSysCamera*)ps)->isAtDistanceFrom(0, -28, 5)) { if(((PositionSysCamera*)ps)->isAtDistanceFrom(1, -28, 2)) {
kick_state++; kick_state++;
kicktimer = millis(); kicktimer = millis();
}else (((PositionSysCamera*)ps)->setMoveSetpoints(0, -28)); }else (((PositionSysCamera*)ps)->setMoveSetpoints(1, -28));
} else if(kick_state == 5){ } else if(kick_state == 5){
drive->prepareDrive(0,0,0); drive->prepareDrive(0,0,0);
bt->can_send = true; bt->can_send = true;

View File

@ -17,22 +17,29 @@ void CornerKick2::realPlay() {
tone(BUZZER, 320, 250); tone(BUZZER, 320, 250);
timer = millis(); timer = millis();
state=1; 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){ if(state == 1){
drive->prepareDrive(265, 50, 0); drive->prepareDrive(265, 50, 0);
if(CURRENT_DATA_READ.bSeen && millis() - timer > 500) { if(CURRENT_DATA_READ.bSeen && millis() - timer > 1000) {
state ++; state ++;
timer = millis(); timer = millis();
} }
}else if(state == 2){ }else if(state == 2){
drive->prepareDrive(315, 50, 0); drive->prepareDrive(315, 50, 0);
if(millis() - timer > 200) state++; if(millis() - timer > 400) state++;
}else if(state == 3){ }else if(state == 3){
catchBall(); catchBall();
}else if(state == 4){ }else if(state == 4){
spinner(0); spinner(-5);
}else if(state == 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; ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
} }
}else if(ball_catch_state == 1){ }else if(ball_catch_state == 1){
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.01; else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.0075;
drive->prepareDrive(0,0,ball_catch_tilt); drive->prepareDrive(0,0,ball_catch_tilt);
@ -90,7 +97,7 @@ void CornerKick2::spinner(int targetx){
// if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD; // if(targetx >= 0) spotx = targetx-CK2_SPINNER_OVERHEAD;
// else 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){ if( !spinner_flag){
spinner_flag = true; spinner_flag = true;
@ -103,19 +110,19 @@ void CornerKick2::spinner(int targetx){
spinner_tilt = CURRENT_DATA_READ.IMUAngle; spinner_tilt = CURRENT_DATA_READ.IMUAngle;
} }
if(targetx >= 0) { // if(targetx >= 0) {
tilt1 = -0.01; tilt1 = -0.0075;
tilt2 = 0.55; tilt2 = 0.55;
limitx = 360-CK2_KICK_LIMIT_TILT1; limitx = 360-CK2_KICK_LIMIT_TILT1;
}else{ // }else{
tilt1 = 0.01; // tilt1 = 0.01;
tilt2 = -0.55; // 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){ }else if(spinner_state == 2){
roller->speed(roller->MAX); roller->speed(roller->MAX);