diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index 3a67b28..b061937 100644 --- a/include/behaviour_control/status_vector.h +++ b/include/behaviour_control/status_vector.h @@ -1,7 +1,7 @@ #pragma once #include #include "strategy_roles/game.h" -#include "position/systems.h" +#include "systems/systems.h" /** * STATUS VECTOR: diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index 0c0f255..dd41ada 100644 --- a/include/sensors/sensors.h +++ b/include/sensors/sensors.h @@ -12,10 +12,10 @@ #include "behaviour_control/ds_ctrl.h" #include "motors_movement/drivecontroller.h" #include "motors_movement/motor.h" -#include "sensors/linesys_2019.h" -#include "sensors/linesys_camera.h" -#include "position/positionsys_zone.h" -#include "position/systems.h" +#include "systems/lines/linesys_2019.h" +#include "systems/lines/linesys_camera.h" +#include "systems/position/positionsys_zone.h" +#include "systems/systems.h" #include "sensors/data_source_ball.h" #include "sensors/data_source_bt.h" #include "sensors/data_source_bno055.h" diff --git a/include/strategy_roles/game.h b/include/strategy_roles/game.h index b2822ce..81af76c 100644 --- a/include/strategy_roles/game.h +++ b/include/strategy_roles/game.h @@ -1,7 +1,7 @@ #pragma once #include "vars.h" -#include "position/systems.h" +#include "systems/systems.h" #include "sensors/sensors.h" class Game { diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index 01c325c..3d55089 100644 --- a/include/strategy_roles/games.h +++ b/include/strategy_roles/games.h @@ -8,10 +8,10 @@ #include #include "strategy_roles/game.h" -#include "strategy_roles/goalie.h" +#include "strategy_roles/striker.h" #include "strategy_roles/keeper.h" void initGames(); -g_extr Game* goalie; +g_extr Game* striker; g_extr Game* keeper; \ No newline at end of file diff --git a/include/strategy_roles/goalie.h b/include/strategy_roles/striker.h similarity index 84% rename from include/strategy_roles/goalie.h rename to include/strategy_roles/striker.h index cefb904..6500e71 100644 --- a/include/strategy_roles/goalie.h +++ b/include/strategy_roles/striker.h @@ -19,16 +19,16 @@ #define GOALIE_ATKDIR_PLUSANG2_COR 70 #define GOALIE_ATKDIR_PLUSANG3_COR 70 -class Goalie : public Game, public PositionSysZone{ +class Striker : public Game, public PositionSysZone{ public: - Goalie(); - Goalie(LineSystem* ls, PositionSystem* ps); + Striker(); + Striker(LineSystem* ls, PositionSystem* ps); private: void realPlay() override; void init() override; - void goalie(); + void striker(); void ballBack(); void storcimentoPorta(); diff --git a/include/sensors/linesys_2019.h b/include/systems/lines/linesys_2019.h similarity index 96% rename from include/sensors/linesys_2019.h rename to include/systems/lines/linesys_2019.h index f8a7d3c..520ec31 100644 --- a/include/sensors/linesys_2019.h +++ b/include/systems/lines/linesys_2019.h @@ -3,7 +3,7 @@ #include #include "behaviour_control/ds_ctrl.h" -#include "position/systems.h" +#include "systems/systems.h" #include "vars.h" diff --git a/include/sensors/linesys_camera.h b/include/systems/lines/linesys_camera.h similarity index 96% rename from include/sensors/linesys_camera.h rename to include/systems/lines/linesys_camera.h index 333ea1d..d14708b 100644 --- a/include/sensors/linesys_camera.h +++ b/include/systems/lines/linesys_camera.h @@ -3,7 +3,7 @@ #include #include "behaviour_control/ds_ctrl.h" -#include "position/systems.h" +#include "systems/systems.h" #include "vars.h" diff --git a/include/sensors/linesys_camera_new.h b/include/systems/lines/linesys_camera_new.h similarity index 100% rename from include/sensors/linesys_camera_new.h rename to include/systems/lines/linesys_camera_new.h diff --git a/include/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h similarity index 93% rename from include/position/positionsys_camera.h rename to include/systems/position/positionsys_camera.h index a5bb905..a099743 100644 --- a/include/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,12 +1,12 @@ #include "PID_v2.h" -#include "position/systems.h" +#include "systems/systems.h" //Note: those variables can be changes, and will need to change depending on camera calibration //Camera center: those setpoints correspond to the center of the field -#define CAMERA_CENTER_X 1 -#define CAMERA_CENTER_Y -10 +#define CAMERA_CENTER_X -5 +#define CAMERA_CENTER_Y -17 //Camera goal: those setpoints correspond to the position of the center of the goal on the field #define CAMERA_GOAL_X 0 diff --git a/include/position/positionsys_zone.h b/include/systems/position/positionsys_zone.h similarity index 99% rename from include/position/positionsys_zone.h rename to include/systems/position/positionsys_zone.h index 3600c83..5d42ddb 100644 --- a/include/position/positionsys_zone.h +++ b/include/systems/position/positionsys_zone.h @@ -1,6 +1,6 @@ #pragma once -#include "position/systems.h" +#include "systems/systems.h" //POSITION #define CENTERGOALPOST_VEL1 220 diff --git a/include/position/systems.h b/include/systems/systems.h similarity index 100% rename from include/position/systems.h rename to include/systems/systems.h diff --git a/src/main.cpp b/src/main.cpp index cd37a1e..dd0da0c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,7 +3,6 @@ #define VARS #include "behaviour_control/status_vector.h" -#include "position/positionsys_zone.h" #include "sensors/sensors.h" #include "strategy_roles/games.h" #include "vars.h" @@ -33,7 +32,7 @@ void loop() { updateSensors(); if(DEBUG.available()) testmenu->testMenu(); - goalie->play(role==1); + striker->play(role==1); keeper->play(role==0); // Last thing to do: movement and update status vector diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp index f9b5b05..2b51478 100644 --- a/src/strategy_roles/game.cpp +++ b/src/strategy_roles/game.cpp @@ -1,6 +1,6 @@ #include "behaviour_control/status_vector.h" #include "strategy_roles/game.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" Game::Game() {} Game::Game(LineSystem* ls_, PositionSystem* ps_) { diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 89d4a8f..8134766 100644 --- a/src/strategy_roles/games.cpp +++ b/src/strategy_roles/games.cpp @@ -1,15 +1,15 @@ #define GAMES_CPP /* #include "sensors/linesys_2019.h" */ -#include "sensors/linesys_camera.h" -#include "position/positionsys_zone.h" -#include "position/positionsys_camera.h" +#include "systems/lines/linesys_camera.h" +#include "systems/position/positionsys_zone.h" +#include "systems/position/positionsys_camera.h" #include "strategy_roles/games.h" 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) }; - goalie = new Goalie(new LineSysCamera(lIn, lOut), new PositionSysCamera()); + 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/keeper.cpp b/src/strategy_roles/keeper.cpp index 7e77f28..120ef0f 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -1,11 +1,11 @@ #include #include "behaviour_control/status_vector.h" -#include "sensors/linesys_2019.h" +#include "systems/lines/linesys_2019.h" #include "sensors/sensors.h" #include "strategy_roles/keeper.h" #include "strategy_roles/games.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" Keeper::Keeper() : Game() { @@ -36,7 +36,7 @@ void Keeper::keeper() { if(ball->distance > KEEPER_ATTACK_DISTANCE){ // Ball is quite near - goalie->play(); + striker->play(); if(!this->ls->tookLine){ keeperAttackTimer = 0; keeper_tookTimer = true; diff --git a/src/strategy_roles/goalie.cpp b/src/strategy_roles/striker.cpp similarity index 89% rename from src/strategy_roles/goalie.cpp rename to src/strategy_roles/striker.cpp index 9ef0f3e..591a641 100644 --- a/src/strategy_roles/goalie.cpp +++ b/src/strategy_roles/striker.cpp @@ -1,32 +1,32 @@ #include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" -#include "strategy_roles/goalie.h" +#include "strategy_roles/striker.h" #include "vars.h" #include "math.h" -Goalie::Goalie() : Game() { +Striker::Striker() : Game() { init(); } -Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { +Striker::Striker(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { init(); } -void Goalie::init(){ +void Striker::init(){ atk_speed = 0; atk_direction = 0; cstorc = 0; } -void Goalie::realPlay(){ - if(CURRENT_DATA_READ.ballSeen) this->goalie(); +void Striker::realPlay(){ + if(CURRENT_DATA_READ.ballSeen) this->striker(); else ((PositionSysCamera*)ps)->goCenter(); } -void Goalie::goalie() { +void Striker::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; @@ -73,7 +73,7 @@ void Goalie::goalie() { -void Goalie::ballBack() { +void Striker::ballBack() { int ball_degrees2; int dir; @@ -91,7 +91,7 @@ void Goalie::ballBack() { } -void Goalie::storcimentoPorta() { +void Striker::storcimentoPorta() { if (CURRENT_DATA_READ.angleAtkFix >= 5 && CURRENT_DATA_READ.angleAtkFix <= 60) cstorc+=9; else if (CURRENT_DATA_READ.angleAtkFix <= 355 && CURRENT_DATA_READ.angleAtkFix >= 210) cstorc-=9; else cstorc *= 0.9; diff --git a/src/sensors/linesys_2019.cpp b/src/system/lines/linesys_2019.cpp similarity index 99% rename from src/sensors/linesys_2019.cpp rename to src/system/lines/linesys_2019.cpp index d8771d6..8ad3276 100644 --- a/src/sensors/linesys_2019.cpp +++ b/src/system/lines/linesys_2019.cpp @@ -1,4 +1,4 @@ -#include "sensors/linesys_2019.h" +#include "systems/lines/linesys_2019.h" #include "sensors/sensors.h" using namespace std; diff --git a/src/sensors/linesys_camera.cpp b/src/system/lines/linesys_camera.cpp similarity index 94% rename from src/sensors/linesys_camera.cpp rename to src/system/lines/linesys_camera.cpp index 9f8bf83..5117329 100644 --- a/src/sensors/linesys_camera.cpp +++ b/src/system/lines/linesys_camera.cpp @@ -1,5 +1,5 @@ -#include "sensors/linesys_camera.h" -#include "position/positionsys_camera.h" +#include "systems/lines/linesys_camera.h" +#include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" #include "strategy_roles/games.h" @@ -78,7 +78,7 @@ void LineSysCamera::outOfBounds(){ } if (exitTimer <= EXTIME){ - ((PositionSysCamera*)goalie->ps)->goCenter(); + ((PositionSysCamera*)striker->ps)->goCenter(); tookLine = true; }else{ drive->canUnlock = true; diff --git a/src/sensors/linesys_camera_new.cpp b/src/system/lines/linesys_camera_new.cpp similarity index 100% rename from src/sensors/linesys_camera_new.cpp rename to src/system/lines/linesys_camera_new.cpp diff --git a/src/position/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp similarity index 98% rename from src/position/positionsys_camera.cpp rename to src/system/positions/positionsys_camera.cpp index f6ee95c..52a074d 100644 --- a/src/position/positionsys_camera.cpp +++ b/src/system/positions/positionsys_camera.cpp @@ -1,5 +1,5 @@ #include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" #include "sensors/sensors.h" #include "vars.h" diff --git a/src/position/positionsys_zone.cpp b/src/system/positions/positionsys_zone.cpp similarity index 99% rename from src/position/positionsys_zone.cpp rename to src/system/positions/positionsys_zone.cpp index f9836ab..68fe989 100644 --- a/src/position/positionsys_zone.cpp +++ b/src/system/positions/positionsys_zone.cpp @@ -1,5 +1,5 @@ #include "behaviour_control/status_vector.h" -#include "position/positionsys_zone.h" +#include "systems/position/positionsys_zone.h" #include "sensors/sensors.h" #include "vars.h" diff --git a/src/test_menu.cpp b/src/test_menu.cpp index 1b10f00..4340afa 100644 --- a/src/test_menu.cpp +++ b/src/test_menu.cpp @@ -3,12 +3,12 @@ #include "sensors/data_source_bno055.h" #include "sensors/data_source_bt.h" #include "sensors/data_source_camera_conicmirror.h" -#include "sensors/linesys_camera.h" -#include "sensors/linesys_2019.h" +#include "systems/lines/linesys_camera.h" +#include "systems/lines/linesys_2019.h" #include "sensors/sensors.h" #include "motors_movement/motor.h" #include "motors_movement/drivecontroller.h" -#include "position/positionsys_camera.h" +#include "systems/position/positionsys_camera.h" #include "strategy_roles/game.h" #include "strategy_roles/games.h" #include "behaviour_control/data_source.h" @@ -91,7 +91,7 @@ void TestMenu :: testMenu(){ currentRole = DEBUG.read(); switch(currentRole){ case '1': - (goalie->ls)->test(); + (striker->ls)->test(); break; case '2': (keeper->ls)->test(); diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py index b04961c..2db0126 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (58, 97, 9, 45, 51, 92), # thresholds yellow goal - (12, 30, -25, 1, -49, -2)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +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) @@ -64,8 +64,8 @@ sensor.set_saturation(3) sensor.set_brightness(0) sensor.set_quality(0) sensor.set_auto_whitebal(False) -sensor.set_auto_exposure(False, 4500) -sensor.set_auto_gain(False, 15) +sensor.set_auto_exposure(False, 5500) +sensor.set_auto_gain(True) sensor.skip_frames(time = 300) clock = time.clock() diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave new file mode 100644 index 0000000..1a7f5e3 --- /dev/null +++ b/utility/OpenMV/conic_eff.py.autosave @@ -0,0 +1,149 @@ +# 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) +