From 788684ef0a2b2588296e6f0f34cacbb2696ec36c Mon Sep 17 00:00:00 2001
From: EmaMaker <emamaker0@gmail.com>
Date: Sun, 27 Jun 2021 13:51:58 +0200
Subject: [PATCH] end of rcj 2021 Co-authored-by: u-siri-ous 
 <sanninosiria@gmail.com>

---
 src/strategy_roles/corner_kick_1.cpp | 15 ++++++-----
 src/strategy_roles/corner_kick_2.cpp | 37 +++++++++++++++++-----------
 2 files changed, 29 insertions(+), 23 deletions(-)

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);