robocup: first technical challenge

still could be better, faster and more precise in some parts

include/motors_movement/drivecontroller.h

@@ -10,8 +10,8 @@
 
 //BEST NUMBERS YET
 //USE MOVING AVERAGE AND ANGLE WRAP
-#define KP 1.2
+#define KP 0.8
-#define KI 0.0
+#define KI 0.5
 #define KD 0.025
 
 #define KSPD 0.3

include/strategy_roles/games.h

@@ -21,3 +21,5 @@ g_extr Game* striker_roller;
 g_extr Game* precision_shooter;
 g_extr Game* pass_and_shoot;
 // g_extr Game* keeper;
+
+g_extr Game* tc1;

src/main.cpp

@@ -53,7 +53,7 @@ void loop() {
   // striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   // keeper_condition = role == LOW;
 
-  striker_roller->play(1);
+  tc1->play(1);
 
   testmenu->testMenu();
 

src/strategy_roles/games.cpp

@@ -16,5 +16,6 @@ void initGames(){
     pass_and_shoot = new PassAndShoot(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
+    tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/striker_roller.cpp

@@ -62,18 +62,16 @@ void StrikerRoller::spinner(int targetx){
       spinner_timer = millis();
     }
 
-    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 50) {
       spinner_state=1;
       spinner_flag = false;
     }
   }else if(spinner_state == 1){
     roller->speed(roller->MAX);
 
-    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
     spinner_state=2;
     spinner_flag = false;
     spinner_tilt = CURRENT_DATA_READ.IMUAngle;
-    }
 
     if(CURRENT_DATA_READ.posx < targetx) {
       tilt1 = -0.15;
@@ -86,7 +84,6 @@ void StrikerRoller::spinner(int targetx){
 
       limitx = STRL_KICK_LIMIT_TILT1;
     }
-    spinner_state=1;
   }else if(spinner_state == 2){
     roller->speed(roller->MAX);
 
@@ -139,33 +136,25 @@ void StrikerRoller::catchBall(){
       ball_catch_timer = millis();
     }
 
-    if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
+    if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 50) {
       ball_catch_state++;
-      ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
     }
   }else if(ball_catch_state == 1){
     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 <= 30){
+    if(CURRENT_DATA_READ.distAtk <= 25){
        ball_catch_state++;
        spinner_state = 0;
        spinner_flag = false;
        spinner_tilt = 0;
-       drive->prepareDrive(0,0,0);
+      //  drive->prepareDrive(0,0,0);
     }
   }else if(ball_catch_state == 2){
     spinner(0);
   }
-
-    // if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
-    // else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
-
-    // drive->prepareDrive(0,0,ball_catch_tilt);
-
-    // if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
-    // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
-    // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
 }
 
 int StrikerRoller::tilt() {
@@ -173,9 +162,6 @@ int StrikerRoller::tilt() {
   else gotta_tilt = false;
 
   if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {
-    atk_tilt *= 0.8;
-    if(atk_tilt <= 10) atk_tilt = 0;
-  }else{
     atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
   }