strategy: introduce a test striker with empty systems and set as only role

include/strategy_roles/games.h

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

include/strategy_roles/striker_test.h

@@ -0,0 +1,36 @@
+#pragma once
+
+#include "sensors/data_source_camera_vshapedmirror.h"
+#include "sensors/sensors.h"
+#include "strategy_roles/game.h"
+
+#define TILT_MULT 1.8
+#define TILT_DIST 170
+#define CATCH_DIST 150
+#define GOALIE_ATKSPD_LAT  120 //255
+#define GOALIE_ATKSPD_BAK  120
+#define GOALIE_ATKSPD_FRT  120
+#define GOALIE_ATKSPD_STRK 120
+#define GOALIE_ATKDIR_PLUSANG1 20
+#define GOALIE_ATKDIR_PLUSANG2 35
+#define GOALIE_ATKDIR_PLUSANG3 40
+#define GOALIE_ATKDIR_PLUSANGBAK 40
+#define GOALIE_ATKDIR_PLUSANG1_COR 60
+#define GOALIE_ATKDIR_PLUSANG2_COR 70
+#define GOALIE_ATKDIR_PLUSANG3_COR 70
+
+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/systems.h

@@ -17,12 +17,14 @@ class PositionSystem{
 
 class LineSystemEmpty : public LineSystem{
     public:
+        LineSystemEmpty();
         void update() override;
         void test() override;
 };
 
 class PositionSystemEmpty : public PositionSystem{
     public:
+        PositionSystemEmpty();
         void update() override;
         void test() override;
         void goCenter() override;

src/main.cpp

@@ -25,6 +25,8 @@ void setup() {
   initGames();
   
   delay(500);
+
+  drive->prepareDrive(0,0,0);
 }
 
 
@@ -32,8 +34,11 @@ void loop() {
   updateSensors();
   if(DEBUG.available()) testmenu->testMenu();
   
-  striker->play(role==1);
+  striker_test->play(1);
-  keeper->play(role==0);
+  // striker->play(role==1);
+  // keeper->play(role==0);
+
+  // drive->prepareDrive(0,0,0);
 
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  

src/sensors/sensors.cpp

@@ -9,7 +9,7 @@ void initSensors(){
     pinMode(LED_Y, OUTPUT);
     pinMode(LED_G, OUTPUT);
   
-    drive = new DriveController(new Motor(11, 12, 4, 35), new  Motor(24, 25, 5, 135), new Motor(26, 27, 2, 225), new Motor(28, 29, 3, 325));
+    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));
     //drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial2, 57600);

src/strategy_roles/game.cpp

@@ -17,6 +17,5 @@ void Game::play(bool condition){
         CURRENT_DATA_WRITE.posSystem = (this->ps);
         CURRENT_DATA_WRITE.lineSystem = (this->ls);
         CURRENT_DATA_WRITE.game = this;
-        ((PositionSysCamera*)ps)->CameraPID();
     }
 }

src/strategy_roles/games.cpp

@@ -2,6 +2,7 @@
 
 /* #include "sensors/linesys_2019.h" */
 #include "systems/lines/linesys_camera.h"
+#include "systems/systems.h"
 #include "systems/position/positionsys_zone.h"
 #include "systems/position/positionsys_camera.h"
 #include "strategy_roles/games.h"
@@ -10,6 +11,7 @@ void initGames(){
     vector<DataSource*> lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
     vector<DataSource*> lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
 
+    striker_test = new StrikerTest(new LineSystemEmpty(), new PositionSystemEmpty());
     striker = new Striker(new LineSysCamera(lIn, lOut), new PositionSysCamera());
     keeper = new Keeper(new LineSysCamera(lOut, lOut), new PositionSysCamera());
 }

src/strategy_roles/striker_test.cpp

@@ -0,0 +1,82 @@
+#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 drive->prepareDrive(0,0,0);
+}
+
+void StrikerTest::striker() {
+  if(CURRENT_DATA_READ.ballAngle>= 350 || CURRENT_DATA_READ.ballAngle<= 20)  {
+    if(CURRENT_DATA_READ.ballDistance > 190) atk_direction = 0;
+    else atk_direction = CURRENT_DATA_READ.ballAngle;
+    atk_speed = GOALIE_ATKSPD_FRT;
+  }
+
+  if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) {
+  int ball_degrees2;
+  int dir;
+
+  int plusang;
+  if(CURRENT_DATA_READ.ballDistance > 130) plusang = GOALIE_ATKDIR_PLUSANGBAK;
+  else plusang = 0;
+  
+  if(CURRENT_DATA_READ.ballAngle> 180) ball_degrees2 = CURRENT_DATA_READ.ballAngle- 360;
+  else ball_degrees2 = CURRENT_DATA_READ.ballAngle;
+  if(ball_degrees2 > 0) dir = CURRENT_DATA_READ.ballAngle+ plusang;              //45 con 8 ruote
+  else dir = CURRENT_DATA_READ.ballAngle- plusang;                                 //45 con 8 ruote
+  if(dir < 0) dir = dir + 360;
+  else dir = dir;
+  atk_direction = dir;
+    atk_speed = GOALIE_ATKSPD_BAK;                   
+  }
+
+    if(CURRENT_DATA_READ.ballAngle> 10   && CURRENT_DATA_READ.ballAngle<  30)   {
+      atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG1;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(CURRENT_DATA_READ.ballAngle>= 30 && CURRENT_DATA_READ.ballAngle<  45)    {
+      atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG2;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(CURRENT_DATA_READ.ballAngle>= 45 && CURRENT_DATA_READ.ballAngle<  90)    {
+      atk_direction = CURRENT_DATA_READ.ballAngle+ GOALIE_ATKDIR_PLUSANG3;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(CURRENT_DATA_READ.ballAngle> 270 && CURRENT_DATA_READ.ballAngle<= 315)   {
+      atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG3_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(CURRENT_DATA_READ.ballAngle> 315 && CURRENT_DATA_READ.ballAngle<= 330)   {
+      atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG2_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+    if(CURRENT_DATA_READ.ballAngle> 330 && CURRENT_DATA_READ.ballAngle<  350)   {
+      atk_direction = CURRENT_DATA_READ.ballAngle- GOALIE_ATKDIR_PLUSANG1_COR;
+      atk_speed = GOALIE_ATKSPD_LAT;
+    }
+
+  drive->prepareDrive(atk_direction, atk_speed);
+}
+
+
+
+void StrikerTest::ballBack() {
+}

src/system/lines/linesys_empty.cpp

@@ -3,6 +3,9 @@
 #include "sensors/sensors.h"
 #include <Arduino.h>
 
+LineSystemEmpty::LineSystemEmpty(){
+}
+
 void LineSystemEmpty::update(){
     tookLine = false;
 }

src/system/positions/positionsys_empty.cpp

@@ -3,6 +3,10 @@
 #include "sensors/sensors.h"
 #include <Arduino.h>
 
+PositionSystemEmpty::PositionSystemEmpty(){
+    
+}
+
 void PositionSystemEmpty::update(){
 }
 

utility/OpenMV/conic_eff.py.autosave

@@ -1,149 +0,0 @@
-# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
-# Based on:
-# color tracking - By: paolix - ven mag 18 2018
-
-# Automatic RGB565 Color Tracking Example
-#
-
-import sensor, image, time, pyb, math
-
-from pyb import UART
-uart = UART(3,19200, timeout_char = 1000)
-
-START_BYTE = chr(105) #'i'
-END_BYTE = chr(115) #'s'
-BYTE_UNKNOWN = chr(116) #'t'
-
-y_found = False
-b_found = False
-
-#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
-def val_map(x, in_min, in_max, out_min, out_max):
-    x = int(x)
-    in_min = int(in_min)
-    in_max = int(in_max)
-    out_min = int(out_min)
-    out_max = int(out_max)
-    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
-
-
-# LED Setup ##################################################################
-red_led = pyb.LED(1)
-green_led = pyb.LED(2)
-blue_led = pyb.LED(3)
-
-red_led.off()
-green_led.off()
-blue_led.on()
-##############################################################################
-
-
-thresholds = [  (71, 100, -24, 12, 57, 99),    # thresholds yellow goal
-                (38, 55, -33, -1, 0, 26)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
-
-roi = (0, 6, 318, 152)
-
-# Camera Setup ###############################################################
-'''sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-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)
-#sensor.set_windowing(roi)
-clock = time.clock()'''
-
-sensor.reset()
-sensor.set_pixformat(sensor.RGB565)
-sensor.set_framesize(sensor.QQVGA)
-sensor.set_contrast(3)
-sensor.set_saturation(3)
-sensor.set_brightness(0)
-sensor.set_quality(0)
-sensor.set_auto_whitebal(False)
-sensor.set_auto_exposure(False, 5500)
-sensor.set_auto_gain(True)
-sensor.skip_frames(time = 300)
-
-clock = time.clock()
-##############################################################################
-
-
-while(True):
-    clock.tick()
-
-    blue_led.off()
-
-    y_found = False
-    b_found = False
-
-    tt_yellow = [(0,999,0,1)]     ## creo una lista di tuple per il giallo, valore x = 999 : non trovata
-    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=50, merge = True):
-        img.draw_rectangle(blob.rect())
-        img.draw_cross(blob.cx(), blob.cy())
-
-        if (blob.code() == 1):
-            tt_yellow = tt_yellow +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-            y_found = True
-        if (blob.code() == 2):
-            tt_blue = tt_blue +  [ (blob.area(),blob.cx(),blob.cy(),blob.code() ) ]
-            b_found = True
-
-    tt_yellow.sort(key=lambda tup: tup[0])  ## ordino le liste
-    tt_blue.sort(key=lambda tup: tup[0])    ## ordino le liste
-
-    ny = len(tt_yellow)
-    nb = len(tt_blue)
-
-    y_area, y1_cx, y1_cy, y_code = tt_yellow[ny-1]
-    b_area, b1_cx, b1_cy, b_code = tt_blue[nb-1]
-
-    y_cx = int(img.width() / 2 - y1_cx)
-    y_cy = int(img.height() / 2 - y1_cy)
-    b_cx = int(img.width() / 2 - b1_cx)
-    b_cy = int(img.height() / 2 - b1_cy)
-
-    #Normalize data between 0 and 100
-    if y_found == True:
-        y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 100, 0)
-        y_cy = val_map(y_cy, -img.height() / 2, img.height() / 2, 0, 100)
-        #Prepare for send as a list of characters
-        s_ycx = chr(y_cx)
-        s_ycy = chr(y_cy)
-    else:
-        y_cx = BYTE_UNKNOWN
-        y_cy = BYTE_UNKNOWN
-        #Prepare for send as a list of characters
-        s_ycx = y_cx
-        s_ycy = y_cy
-
-    if b_found == True:
-        b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 100, 0)
-        b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
-
-        #Prepare for send as a list of characters
-        s_bcx = chr(b_cx)
-        s_bcy = chr(b_cy)
-    else:
-        b_cx = BYTE_UNKNOWN
-        b_cy = BYTE_UNKNOWN
-        #Prepare for send as a list of characters
-        s_bcx = b_cx
-        s_bcy = b_cy
-
-    print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
-
-
-    uart.write(START_BYTE)
-    uart.write(s_bcx)
-    uart.write(s_bcy)
-    uart.write(s_ycx)
-    uart.write(s_ycy)
-    uart.write(END_BYTE)
-

utility/OpenMV/test.py

@@ -0,0 +1,78 @@
+# color tracking with conic mirror - By: EmaMaker - wed 15 jan 2020
+# Based on:
+# color tracking - By: paolix - ven mag 18 2018
+
+# Automatic RGB565 Color Tracking Example
+#
+
+import sensor, image, time, pyb, math
+
+from pyb import UART
+uart = UART(3,19200, timeout_char = 1000)
+
+START_BYTE = chr(105) #'i'
+END_BYTE = chr(115) #'s'
+BYTE_UNKNOWN = chr(116) #'t'
+
+y_found = False
+b_found = False
+
+#From Arduino Documentation at: https://www.arduino.cc/reference/en/language/functions/math/map/
+def val_map(x, in_min, in_max, out_min, out_max):
+    x = int(x)
+    in_min = int(in_min)
+    in_max = int(in_max)
+    out_min = int(out_min)
+    out_max = int(out_max)
+    return int((x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min)
+
+
+# LED Setup ##################################################################
+red_led = pyb.LED(1)
+green_led = pyb.LED(2)
+blue_led = pyb.LED(3)
+
+red_led.off()
+green_led.off()
+blue_led.on()
+##############################################################################
+
+
+thresholds = [  (71, 100, -24, 12, 57, 99),    # thresholds yellow goal
+                (38, 55, -33, -1, 0, 26)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+
+roi = (0, 6, 318, 152)
+
+# Camera Setup ###############################################################
+'''sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+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)
+#sensor.set_windowing(roi)
+clock = time.clock()'''
+
+sensor.reset()
+sensor.set_pixformat(sensor.RGB565)
+sensor.set_framesize(sensor.QQVGA)
+sensor.set_contrast(3)
+sensor.set_saturation(3)
+sensor.set_brightness(0)
+sensor.set_quality(0)
+sensor.set_auto_whitebal(False)
+sensor.set_auto_exposure(False, 5500)
+sensor.set_auto_gain(True)
+sensor.skip_frames(time = 300)
+
+clock = time.clock()
+##############################################################################
+
+
+while(True):
+    clock.tick()
+    uart.write(42)
+