precision shooter: catch and spinning kick

include/strategy_roles/precision_shooter.h

@@ -1,6 +1,7 @@
 #pragma once
 
 #include "sensors/data_source_camera_vshapedmirror.h"
+#include "behaviour_control/complementary_filter.h"
 #include "sensors/sensors.h"
 #include "strategy_roles/game.h"
 
@@ -9,6 +10,9 @@
 #define PS_PLUSANG 55
 #define PS_PLUSANG_VISIONCONE 10
 
+// There needs to be a little bit of space between the target point and the spot to be in
+#define PS_SPINNER_OVERHEAD 10
+
 class PrecisionShooter : public Game{
 
     public:
@@ -18,13 +22,13 @@ class PrecisionShooter : public Game{
     private:
         void realPlay() override;
         void init() override;
-        void striker();
+        void catchBall();
+        void spinner();
         int tilt();
-
+        
+    private:
         int atk_speed, atk_direction, atk_tilt;
         float cstorc;
-
         bool gotta_tilt;
-
+        ComplementaryFilter* ballAngleFilter;        
-        unsigned long pas_counter;
 };

include/systems/position/positionsys_camera.h

@@ -22,6 +22,8 @@
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
 #define DIST_MULT 1.7
 
+#define VICINITY_DIST_TRESH 3
+
 #define Kpx 1
 #define Kix 0
 #define Kdx 0
@@ -42,6 +44,7 @@ class PositionSysCamera : public PositionSystem{
         void setCameraPID();
         void CameraPID();
         int calcOtherGoalY(int goalY);
+        bool isInTheVicinityOf(int, int);
 
         double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
         int MAX_DIST, vx, vy, axisx, axisy;

src/strategy_roles/precision_shooter.cpp

@@ -25,62 +25,129 @@ void PrecisionShooter::init()
   cstorc = 0;
 
   gotta_tilt = false;
+  
+  ballAngleFilter = new ComplementaryFilter(0.85);
 }
 
 void PrecisionShooter::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
-    this->striker();
+    this->catchBall();
   else
     ps->goCenter();
 }
 
-unsigned long t3 = 0;
+/*
-unsigned long t4 = 0;
+Spinning kick state machine
-boolean ignited = false;
+0: wait for the ball to be in mouth, calculate movement direction
+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::striker(){
+void PrecisionShooter::spinner(int targetx){
+  if(!ballPresence->isInMouth()) {
+    spinner_state=0;
+    spinner_flag = false;
+  }
 
-  #ifdef MAX_VEL
+  if(spinner_state == 0){
-  #undef MAX_VEL
+    if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
-  #define MAX_VEL 100
+    if(ballPresence->isInMouth() && !spinner_flag){
-  #endif
+      spinner_flag = true;
-
+      spinner_timer = millis();
-  //seguo palla
-  int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang =  PS_PLUSANG;
-  
-  if(ball_deg >= 346 || ball_deg <= 16) plusang = PS_PLUSANG_VISIONCONE;            //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
-  if(ball_deg > 180) ball_degrees2 = ball_deg - 360;            //ragiono in +180 -180  
-  else ball_degrees2 = ball_deg;
-
-  if(ball_degrees2 > 0) dir = ball_deg + plusang;               //se sto nel quadrante positivo aggiungo
-  else dir = ball_deg - plusang;                                //se sto nel negativo sottraggo
-
-  dir = (dir + 360) % 360;
-  drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
-
-  // if(ball->isInFront()) roller->speed(ROLLER_DEFAULT_SPEED);
-  // else roller->speed(roller->MIN);
-
-  if(ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 78 && ( (CURRENT_DATA_READ.posy >= 32 &&  (CURRENT_DATA_READ.posx >= 15  || CURRENT_DATA_READ.posx <= -15)) ||  abs(tilt()) > 65 ) ) {
-    // Just let the robot slowly approach the ball
-    if(!ignited){
-      ignited = true;
-      t4 = millis();
     }
-    if(millis() - t4 > 250  && ignited){
+
-      t3 = millis();
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 250) {
+      spinner_state++;
+      spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+    }
+  }else if(spinner_state == 1){
+    int spotx;
+
+    if(targetx > 0) spotx = targetx-PS_SPINNER_OVERHEAD;
+    else spotx = targetx+PS_SPINNER_OVERHEAD;
+
+    ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
+    if(((PositionSysCamera*)ps)->isInTheVicinityOf(spotx, 0)){
+      ball_catch_state++;
+
+      if(CURRENT_DATA_READ.posx > targetx){
+        tilt1 = -0.1;
+        tilt2 = 0.35;
+      }else{
+        tilt1 = 0.1;
+        tilt2 = -0.35;
+      }
+    }
+  }else if(spinner_state == 2){
+    spinner_tilt += tilt1;
+    drive(0,0,spinner_tilt);
+    
+    if(CURRENT_DATA_READ.IMUAngle > 175 && CURRENT_DATA_READ.IMUAngle < 185) {
+      spinner_state++;
+      spinner_tilt = CURRENT_DATA_READ.IMUAngle;
+    }
+    
+  }else if(spinner_state == 3){
+    spinner_tilt += tilt2;
+    drive(0,0,spinner_tilt);
+
+    if(CURRENT_DATA_READ.IMUAngle > 225 || CURRENT_DATA_READ.IMUAngle < 135){
+      roller->speed(roller->MIN);
+      spinner_state++;
     }
   }
 
-  if(millis() - t3 < 800){
+}
-    // roller->speed(roller->MAX);
+
-    drive->prepareDrive(180, MAX_VEL_3QUARTERS, 0);
+/*
-    ignited = false;
+Ball catch state machine
+0: go towards the ball, until it's been in robot's mouth for 250ms
+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(){
+
+  if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
+  else roller->speed(roller->MIN);
+
+  if(!ball->isInFront()){
+    ball_catch_state = 0;
+    ball_catch_flag = false;
   }
 
+  if(ball_catch_state == 0){
+    drive->prepareDrive(0, 30, 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) {
+      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;
+    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;
+  }else if(ball_catch_state == 2){
+    ((PositionSysCamera*)ps)->setMoveSetpoints(CURRENT_DATA_READ.xAtkFix, 0);
+    if(((PositionSysCamera*)ps)->isInTheVicinityOf(CURRENT_DATA_READ.xAtkFix, 0)) ball_catch_state++;
+  }
 }
 
 int PrecisionShooter::tilt() {

src/strategy_roles/striker.cpp

@@ -40,7 +40,7 @@ float ctilt = 0;
 unsigned long ttilt = 0;
 
 
-void Striker::striker(){
+/*void Striker::striker(){
   //seguo palla
   if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
   else roller->speed(roller->MIN);
@@ -58,21 +58,36 @@ void Striker::striker(){
 
   // old_ball_Angle = ball_angle;
   // old_ball_tilt = (int) ball_tilt;
-}
+}*/
 
-float Striker::ballTilt(){
+void Striker::striker(){
-  if(CURRENT_DATA_READ.ballAngle > 180 && CURRENT_DATA_READ.ballAngle < 340) ball_tilt -= 0.15f;
+  //seguo palla
-  else if(CURRENT_DATA_READ.ballAngle <= 180 && CURRENT_DATA_READ.ballAngle > 20) ball_tilt += 0.15f;
+  int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
-  return ball_tilt;
+  
-}
+  if(ball_deg >= 344 || ball_deg <= 16) plusang = STRIKER_PLUSANG_VISIONCONE;            //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
+  if(ball_deg > 180) ball_degrees2 = ball_deg - 360;            //ragiono in +180 -180  
+  else ball_degrees2 = ball_deg;
 
+  if(ball_degrees2 > 0) dir = ball_deg + plusang;               //se sto nel quadrante positivo aggiungo
+  else dir = ball_deg - plusang;                                //se sto nel negativo sottraggo
+
+  dir = (dir + 360) % 360;
+  drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
+
+  if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
+  else roller->speed(roller->MIN);
+  
+}
 
 int Striker::tilt() {
-  if(CURRENT_DATA_READ.atkSeen && (CURRENT_DATA_READ.ballAngleFix > 340 || CURRENT_DATA_READ.ballAngleFix < 20)){
+  if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
-    atk_tilt = roller->roller_armed ? CURRENT_DATA_READ.angleAtkFix : constrain(CURRENT_DATA_READ.angleAtkFix, -45, 45);
+  else gotta_tilt = false;
-  }else{
+
+  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);
   }
 
   return atk_tilt;

src/system/positions/positionsys_camera.cpp

@@ -4,6 +4,7 @@
 #include "vars.h"
 #include "math.h"
 
+
 PositionSysCamera::PositionSysCamera() {
     MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
     Inputx = 0;
@@ -122,6 +123,11 @@ int PositionSysCamera::calcOtherGoalY(int goalY){
     return otherGoalY;
 }
 
+bool PositionSysCamera::isInTheVicinityOf(int x_, int y_){
+    // Distance using pytagorean theorem
+    return pow(CURRENT_DATA_READ.posx-x_, 2) + pow(CURRENT_DATA_READ.posy-y_, 2) <= VICINITY_DIST_TRESH*VICINITY_DIST_TRESH;
+}
+
 void PositionSysCamera::CameraPID(){   
     if(givenMovement){
 
@@ -139,12 +145,11 @@ void PositionSysCamera::CameraPID(){
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
-        int dist = sqrt( ( (CURRENT_DATA_WRITE.posx-Setpointx)*(CURRENT_DATA_WRITE.posx-Setpointx) ) + (CURRENT_DATA_WRITE.posy-Setpointy)*(CURRENT_DATA_WRITE.posy-Setpointy) );
+        int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
-        // int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
+        int speed = dist*DIST_MULT;
-        int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0,  MAX_VEL);
+
-        speed = speed > 30 ? speed : 0;
+        speed = speed > 10 ? speed : 0;
-        dir = filterDir->calculate(dir);;
+        dir = filterDir->calculate(dir);
-        //speed = filterSpeed->calculate(speed);
 
         #ifdef DRIVE_VECTOR_SUM
         vx = ((speed * cosins[dir]));