diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 3d55089..f3ba188 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -9,9 +9,11 @@ #include #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; \ No newline at end of file diff --git a/include/strategy_roles/striker_test.h b/include/strategy_roles/striker_test.h new file mode 100644 index 0000000..b9126e1 --- /dev/null +++ b/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; + +}; diff --git a/include/systems/systems.h b/include/systems/systems.h index 021149c..3480d50 100644 --- a/include/systems/systems.h +++ b/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; diff --git a/src/main.cpp b/src/main.cpp index dd0da0c..e830d9d 100755 --- a/src/main.cpp +++ b/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); - keeper->play(role==0); + striker_test->play(1); + // striker->play(role==1); + // keeper->play(role==0); + + // drive->prepareDrive(0,0,0); // Last thing to do: movement and update status vector drive->drivePrepared(); diff --git a/src/sensors/sensors.cpp b/src/sensors/sensors.cpp index 9e5fa1d..a89c522 100644 --- a/src/sensors/sensors.cpp +++ b/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); diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp index 2b51478..42aa7bf 100644 --- a/src/strategy_roles/game.cpp +++ b/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(); } } \ No newline at end of file diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 8134766..5485fe8 100644 --- a/src/strategy_roles/games.cpp +++ b/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 lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) }; vector 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()); } \ No newline at end of file diff --git a/src/strategy_roles/striker_test.cpp b/src/strategy_roles/striker_test.cpp new file mode 100644 index 0000000..918cccf --- /dev/null +++ b/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() { +} \ No newline at end of file diff --git a/src/system/lines/linesys_empty.cpp b/src/system/lines/linesys_empty.cpp index 0f25ac9..58bc1fd 100644 --- a/src/system/lines/linesys_empty.cpp +++ b/src/system/lines/linesys_empty.cpp @@ -3,6 +3,9 @@ #include "sensors/sensors.h" #include +LineSystemEmpty::LineSystemEmpty(){ +} + void LineSystemEmpty::update(){ tookLine = false; } diff --git a/src/system/positions/positionsys_empty.cpp b/src/system/positions/positionsys_empty.cpp index 2ccccf0..a908344 100644 --- a/src/system/positions/positionsys_empty.cpp +++ b/src/system/positions/positionsys_empty.cpp @@ -3,6 +3,10 @@ #include "sensors/sensors.h" #include +PositionSystemEmpty::PositionSystemEmpty(){ + +} + void PositionSystemEmpty::update(){ } diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave deleted file mode 100644 index 1a7f5e3..0000000 --- a/utility/OpenMV/conic_eff.py.autosave +++ /dev/null @@ -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) - diff --git a/utility/OpenMV/test.py b/utility/OpenMV/test.py new file mode 100644 index 0000000..8232cbb --- /dev/null +++ b/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) +