superteam challenges: challenge 3: spinner

include/motors_movement/drivecontroller.h

@@ -20,7 +20,9 @@
 
 //Max possible vel 310
 
-#define MAX_VEL 110
+// #define MAX_POSSIBLE_VEL 310
+#define MAX_POSSIBLE_VEL 280
+#define MAX_VEL 100
 #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

@@ -12,7 +12,7 @@
 //Imu To Camera Angle Mult
 #define IMUTOC_AMULT 1
 
-#define FILTER_DEFAULT_COEFF 0.6
+#define FILTER_DEFAULT_COEFF 0.25
 #define FILTER_BY_COEFF FILTER_DEFAULT_COEFF
 #define FILTER_BX_COEFF FILTER_DEFAULT_COEFF
 #define FILTER_YY_COEFF FILTER_DEFAULT_COEFF
@@ -27,8 +27,8 @@ These values need to be subtracted from the coords used in setMoveSetpoints*/
 // #define CAMERA_TRANSLATION_Y 7
 
 //Robot with roller
-#define CAMERA_TRANSLATION_X 0
+#define CAMERA_TRANSLATION_X -3
-#define CAMERA_TRANSLATION_Y 12
+#define CAMERA_TRANSLATION_Y -3
 
 class DataSourceCameraConic : public DataSource{
 

include/strategy_roles/games.h

@@ -13,6 +13,7 @@
 #include "strategy_roles/precision_shooter.h"
 #include "strategy_roles/pass_and_shoot.h"
 #include "strategy_roles/spot_finder.h"
+#include "strategy_roles/the_spinner.h"
 // #include "strategy_roles/keeper.h"
 
 void initGames();
@@ -24,4 +25,5 @@ g_extr Game* pass_and_shoot;
 // g_extr Game* keeper;
 
 g_extr Game* tc1;
-g_extr Game* tc2;
+g_extr Game* st_tc1;
+g_extr Game* st_tc3;

include/strategy_roles/the_spinner.h

@@ -0,0 +1,56 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define X_COORD 10
+#define Y_COORD 15
+
+class Spinner : public Game{
+
+    public:
+        Spinner();
+        Spinner(LineSystem* ls, PositionSystem* ps);
+
+    private:
+        void realPlay() override;
+        void init() override;
+        void circle();
+        bool doingCircle = false;
+        bool firstCircle = true;        
+        bool flag = false;
+        unsigned long t =0;
+
+        int step = 0;
+
+        typedef struct v{
+            v(){
+                x = 0;
+                y = 0;
+            }
+            v(int x_, int y_){
+                x = x_;
+                y = y_;
+            }
+            int x, y;
+        } spot;
+
+        /*
+        spot(13, 15), //top right spot
+        spot(-13, 15), //top left spot
+        spot(-13, -15) //bottom left spot
+        spot(13, -15), //bottom right spot
+        */
+
+        vector<spot> spots = {
+            /*START: centre*/
+            spot(0,0),
+            
+            // /*1ST BOTTLE*/
+            spot(20, 15),
+        };
+
+        int current_spot_i = 0;
+        spot current_spot;
+};

include/systems/position/positionsys_camera.h

@@ -20,7 +20,8 @@
 //Actually it's ± MAX_VAL
 #define MAX_X 50
 #define MAX_Y (CAMERA_CENTER_Y_ABS_SUM/2)
-#define DIST_MULT 5
+#define MAX_DIST_EXPERIMENTAL 70
+#define DIST_MULT 8
 
 #define VICINITY_DIST_TRESH 2
 #define ROUGH_VICINITY_DIST_TRESH 10

src/main.cpp

@@ -54,8 +54,7 @@ void loop() {
   // keeper_condition = role == LOW;
 
   if(robot_indentifier){
-    tc2->play(1);
+    striker->play(1);
-    // if(roller->roller_armed) roller->speed(roller->MAX);
     // Last thing to do: movement and update status vector
     drive->drivePrepared();  
   }else{

src/strategy_roles/games.cpp

@@ -17,6 +17,7 @@ void initGames(){
     precision_shooter = new PrecisionShooter(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     striker_roller = new StrikerRoller(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     tc1 = new StrikerRoller(new LineSystemEmpty(), new PositionSysCamera());
-    tc2 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
+    st_tc1 = new SpotFinder(new LineSystemEmpty(), new PositionSysCamera());
+    st_tc3 = new Spinner(new LineSystemEmpty(), new PositionSysCamera());
     // keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/striker.cpp

@@ -72,7 +72,8 @@ void Striker::striker(){
   else dir = ball_deg - plusang;                                //se sto nel negativo sottraggo
 
   dir = (dir + 360) % 360;
-  drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
+  // drive->prepareDrive(dir, MAX_VEL_HALF, tilt());
+  drive->prepareDrive(dir, MAX_VEL_HALF, 0);
 
   if(ball->isInFront() && roller->roller_armed) roller->speed(ROLLER_DEFAULT_SPEED);
   else roller->speed(roller->MIN);

src/strategy_roles/the_spinner.cpp

@@ -0,0 +1,48 @@
+#include "strategy_roles/the_spinner.h"
+#include "systems/position/positionsys_camera.h"
+#include "vars.h"
+#include "math.h"
+
+Spinner::Spinner() : Game() {}
+Spinner::Spinner(LineSystem *ls_, PositionSystem *ps_) : Game(ls_, ps_) {}
+
+void Spinner::init() {}
+void Spinner::realPlay() {
+  if(current_spot_i >= spots.size()) return;
+
+  current_spot = spots[current_spot_i];
+
+  if(doingCircle){
+    circle();
+    return;
+  }
+
+  if (((PositionSysCamera*)ps)->isInTheVicinityOf(current_spot.x, current_spot.y)) {
+    if(current_spot.x == 0 && current_spot.y == 0) {
+      current_spot_i++;
+      return;
+    }
+    doingCircle = true;
+    firstCircle = true;
+    step = 0;
+  }
+  else (((PositionSysCamera*)ps)->setMoveSetpoints(current_spot.x, current_spot.y));
+}
+
+void Spinner::circle(){
+  if(millis() - t < 1000){
+    if(step == 0) drive->prepareDrive(45, 50, 0);
+    else if(step == 1) drive->prepareDrive(315, 50, 0);
+    else if(step == 2) drive->prepareDrive(225, 50, 0);
+    else if(step == 3) {
+      if(firstCircle){
+      drive->prepareDrive(135, 80, 0);
+      firstCircle = false;
+      }else doingCircle = false;
+    }
+  }else {
+    step++;
+    t = millis();
+  }
+  
+}

src/system/positions/positionsys_camera.cpp

@@ -7,6 +7,7 @@
 
 PositionSysCamera::PositionSysCamera() {
     MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
+
     Inputx = 0;
     Outputx = 0;
     Setpointx = 0;
@@ -52,7 +53,7 @@ void PositionSysCamera::update(){
     //IMPORTANT STEP: or the direction of the plane will be flipped
     posx *= -1;
     posy *= -1;
-    
+
     //Filtering error in calculation like this is a dirty hack, I know
     if(posx < -MAX_X || posx > MAX_X || posy <  -MAX_Y || 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
@@ -150,11 +151,17 @@ void PositionSysCamera::CameraPID(){
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
-        // int dist = sqrt( ( pow(CURRENT_DATA_WRITE.posx-Setpointx,2) ) + pow(CURRENT_DATA_WRITE.posy-Setpointy, 2) );
+        float distance = hypot(Outputx, Outputy);
-        // int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL);
+        float speed = distance > 3 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_POSSIBLE_VEL) : 0;
-        int speed = hypot(Outputx, Outputy) * DIST_MULT;
+
-        // speed = speed > 10 ? speed : 0;
+        // DEBUG.print("x: ");
-        dir = filterDir->calculate(dir);
+        // DEBUG.print(Outputx);
+        // DEBUG.print(" y:");
+        // DEBUG.print(Outputy);
+        // DEBUG.print(" Hypot:");
+        // DEBUG.print(hypot(Outputx, Outputy));
+        // DEBUG.print(" Speed:");
+        // DEBUG.println(speed);
 
         #ifdef DRIVE_VECTOR_SUM
         vx = ((speed * cosins[dir]));

utility/OpenMV/conic_eff_h7.py

@@ -45,8 +45,8 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (84, 100, -29, 1, 36, 127),    # thresholds yellow goalz
+thresholds = [  (68, 100, -16, 26, 24, 85),    # thresholds yellow goalz
-                (36, 100, -34, 12, -59, -19)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+                (36, 70, -3, 27, -75, -27)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 
 roi = (70, 0, 250, 200)
@@ -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, -1.804))
+sensor.set_auto_whitebal(True, (-6.02073, -5.119987, -1.006964))
 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=80, area_threshold=100, merge = True):
+    for blob in img.find_blobs(thresholds, pixels_threshold=100, area_threshold=100, merge = True):
         img.draw_rectangle(blob.rect())
         #img.draw_cross(blob.cx(), blob.cy())