romecup: initial working pass and shoot

include/sensors/data_source_camera_conicmirror.h

@@ -22,7 +22,7 @@
 To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
 These values need to be subtracted from the coords used in setMoveSetpoints*/
 #define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y 4
+#define CAMERA_TRANSLATION_Y 0
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/games.h

@@ -10,10 +10,12 @@
 #include "strategy_roles/game.h"
 #include "strategy_roles/striker.h"
 #include "strategy_roles/precision_shooter.h"
+#include "strategy_roles/pass_and_shoot.h"
 // #include "strategy_roles/keeper.h"
 
 void initGames();
 
 g_extr Game* striker;
 g_extr Game* precision_shooter;
+g_extr Game* pass_and_shoot;
 // g_extr Game* keeper;

include/strategy_roles/pass_and_shoot.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define PAS_ATTACK_DISTANCE 110
+#define PAS_TILT_STOP_DISTANCE 140
+#define PAS_PLUSANG 55
+#define PAS_PLUSANG_VISIONCONE 10
+
+class PassAndShoot : public Game{
+
+    public:
+        PassAndShoot();
+        PassAndShoot(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+        void striker();
+        int tilt();
+
+        int atk_speed, atk_direction, atk_tilt;
+        float cstorc;
+        unsigned long pass_counter;
+        bool gotta_tilt;
+};

include/strategy_roles/precision_shooter.h

@@ -25,4 +25,6 @@ class PrecisionShooter : public Game{
         float cstorc;
 
         bool gotta_tilt;
+
+        unsigned long pas_counter;
 };

src/main.cpp

@@ -17,8 +17,8 @@ bool striker_condition = false;
 bool keeper_condition = false;
 
 void setup() {
-  pinMode(LED_BUILTIN, OUTPUT);
   pinMode(BUZZER, OUTPUT);
+  
   tone(BUZZER, 220, 250);
   delay(1500);
   DEBUG.begin(115200);
@@ -52,8 +52,10 @@ void loop() {
   drive->resetDrive();
   
   striker_condition = role == HIGH;
-  striker->play(striker_condition);
+  // striker->play(striker_condition);
-  // precision_shooter->play(1);
+
+  if(role) precision_shooter->play(1);
+  else pass_and_shoot->play(1);
 
   // keeper_condition = role == LOW;
   // keeper->play(keeper_condition);

src/sensors/data_source_bt.cpp

@@ -29,7 +29,8 @@ DataSourceBT :: DataSourceBT(HardwareSerial* ser_, int baud) : DataSource(ser_,
     last_received = 0;
     t = 0;
 
-    tosend = 'B';
+    tosend = 'A';
+    received = '0';
 
     connect();
 }
@@ -57,7 +58,6 @@ void DataSourceBT :: receive(){
     last_received = millis();
     received = (char) Serial1.read();
     comrade = true;
-    DEBUG.println(received);
   }
   if(millis() - last_received > 2000) 
   comrade = false;
@@ -66,8 +66,6 @@ void DataSourceBT :: receive(){
 void DataSourceBT::send(){
   if(millis() - t >= 250){
     Serial1.print(tosend);
-    // DEBUG.print("Sending: ");
-    DEBUG.println(tosend);
   }
 }
 
@@ -75,8 +73,7 @@ void DataSourceBT::update(){
   // if(!bt_bombarded && can_bombard) connect();
   receive();
   send();
-  digitalWriteFast(LED_BUILTIN, comrade);
+  // if(comrade)Serial2.write(0b00000100);
-  digitalWriteFast(BUZZER, received == 'B') ;
 }
 
 void DataSourceBT :: test(){

src/strategy_roles/games.cpp

@@ -13,6 +13,7 @@ void initGames(){
 
     // striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     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 LineSystemEmpty(), new PositionSysCamera());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/pass_and_shoot.cpp

@@ -0,0 +1,92 @@
+#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/pass_and_shoot.h"
+#include "vars.h"
+#include "math.h"
+
+
+PassAndShoot::PassAndShoot() : Game()
+{
+  init();
+}
+
+PassAndShoot::PassAndShoot(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
+{
+  init();
+}
+
+void PassAndShoot::init()
+{
+  atk_speed = 0;
+  atk_direction = 0;
+  atk_tilt = 0;
+  cstorc = 0;
+
+  gotta_tilt = false;
+
+  pass_counter = millis();
+}
+
+void PassAndShoot::realPlay()
+{
+  if (CURRENT_DATA_READ.ballSeen){
+    if(robot_indentifier == HIGH){
+      if(millis() - pass_counter <= 1500) {
+        striker();
+      }else{
+        ((PositionSysCamera*)ps)->setMoveSetpoints(CAMERA_GOAL_MIN_X, CAMERA_GOAL_Y);
+        bt->tosend = 'C';
+      }
+    }else{
+      if(bt->received == 'C'){
+        // //Show on 32u4
+        Serial2.write(0b00000100);
+        striker();
+      }else drive->prepareDrive(0,0,0);
+    }
+  }else drive->prepareDrive(0,0,0);
+
+
+    
+}
+
+void PassAndShoot::striker(){
+
+  #ifdef MAX_VEL
+  #undef MAX_VEL
+  #define MAX_VEL 100
+  #endif
+
+  //seguo palla
+  int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang =  PAS_PLUSANG;
+  
+  if(ball_deg >= 346 || ball_deg <= 16) plusang = PAS_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);
+  
+}
+
+int PassAndShoot::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;
+}

utility/OpenMV/conic_eff_h7.py

@@ -46,10 +46,10 @@ blue_led.on()
 
 
 thresholds = [  (70, 100, -36, 8, 38, 112),    # thresholds yellow goalz
-                (46, 65, -18, 19, -59, -15)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (34, 54, -23, 13, -61, -18)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (80, 0, 240, 200)
+roi = (50, 0, 250, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx