romecup: precision shooter

include/motors_movement/drivecontroller.h

@@ -20,7 +20,7 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 150
+#define MAX_VEL 180
 #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)

include/sensors/data_source_camera_conicmirror.h

@@ -21,8 +21,8 @@
 /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
 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 1
+#define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y -4
+#define CAMERA_TRANSLATION_Y 0
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/games.h

@@ -9,11 +9,11 @@
 #include <Arduino.h>
 #include "strategy_roles/game.h"
 #include "strategy_roles/striker.h"
-#include "strategy_roles/striker_test.h"
+#include "strategy_roles/precision_shooter.h"
 #include "strategy_roles/keeper.h"
 
 void initGames();
 
 g_extr Game* striker;
-g_extr Game* striker_test;
+g_extr Game* precision_shooter;
 g_extr Game* keeper;

include/strategy_roles/precision_shooter.h

@@ -0,0 +1,28 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define PS_ATTACK_DISTANCE 110
+#define PS_TILT_STOP_DISTANCE 140
+#define PS_PLUSANG 55
+#define PS_PLUSANG_VISIONCONE 7
+
+class PrecisionShooter : public Game{
+
+    public:
+        PrecisionShooter();
+        PrecisionShooter(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+        void striker();
+        int tilt();
+
+        int atk_speed, atk_direction, atk_tilt;
+        float cstorc;
+
+        bool gotta_tilt;
+};

include/strategy_roles/striker.h

@@ -6,7 +6,7 @@
 
 #define STRIKER_ATTACK_DISTANCE 110
 #define STRIKER_TILT_STOP_DISTANCE 140
-#define STRIKER_PLUSANG 55
+#define STRIKER_PLUSANG 57
 #define STRIKER_PLUSANG_VISIONCONE 7
 
 class Striker : public Game{

include/strategy_roles/striker_test.h

@@ -1,26 +0,0 @@
-#pragma once
-
-#include "sensors/data_source_camera_vshapedmirror.h"
-#include "sensors/sensors.h"
-#include "strategy_roles/game.h"
-
-#define TARGET_DIST 120
-#define ATTACK_DIST 150
-#define ANGLE_SHIFT_STEP 5
-#define STRIKER_SPD 80
-
-class StrikerTest : public Game{
-
-    public:
-        StrikerTest();
-        StrikerTest(LineSystem* ls, PositionSystem* ps);
-
-    private:
-        void realPlay() override;
-        void init() override;
-        void striker();
-        void ballBack(); 
-
-        int atk_speed, atk_direction;
-    
-};

include/systems/lines/linesys_camera.h

@@ -7,14 +7,14 @@
 
 #include "vars.h" 
 
-#define S1I A7
+#define S1O A7
-#define S1O A6
+#define S1I A6
-#define S2I A2
+#define S2O A2
-#define S2O A3
+#define S2I A3
-#define S3I A0
+#define S3I A9
-#define S3O A1
+#define S3O A8
-#define S4I A9
+#define S4I A0
-#define S4O A8
+#define S4O A1
 
 #define LINE_THRESH_CAM 325
 #define EXIT_TIME 300

src/main.cpp

@@ -53,8 +53,10 @@ void loop() {
   striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   keeper_condition = role == LOW;
 
-  striker->play(striker_condition);
+  // striker->play(striker_condition);
-  keeper->play(keeper_condition);
+  // keeper->play(keeper_condition);
+  precision_shooter->play(1);
+
   testmenu->testMenu();
 
   // Last thing to do: movement and update status vector

src/strategy_roles/games.cpp

@@ -13,5 +13,6 @@ void initGames(){
 
     // striker_test = new StrikerTest(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     striker = new Striker(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/precision_shooter.cpp

@@ -0,0 +1,88 @@
+#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/precision_shooter.h"
+#include "vars.h"
+#include "math.h"
+
+
+PrecisionShooter::PrecisionShooter() : Game()
+{
+  init();
+}
+
+PrecisionShooter::PrecisionShooter(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_)
+{
+  init();
+}
+
+void PrecisionShooter::init()
+{
+  atk_speed = 0;
+  atk_direction = 0;
+  atk_tilt = 0;
+  cstorc = 0;
+
+  gotta_tilt = false;
+}
+
+void PrecisionShooter::realPlay()
+{
+  if (CURRENT_DATA_READ.ballSeen)
+    this->striker();
+  else
+    ps->goCenter();
+}
+
+unsigned long t3 = 0;
+
+void PrecisionShooter::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 =  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 <= 85 &&  CURRENT_DATA_READ.posy >= 32 &&  (CURRENT_DATA_READ.posx >= 15  || CURRENT_DATA_READ.posx <= -15) ) {
+    t3 = millis();
+  }
+
+  if(millis() - t3 < 1000){
+    roller->speed(1800);
+    ps->goCenter();
+  }
+
+
+  
+}
+
+int PrecisionShooter::tilt() {
+  if (ball->isInMouth() /* || (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;
+}

src/strategy_roles/striker.cpp

@@ -39,7 +39,7 @@ void Striker::striker(){
   //seguo palla
   int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
   
-  if(ball_deg >= 346 || 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 >= 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;
 

src/strategy_roles/striker_test.cpp

@@ -1,100 +0,0 @@
-#include "behaviour_control/status_vector.h"
-#include "sensors/sensors.h"
-#include "strategy_roles/striker_test.h"
-#include "vars.h"
-
-#include "math.h"
-
-StrikerTest::StrikerTest() : Game() {
-  init();
-}
-
-StrikerTest::StrikerTest(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) {
-  init();
-}
-
-void StrikerTest::init(){
-    atk_speed = 0;
-    atk_direction = 0;
-}
-
-void StrikerTest::realPlay(){
-  if(CURRENT_DATA_READ.ballSeen) striker();
-  else ps->goCenter();
-}
-
-void StrikerTest::striker()
-{
-  /*First implementation of "orbital striker", a new way to approach the problem with less lines.
-  It works with robot's positions, calculating the final one using the angle shift over
-  and over again until we have an acceptable result. That result is used to drive->prepareDrive with the speed.*/
-  // if (CURRENT_DATA_READ.ballDistance > )
-  //   drive->prepareDrive(CURRENT_DATA_READ.ballAngle, STRIKER_SPD);
-  // else
-  // {
-    // if (CURRENT_DATA_READ.ballAngle > 340 && CURRENT_DATA_READ.ballAngle < 20)
-    // {
-    //   drive->prepareDrive(0, 100, 0);
-    // }
-    // else
-    // {
-    //   int ballAngle = (90 - CURRENT_DATA_READ.ballAngle  + 360) % 360;
-    //   robotAngle = (ballAngle - 180 + 360) % 360;
-
-    //   DEBUG.print("Ball is at angle (goniometric circle): ");
-    //   DEBUG.println(ballAngle);
-    //   DEBUG.print("Robot is at angle ");
-    //   DEBUG.println(robotAngle);
-
-    //   float robotAngle_rad = robotAngle*3.14 / 180;
-    //   robotX = CURRENT_DATA_READ.ballDistance * cos(robotAngle_rad);
-    //   robotY = CURRENT_DATA_READ.ballDistance * sin(robotAngle_rad);
-
-    //   DEBUG.print("Coords of the robot relative to the ball: (");
-    //   DEBUG.print(robotX);
-    //   DEBUG.print(", ");
-    //   DEBUG.print(robotY);
-    //   DEBUG.println(")");
-
-    //   // angleDiff = min(((-robotAngle + 360) % 360), ((robotAngle + 360) % 360));
-    //   // angleShift = min(angleDiff, ANGLE_SHIFT_STEP);
-
-    //   angleShift = ANGLE_SHIFT_STEP;
-
-    //   if (robotAngle >= 0 && robotAngle <= 180)
-    //     newAngle = robotAngle - angleShift;
-    //   else
-    //     newAngle = robotAngle + angleShift;
-
-    //   DEBUG.print("New ball-robot angle: ");
-    //   DEBUG.println(newAngle);
-
-    //   float newAngle_rad = (newAngle)*3.14 / 180;
-
-    //   robotX_new = TARGET_DIST * cos(newAngle_rad);
-    //   robotY_new = TARGET_DIST * sin(newAngle_rad);
-
-    //   DEBUG.print("New coords of the robot relative to the ball: (");
-    //   DEBUG.print(robotX_new);
-    //   DEBUG.print(", ");
-    //   DEBUG.print(robotY_new);
-    //   DEBUG.println(")");
-
-    //   moveAngle = (atan2((robotX_new - robotX), (robotY_new - robotY))) * 180 / 3.14;
-    //   moveAngle = (moveAngle + 360) % 360;
-
-    //   DEBUG.print("Direction to move in: ");
-    //   DEBUG.println(moveAngle);
-
-    //   drive->prepareDrive(moveAngle, STRIKER_SPD);
-    // }
-  // }
-
-  // delay(1000);
-  // DEBUG.println("==========");
-}
-
-
-
-void StrikerTest::ballBack() {
-}

src/system/positions/positionsys_camera.cpp

@@ -60,7 +60,9 @@ void PositionSysCamera::update(){
         //IMPORTANT STEP: or the direction of the plane will be flipped
         posx *= -1;
         posy *= -1;
-    }else{
+    }
+    
+    if(abs(CURRENT_DATA_READ.posx-CURRENT_DATA_WRITE.posx)>MAX_X  || abs(CURRENT_DATA_READ.posy-CURRENT_DATA_WRITE.posy)>MAX_Y|| (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == false) ) {
         // Go back in time until we found a valid status, when we saw at least one goal
         int i = 1;
         do{

src/system/positions/positionsys_empty.cpp

@@ -15,9 +15,9 @@ void PositionSystemEmpty::test(){
 }
 
 void PositionSystemEmpty::goCenter(){
-    drive->prepareDrive(0,0,0);
+    // drive->prepareDrive(0,0,0);
 }
 
 void PositionSystemEmpty::centerGoal(){
-    drive->prepareDrive(0,0,0);
+    // drive->prepareDrive(0,0,0);
 }

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (61, 100, -11, 18, 60, 127),    # thresholds yellow goalz
+thresholds = [  (66, 88, -12, 26, 50, 79),    # thresholds yellow goalz
-                (34, 52, -14, 21, -67, -20)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (40, 61, -9, 19, -61, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (80, 0, 220, 200)
+roi = (80, 0, 240, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -71,7 +71,7 @@ sensor.set_windowing(roi)
 sensor.set_contrast(0)
 sensor.set_saturation(2)
 sensor.set_brightness(3)
-sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
+sensor.set_auto_whitebal(True, (-6.02073, -4.528669, -1.804))
 sensor.set_auto_exposure(False, 6576)
 #sensor.set_auto_gain(False, gain_db=8.78)
 sensor.skip_frames(time = 300)
@@ -94,7 +94,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())
 

utility/OpenMV/conic_eff_h7.py.autosave

@@ -36,8 +36,7 @@ def isInRightSide(img, x):
 
 # LED Setup ##################################################################
 red_led = pyb.LED(1)
-green_led
+green_led = pyb.LED(2)
-= pyb.LED(2)
 blue_led = pyb.LED(3)
 
 red_led.off()
@@ -46,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (61, 100, -11, 18, 60, 127),    # thresholds yellow goalz
+thresholds = [  (66, 88, -12, 26, 50, 79),    # thresholds yellow goalz
-                (34, 52, -14, 21, -67, -20)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (40, 61, -9, 19, -61, -21)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (80, 0, 220, 200)
+roi = (80, 0, 240, 200)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -59,6 +58,7 @@ sensor.set_framesize(sensor.QVGA)
 sensor.skip_frames(time = 2000)
 sensor.set_auto_gain(False)         # must be turned off for color tracking
 sensor.set_auto_whitebal(False)     # must be turned off for color tracking
+++++++++++++++++++++++++++++++  
 sensor.set_auto_exposure(False, 10000) vbc
 #sensor.set_backlight(1)
 #sensor.set_brightness(+2 )
@@ -72,7 +72,7 @@ sensor.set_windowing(roi)
 sensor.set_contrast(0)
 sensor.set_saturation(2)
 sensor.set_brightness(3)
-sensor.set_auto_whitebal(False, (-6.02073, -4.528669, -0.707413))
+sensor.set_auto_whitebal(True, (-6.02073, -4.528669, -1.804))
 sensor.set_auto_exposure(False, 6576)
 #sensor.set_auto_gain(False, gain_db=8.78)
 sensor.skip_frames(time = 300)
@@ -95,7 +95,7 @@ while(True):
     tt_blue = [(0,999,0,2)]       ## creo una lista di tuple per il blue, valore x = 999 : non trovata
 
     img = sensor.snapshot()
-    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=80, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=80, area_threshold=100, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())