a 'pecionata' of a technical challenge 1. 19 goals/min

the robot needs to face the defensive goal and it just recenters. Maybe one day we will remake the spinning kick to account any situations

include/motors_movement/drivecontroller.h

@@ -10,7 +10,7 @@
 
 //BEST NUMBERS YET
 //USE MOVING AVERAGE AND ANGLE WRAP
-#define KP 0.8
+#define KP 1.0
 #define KI 0.5
 #define KD 0.025
 
@@ -20,7 +20,7 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 70
+#define MAX_VEL 110
 #define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
 #define MAX_VEL_HALF ((int)MAX_VEL*0.5)
 #define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)

include/sensors/data_source_camera_conicmirror.h

@@ -28,7 +28,7 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 
 //Robot with roller
 #define CAMERA_TRANSLATION_X -8
-#define CAMERA_TRANSLATION_Y 4
+#define CAMERA_TRANSLATION_Y -3
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/striker_roller.h

@@ -13,9 +13,9 @@
 // There needs to be a little bit of space between the target point and the spot to be in
 #define STRL_SPINNER_OVERHEAD 7
 
-#define STRL_KICK_LIMIT_TILT1 200
+#define STRL_KICK_LIMIT_TILT1 180
-#define STRL_KICK_LIMIT_MAX 315
+#define STRL_KICK_LIMIT_MAX 300
-#define STRL_KICK_LIMIT_MIN 45
+#define STRL_KICK_LIMIT_MIN 60
 
 class StrikerRoller : public Game{
 
@@ -47,5 +47,7 @@ class StrikerRoller : public Game{
         unsigned long ball_catch_timer = 0;
         float ball_catch_tilt = 0;
         int limitx = 0;
+        unsigned long spinner_end_time = 0;
+        bool spinner_end_flag = false;
    
 };

src/main.cpp

@@ -53,13 +53,15 @@ void loop() {
   // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   // keeper_condition = role == LOW;
 
-  tc1->play(1);
+  if(robot_indentifier){
+    tc1->play(1);
+    // if(roller->roller_armed) roller->speed(roller->MAX);
+    // Last thing to do: movement and update status vector
+    drive->drivePrepared();  
+  }else{
+    drive->stopAll();
+  }
 
   testmenu->testMenu();
-
-  // if(roller->roller_armed) roller->speed(roller->MAX);
-
-  // Last thing to do: movement and update status vector
-  drive->drivePrepared();  
   updateStatusVector();
 }

src/motors_movement/drivecontroller.cpp

@@ -61,8 +61,8 @@ float DriveController::torad(float f){
 }
 
 void DriveController::drive(int dir, int speed, int tilt){
-
     speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT;
+    
     //tilt = tilt > 180 ? tilt - 360 : tilt;
 
     //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?

src/strategy_roles/striker_roller.cpp

@@ -26,7 +26,7 @@ void StrikerRoller::init()
 
   gotta_tilt = false;
   
-  ballAngleFilter = new ComplementaryFilter(0.85);
+  ballAngleFilter = new ComplementaryFilter(0);
 }
 
 void StrikerRoller::realPlay()
@@ -71,44 +71,82 @@ void StrikerRoller::spinner(int targetx){
 
     spinner_state=2;
     spinner_flag = false;
+    
+    // decide with direction to rotate
     spinner_tilt = CURRENT_DATA_READ.IMUAngle;
 
-    if(CURRENT_DATA_READ.posx < targetx) {
+    if(CURRENT_DATA_READ.angleAtk >= 0) {
-      tilt1 = -0.15;
+        tilt1 = -0.25;
-      tilt2 = 0.55;
+        tilt2 = 5;
+        limitx = 360-STRL_KICK_LIMIT_TILT1;
+      }else{
+        tilt1 = 0.25;
+        tilt2 = -5;
+        limitx = STRL_KICK_LIMIT_TILT1;
+      }
+    // if(CURRENT_DATA_READ.posx >= -5 && CURRENT_DATA_READ.posx <= 5){
+    //   if(CURRENT_DATA_READ.IMUAngle >= 180) {
+    //     tilt1 = -0.25;
+    //     tilt2 = 15;
+    //     limitx = 360-STRL_KICK_LIMIT_TILT1;
+    //   }else{
+    //     tilt1 = 0.25;
+    //     tilt2 = -15;
+    //     limitx = STRL_KICK_LIMIT_TILT1;
+    //   }
+    // }else if(CURRENT_DATA_READ.posx < -5){
+    //     tilt1 = 0.25;
+    //     tilt2 = -5;
+    //     limitx = STRL_KICK_LIMIT_TILT1;
+    // }else if(CURRENT_DATA_READ.posx > 5){
+    //     tilt1 = -0.25;
+    //     tilt2 = 5;
+    //     limitx = 360-STRL_KICK_LIMIT_TILT1;
+    // }
+    // if(CURRENT_DATA_READ.IMUAngle < limitx) tilt1 *= -1; //we need to invert the rotatiion if we are on the opposite side
 
-      limitx = 360-STRL_KICK_LIMIT_TILT1;
-    }else{
-      tilt1 = 0.15;
-      tilt2 = -0.55;
-
-      limitx = STRL_KICK_LIMIT_TILT1;
-    }
   }else if(spinner_state == 2){
     roller->speed(roller->MAX);
 
-    spinner_tilt += tilt1;
+    spinner_tilt = spinner_tilt + tilt1;
+    // This ensures no strange rotation happens
+    if(spinner_tilt <= -360) spinner_tilt+=360;
+    if(spinner_tilt >= 360) spinner_tilt -= 360;
+
     drive->prepareDrive(0,0,spinner_tilt);
     
     if(CURRENT_DATA_READ.IMUAngle > limitx-5 && CURRENT_DATA_READ.IMUAngle < limitx+5) {
-      spinner_timer = millis();
       spinner_state=3;
+      spinner_tilt = CURRENT_DATA_READ.IMUAngle;
     }
   }else if(spinner_state == 3){
     roller->speed(roller->MAX);
+    // stay there a little bit
     drive->prepareDrive(0,0,spinner_tilt);
+
     if(millis() - spinner_timer > 150) {
       spinner_state=4;
       spinner_tilt = CURRENT_DATA_READ.IMUAngle;
     }
   }else if(spinner_state == 4){
-    drive->prepareDrive(0,0,0);
+    // fast return which gives momentum to the ball
+    drive->prepareDrive(0,0,spinner_tilt);
 
-    // spinner_tilt += tilt2;
+    spinner_tilt += tilt2;
-    // spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
+    spinner_tilt = constrain(spinner_tilt, 5, 355);
-    // drive->prepareDrive(0,0,spinner_tilt);
 
-    if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) roller->speed(roller->MIN);
+    // turn roller off and kick
+    if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) {
+      roller->speed(roller->MIN);
+      if(!spinner_end_flag){
+        spinner_end_time = millis();
+        spinner_end_flag = true;
+      }
+    }
+    // if(ballPresence->isInMouth()) {
+    //   spinner_state = 1;
+    //   spinner_flag = false; 
+    // }
   }
 }
 /*
@@ -123,12 +161,18 @@ void StrikerRoller::catchBall(){
   if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
   else roller->speed(roller->MIN);
 
+  // if(!ball->isInFront() && millis() - spinner_end_time > 500 && spinner_end_flag){
+  //   ball_catch_state = 0;
+  //   ball_catch_flag = false;
+  //   spinner_end_flag = false;
+  // }
   if(!ball->isInFront()){
     ball_catch_state = 0;
     ball_catch_flag = false;
   }
 
   if(ball_catch_state == 0){
+    // tilt torwards the ball until it's in the mouth
     drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
 
     if(ballPresence->isInMouth() && !ball_catch_flag){
@@ -140,20 +184,45 @@ void StrikerRoller::catchBall(){
       ball_catch_state++;
     }
   }else if(ball_catch_state == 1){
+    // go towards the goal without resetting tilt
+    // this makes it so that the robot sometimes walks backwards, hiding the ball from any opponent
+    // a spinning kick is the only way to free the ball here
     int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
-    // val = val > 10 || val < 10 ? val : 0;
 
     drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
 
-    if(CURRENT_DATA_READ.distAtk <= 25){
+    if(CURRENT_DATA_READ.distAtk <= 26){
-       ball_catch_state++;
+      //  ball_catch_state++;
-       spinner_state = 0;
+      //  spinner_state = 0;
-       spinner_flag = false;
+      //  spinner_flag = false;
-       spinner_tilt = 0;
+      //  spinner_tilt = CURRENT_DATA_READ.IMUAngle;
-      //  drive->prepareDrive(0,0,0);
+
+      drive->prepareDrive(0,0,0);
+      if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) roller->speed(roller->MIN);
     }
   }else if(ball_catch_state == 2){
-    spinner(0);
+    // Spinny kick
+    
+    // spinner(0);
+    if(CURRENT_DATA_READ.angleAtkFix >= 0) {
+      tilt2 = 3;
+    }else{
+      tilt2 = -3;
+    }
+
+    drive->prepareDrive(0,0,spinner_tilt);
+
+    spinner_tilt += tilt2;
+    spinner_tilt = constrain(spinner_tilt, 5, 355);
+
+    // turn roller off and kick
+    if(CURRENT_DATA_READ.IMUAngle >= STRL_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= STRL_KICK_LIMIT_MAX) {
+      roller->speed(roller->MIN);
+      if(!spinner_end_flag){
+        spinner_end_time = millis();
+        spinner_end_flag = true;
+      }
+    }
   }
 }
 

src/test_menu.cpp

@@ -114,6 +114,7 @@ void TestMenu::testMenu()
             ballPresence->test();
             break;
         case 'r':
+            drive->stopAll();
             DEBUG.println("Roller at default speed");
             roller->speed(roller->MAX);
             break;