preparation for romecup: refine striker

include/motors_movement/roller.h

@@ -2,7 +2,7 @@
 
 #include "ESC.h"
 
-#define ROLLER_DEFAULT_SPEED 1300
+#define ROLLER_DEFAULT_SPEED 1100
 
 class Roller{
     public:

include/strategy_roles/striker.h

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

include/systems/lines/linesys_camera.h

@@ -16,7 +16,7 @@
 #define S4I A9
 #define S4O A8
 
-#define LINE_THRESH_CAM 350
+#define LINE_THRESH_CAM 325
 #define EXIT_TIME 250
 #define LINES_EXIT_SPD 350
 

include/systems/position/positionsys_camera.h

@@ -8,7 +8,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 4
-#define CAMERA_TRANSLATION_Y 4
+#define CAMERA_TRANSLATION_Y 12
 //Camera center: those setpoints correspond to what we consider the center of the field
 #define CAMERA_CENTER_X 0
 #define CAMERA_CENTER_Y 0

include/vars.h

@@ -21,4 +21,6 @@ in the 32U4 code*/
 #define ROLLER_INA 34
 #define ROLLER_INB 35
 
+#define BALL_32U4 Serial2
+
 extr float sins[360], cosins[360];

src/motors_movement/drivecontroller.cpp

@@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int tilt){
     //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
     // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
     // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
-    vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
+    //vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
-    vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
+    //vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
 
-    // vx = ((speed * cosins[dir]));
+    vx = ((speed * cosins[dir]));
-    // vy = ((-speed * sins[dir]));
+    vy = ((-speed * sins[dir]));
 
     // if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
     //     vxn = 0;

src/motors_movement/roller.cpp

@@ -91,5 +91,6 @@ void Roller::update(){
 }
 
 void Roller::speed(int speed){
+    if(roller_armed)
         roller->speed(speed);
 }

src/sensors/data_source_camera_conicmirror.cpp

@@ -1,5 +1,6 @@
 #include "behaviour_control/status_vector.h"
 #include "sensors/data_source_camera_conicmirror.h"
+#include "vars.h"
 
 //Comment out to disable complementary filters on angles
 #define CAMERA_CONIC_FILTER_POINTS
@@ -170,6 +171,11 @@ void DataSourceCameraConic ::computeCoordsAngles() {
     CURRENT_DATA_WRITE.angleDefFix = CURRENT_DATA_WRITE.yAngleFix;
     CURRENT_DATA_WRITE.defSeen = CURRENT_DATA_WRITE.ySeen;
   }
+
+  byte to_32u4 = 0;
+  to_32u4 |= (CURRENT_DATA_READ.ySeen);
+  to_32u4 |= (CURRENT_DATA_READ.bSeen) << 1;
+  BALL_32U4.write(to_32u4);
 }
 
 void DataSourceCameraConic::test(){

src/sensors/sensors.cpp

@@ -8,7 +8,7 @@ void initSensors(){
     
     drive = new DriveController(new Motor(12, 11, 4, 55), new  Motor(25, 24, 5, 135), new Motor(27, 26, 2, 225), new Motor(29, 28, 3, 305));
     compass = new DataSourceBNO055();
-    ball = new DataSourceBall(&Serial2, 57600);
+    ball = new DataSourceBall(&BALL_32U4, 57600);
     camera = new DataSourceCameraConic(&Serial3, 19200);
     bt = new DataSourceBT(&Serial1, 115200);
     roller = new Roller(30, 31, 1000, 2000, 500);

src/strategy_roles/striker.cpp

@@ -35,38 +35,24 @@ void Striker::realPlay()
     ps->goCenter();
 }
 
-void Striker::striker()
+void Striker::striker(){
-{
+  //seguo palla
-  int plusang = STRIKER_PLUSANG, ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle;
+  int ball_degrees2, dir, ball_deg = CURRENT_DATA_READ.ballAngle, plusang = STRIKER_PLUSANG;
   
-  if (CURRENT_DATA_READ.ballDistance > STRIKER_ATTACK_DISTANCE)
+  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 > 180) ball_degrees2 = ball_deg - 360;            //ragiono in +180 -180  
-    drive->prepareDrive(ball_deg > 180 ? CURRENT_DATA_READ.ballAngle - 20 : CURRENT_DATA_READ.ballAngle + 20, MAX_VEL_HALF, 0);
+  else ball_degrees2 = ball_deg;
-    return;
-  }
 
-  if (ball_deg > 340 || ball_deg < 20)
+  if(ball_degrees2 > 0) dir = ball_deg + plusang;               //se sto nel quadrante positivo aggiungo
-    plusang -= STRIKER_PLUSANG_VISIONCONE; //se ho la palla in un range di +-20 davanti, diminuisco di 20 il plus
+  else dir = ball_deg - plusang;                                //se sto nel negativo sottraggo
-  if (ball_deg > 180)
-    ball_degrees2 = ball_deg - 360; //ragiono in +180 -180
-  else
-    ball_degrees2 = ball_deg;
 
-  if (ball_degrees2 > 0)
+  dir = (dir + 360) % 360;
-    dir = ball_deg + plusang; //se sto nel quadrante positivo aggiungo
-  else
-    dir = ball_deg - plusang; //se sto nel negativo sottraggo
-
-  if (dir < 0)
-    dir = dir + 360; //se sto nel quadrante negativo ricappotto
-  else
-    dir = dir;
 
   drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
 }
 
 int Striker::tilt() {
-  if (ball->isInMouth() || (ball->isInMouthMaxDistance() && gotta_tilt)) gotta_tilt = true;
+  if (ball->isInMouth() /* || (ball->isInMouthMaxDistance() && gotta_tilt)*/ ) gotta_tilt = true;
   else gotta_tilt = false;
 
   if(!gotta_tilt || !CURRENT_DATA_READ.atkSeen) {

src/system/positions/positionsys_camera.cpp

@@ -126,18 +126,18 @@ void PositionSysCamera::CameraPID(){
         speed = filterSpeed->calculate(speed);
         speed = speed > 40 ? speed : 0;
         dir = filterDir->calculate(dir);
-        // drive->prepareDrive(dir, speed, 0);
+        drive->prepareDrive(dir, speed, 0);
 
 
         //Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
         //and check the notes in drivecontroller for the other stuff to comment and uncomment
 
         //TODO: add complementary filter on this speed if we keep using it
-        vx = ((speed * cosins[dir]));
+        // vx = ((speed * cosins[dir]));
-        vy = ((-speed * sins[dir]));
+        // vy = ((-speed * sins[dir]));
 
-        CURRENT_DATA_WRITE.addvx = vx;
+        // CURRENT_DATA_WRITE.addvx = vx;
-        CURRENT_DATA_WRITE.addvy = vy;
+        // CURRENT_DATA_WRITE.addvy = vy;
     }
 }
 

utility/OpenMV/conic_eff_h7.py

@@ -45,11 +45,11 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (48, 83, -9, 18, 38, 82),    # thresholds yellow goal
+thresholds = [  (55, 92, -3, 24, 60, 90),    # thresholds yellow goal
-                (45, 61, -11, 14, -42, -22)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (33, 49, -9, 12, -52, -12)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
-roi = (20, 0, 300, 220)
+roi = (30, 0, 290, 240)
 
 # Camera Setup ###############################################################
 '''sensor.reset()xxxx
@@ -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=50, area_threshold=70, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=60, area_threshold=90, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())