striker-roller: complete striker using the roller

include/behaviour_control/status_vector.h

@@ -39,7 +39,7 @@ typedef struct data{
     int IMUAngle, ballAngle, ballAngleFix, ballDistance, ballPresenceVal, 
         yAngle, bAngle, yAngleFix, bAngleFix, 
         yDist, bDist, 
-        angleAtk, angleAtkFix, angleDef, angleDefFix, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
+        angleAtk, angleAtkFix, angleDef, angleDefFix, distAtk, distDef, yAtk, yAtkFix, xAtk, xAtkFix, yDef, yDefFix, xDef, xDefFix,
         cam_xb, cam_yb, cam_xy, cam_yy,
         speed, tilt, dir, axisBlock[4],
         USfr, USsx, USdx, USrr, 

include/strategy_roles/games.h

@@ -9,6 +9,7 @@
 #include <Arduino.h>
 #include "strategy_roles/game.h"
 #include "strategy_roles/striker.h"
+#include "strategy_roles/striker_roller.h"
 #include "strategy_roles/precision_shooter.h"
 #include "strategy_roles/pass_and_shoot.h"
 // #include "strategy_roles/keeper.h"
@@ -16,6 +17,7 @@
 void initGames();
 
 g_extr Game* striker;
+g_extr Game* striker_roller;
 g_extr Game* precision_shooter;
 g_extr Game* pass_and_shoot;
 // g_extr Game* keeper;

include/strategy_roles/precision_shooter.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 PS_SPINNER_OVERHEAD 7
 
-#define KICK_LIMIT_TILT1 200
+#define PS_KICK_LIMIT_TILT1 200
-#define KICK_LIMIT_MAX 315
+#define PS_KICK_LIMIT_MAX 315
-#define KICK_LIMIT_MIN 45
+#define PS_KICK_LIMIT_MIN 45
 
 class PrecisionShooter : public Game{
 
@@ -34,5 +34,17 @@ class PrecisionShooter : public Game{
         int atk_speed, atk_direction, atk_tilt;
         float cstorc;
         bool gotta_tilt;
-        ComplementaryFilter* ballAngleFilter;        
+        ComplementaryFilter* ballAngleFilter;
+
+        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;
+        int limitx = 0;
 };

include/strategy_roles/striker_roller.h

@@ -0,0 +1,51 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "behaviour_control/complementary_filter.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define STRL_ATTACK_DISTANCE 110
+#define STRL_TILT_STOP_DISTANCE 140
+#define STRL_PLUSANG 55
+#define STRL_PLUSANG_VISIONCONE 10
+
+// 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_MAX 315
+#define STRL_KICK_LIMIT_MIN 45
+
+class StrikerRoller : public Game{
+
+    public:
+        StrikerRoller();
+        StrikerRoller(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+        void catchBall();
+        void spinner(int);
+        int tilt();
+        
+    private:
+        int atk_speed, atk_direction, atk_tilt;
+        float cstorc;
+        bool gotta_tilt;
+        ComplementaryFilter* ballAngleFilter;
+        
+        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;
+        int limitx = 0;
+   
+};

src/main.cpp

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

src/sensors/data_source_camera_conicmirror.cpp

@@ -160,6 +160,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.yAngle;
     CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.yAngleFix;
     CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.ySeen;
+    CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.yDist;
     
     CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yy;
     CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yy_fixed;
@@ -169,6 +170,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.bAngle;
     CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.bAngleFix;
     CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.bSeen;
+    CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.bDist;
 
     CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yb;
     CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yb_fixed;
@@ -178,6 +180,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleAtk = CURRENT_DATA_WRITE.bAngle;
     CURRENT_DATA_WRITE.angleAtkFix = CURRENT_DATA_WRITE.bAngleFix;
     CURRENT_DATA_WRITE.atkSeen = CURRENT_DATA_WRITE.bSeen;
+    CURRENT_DATA_WRITE.distAtk = CURRENT_DATA_WRITE.bDist;
 
     CURRENT_DATA_WRITE.yAtk = CURRENT_DATA_WRITE.cam_yb;
     CURRENT_DATA_WRITE.yAtkFix = CURRENT_DATA_WRITE.cam_yb_fixed;
@@ -187,6 +190,7 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleDef = CURRENT_DATA_WRITE.yAngle;
     CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
     CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
+    CURRENT_DATA_WRITE.distDef = CURRENT_DATA_WRITE.yDist;
 
     CURRENT_DATA_WRITE.yDef = CURRENT_DATA_WRITE.cam_yy;
     CURRENT_DATA_WRITE.yDefFix = CURRENT_DATA_WRITE.cam_yy_fixed;

src/strategy_roles/games.cpp

@@ -15,5 +15,6 @@ void initGames(){
     striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     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());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/precision_shooter.cpp

@@ -29,18 +29,6 @@ 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;
-int limitx = 0;
-
 void PrecisionShooter::realPlay()
 {
   if (CURRENT_DATA_READ.ballSeen)
@@ -103,12 +91,12 @@ void PrecisionShooter::spinner(int targetx){
         tilt1 = -0.15;
         tilt2 = 0.55;
 
-        limitx = 360-KICK_LIMIT_TILT1;
+        limitx = 360-PS_KICK_LIMIT_TILT1;
       }else{
         tilt1 = 0.15;
         tilt2 = -0.55;
 
-        limitx = KICK_LIMIT_TILT1;
+        limitx = PS_KICK_LIMIT_TILT1;
       }
     
     }else ((PositionSysCamera*)ps)->setMoveSetpoints(spotx, 0);
@@ -133,10 +121,10 @@ void PrecisionShooter::spinner(int targetx){
     drive->prepareDrive(0,0,0);
 
     // spinner_tilt += tilt2;
-    // spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
+    // spinner_tilt = constrain(spinner_tilt, PS_KICK_LIMIT_MIN, PS_KICK_LIMIT_MAX);
     // drive->prepareDrive(0,0,spinner_tilt);
 
-    if(CURRENT_DATA_READ.IMUAngle >= KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= KICK_LIMIT_MIN) roller->speed(roller->MIN);
+    if(CURRENT_DATA_READ.IMUAngle >= PS_KICK_LIMIT_MAX || CURRENT_DATA_READ.IMUAngle <= PS_KICK_LIMIT_MIN) roller->speed(roller->MIN);
   }
 }
 /*

src/strategy_roles/striker_roller.cpp

@@ -0,0 +1,183 @@
+#include "behaviour_control/status_vector.h"
+#include "systems/position/positionsys_camera.h"
+#include "sensors/sensors.h"
+#include "sensors/data_source_ball.h"
+#include "strategy_roles/striker_roller.h"
+#include "vars.h"
+#include "math.h"
+
+
+StrikerRoller::StrikerRoller() : Game()
+{
+  init();
+}
+
+StrikerRoller::StrikerRoller(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
+{
+  init();
+}
+
+void StrikerRoller::init()
+{
+  atk_speed = 0;
+  atk_direction = 0;
+  atk_tilt = 0;
+  cstorc = 0;
+
+  gotta_tilt = false;
+  
+  ballAngleFilter = new ComplementaryFilter(0.85);
+}
+
+void StrikerRoller::realPlay()
+{
+  if (CURRENT_DATA_READ.ballSeen)
+    this->catchBall();
+  else{
+    ps->goCenter();
+    ball_catch_flag = false;
+    spinner_flag = false;
+    ball_catch_state = 0;
+    spinner_state = 0;
+  }
+}
+
+/*
+Spinning kick state machine
+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
+*/
+void StrikerRoller::spinner(int targetx){
+  // if(!ballPresence->isInMouth()) {
+  //   spinner_state=0;
+  //   spinner_flag = false;
+  // }
+
+  if(spinner_state == 0){
+    if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
+
+    if(ballPresence->isInMouth() && !spinner_flag){
+      spinner_flag = true;
+      spinner_timer = millis();
+    }
+
+    if(ballPresence->isInMouth() && spinner_flag && millis() - spinner_timer > 500) {
+      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;
+      tilt2 = 0.55;
+
+      limitx = 360-STRL_KICK_LIMIT_TILT1;
+    }else{
+      tilt1 = 0.15;
+      tilt2 = -0.55;
+
+      limitx = STRL_KICK_LIMIT_TILT1;
+    }
+    spinner_state=1;
+  }else if(spinner_state == 2){
+    roller->speed(roller->MAX);
+
+    spinner_tilt += tilt1;
+    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;
+    }
+  }else if(spinner_state == 3){
+    roller->speed(roller->MAX);
+    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);
+
+    // spinner_tilt += tilt2;
+    // spinner_tilt = constrain(spinner_tilt, KICK_LIMIT_MIN, KICK_LIMIT_MAX);
+    // 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);
+  }
+}
+/*
+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
+*/
+
+void StrikerRoller::catchBall(){
+
+  if(ball->isInFront() && roller->roller_armed) roller->speed(roller->MAX);
+  else roller->speed(roller->MIN);
+
+  if(!ball->isInFront()){
+    ball_catch_state = 0;
+    ball_catch_flag = false;
+  }
+
+  if(ball_catch_state == 0){
+    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 > 350) {
+      ball_catch_state++;
+      ball_catch_tilt = CURRENT_DATA_READ.IMUAngle;
+    }
+  }else if(ball_catch_state == 1){
+    int val = ballAngleFilter->calculate(CURRENT_DATA_READ.ballAngleFix);
+    drive->prepareDrive(drive->directionAccountingForTilt(CURRENT_DATA_READ.angleAtkFix, val ),MAX_VEL_HALF,val);
+
+    if(CURRENT_DATA_READ.distAtk <= 30){
+       ball_catch_state++;
+       spinner_state = 0;
+       spinner_flag = false;
+       spinner_tilt = 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() {
+  if (ball->isInFront() && CURRENT_DATA_READ.ballDistance <= 90 /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
+  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);
+  }
+
+  return atk_tilt;
+}