precision shooter: catches ball and goes towards goal

depending on the angle it catches the ball at, sometimes it hides the ball, sometimes it does a random spinning kick when it founds the line

include/motors_movement/drivecontroller.h

@@ -10,7 +10,7 @@
 
 //BEST NUMBERS YET
 //USE MOVING AVERAGE AND ANGLE WRAP
-#define KP 1.35
+#define KP 1.2
 #define KI 0.0
 #define KD 0.025
 
@@ -20,7 +20,7 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 50
+#define MAX_VEL 60
 #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)
@@ -37,6 +37,7 @@ class DriveController{
         void drive(int dir=0, int speed=0, int tilt=0);
         void prepareDrive(int dir, int speed, int tilt=0);
         void drivePrepared();
+        int directionAccountingForTilt(int, int);
         float updatePid();
         float torad(float f);
         void resetDrive();

include/sensors/data_source_camera_conicmirror.h

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

src/motors_movement/drivecontroller.cpp

@@ -167,4 +167,8 @@ void DriveController::stopAll(){
     m2->stop();
     m3->stop();
     m4->stop();
+}
+
+int DriveController::directionAccountingForTilt(int wanted_direction, int tilt){
+  return ((360-tilt+wanted_direction)+360)%360;
 }

src/strategy_roles/precision_shooter.cpp

@@ -29,12 +29,28 @@ void PrecisionShooter::init()
   ballAngleFilter = new ComplementaryFilter(0.85);
 }
 
+float tilt1 = 0;
+float tilt2 = 0;
+int spinner_state = 0;
+bool spinner_flag = false;
+unsigned long spinner_timer = 0;
+float spinner_tilt = 0;
+int ball_catch_state = 0;
+bool ball_catch_flag = false;
+unsigned long ball_catch_timer = 0;
+float ball_catch_tilt = 0;
+
 void PrecisionShooter::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
-    this->spinner(5);
+    this->catchBall();
-  else
+  else{
     ps->goCenter();
+    ball_catch_flag = false;
+    spinner_flag = false;
+    ball_catch_state = 0;
+    spinner_state = 0;
+  }
 }
 
 /*
@@ -43,13 +59,6 @@ Spinning kick state machine
 1: tilt toward 180 deg
 2: tilt back to 0 in the needed direction, stopping the roller whenn needed
 */
-float tilt1 = 0;
-float tilt2 = 0;
-int spinner_state = 0;
-bool spinner_flag = false;
-unsigned long spinner_timer = 0;
-float spinner_tilt = 0;
-
 void PrecisionShooter::spinner(int targetx){
   // if(!ballPresence->isInMouth()) {
   //   spinner_state=0;
@@ -128,10 +137,6 @@ Ball catch state machine
 1: slowly return facing to north (slowly otherwise we might lose the ball)
 2: reach the spot
 */
-int ball_catch_state = 0;
-bool ball_catch_flag = false;
-unsigned long ball_catch_timer = 0;
-float ball_catch_tilt = 0;
 
 void PrecisionShooter::catchBall(){
 
@@ -144,27 +149,29 @@ void PrecisionShooter::catchBall(){
   }
 
   if(ball_catch_state == 0){
-    drive->prepareDrive(0, 30, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
+    drive->prepareDrive(0, MAX_VEL_HALF, ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix));
 
     if(ballPresence->isInMouth() && !ball_catch_flag){
       ball_catch_flag = true;
       ball_catch_timer = millis();
     }
 
-    if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 250) {
+    if(ballPresence->isInMouth() && ball_catch_flag && millis() - ball_catch_timer > 350) {
       ball_catch_state++;
       ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
     }
   }else if(ball_catch_state == 1){
-    if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
+    int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
-    else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
+    drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
 
-    drive->prepareDrive(0,0,ball_catch_tilt);
+    // if(ball_catch_tilt > 180) ball_catch_tilt += 0.15;
+    // else if(ball_catch_tilt <= 180) ball_catch_tilt -= 0.15;
 
-    if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
+    // drive->prepareDrive(0,0,ball_catch_tilt);
-  }else if(ball_catch_state == 2){
+
-    ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
+    // if(CURRENT_DATA_READ.IMUAngle >= 355 || CURRENT_DATA_READ.IMUAngle <= 5) ball_catch_state = 0;
-    if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
+    // ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
+    // if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
   }
 }
 

utility/OpenMV/conic_eff_h7.py

@@ -46,10 +46,10 @@ blue_led.on()
 
 
 thresholds = [  (70, 100, -36, 8, 38, 112),    # thresholds yellow goalz
-                (31, 74, -31, 15, -50, -9)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (37, 57, -19, 8, -52, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (50, 0, 250, 200)
+roi = (50, 0, 270, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx