diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index 6ef1bd0..45cf268 100644 --- a/include/motors_movement/drivecontroller.h +++ b/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) diff --git a/include/sensors/data_source_camera_conicmirror.h b/include/sensors/data_source_camera_conicmirror.h index 5714404..449aa75 100644 --- a/include/sensors/data_source_camera_conicmirror.h +++ b/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_Y 12 +#define CAMERA_TRANSLATION_X -3 +#define CAMERA_TRANSLATION_Y -3 class DataSourceCameraConic : public DataSource{ diff --git a/include/strategy_roles/games.h b/include/strategy_roles/games.h index c881a32..e48a036 100644 --- a/include/strategy_roles/games.h +++ b/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; \ No newline at end of file +g_extr Game* st_tc1; +g_extr Game* st_tc3; \ No newline at end of file diff --git a/include/strategy_roles/the_spinner.h b/include/strategy_roles/the_spinner.h new file mode 100644 index 0000000..d6f4f81 --- /dev/null +++ b/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 spots = { + /*START: centre*/ + spot(0,0), + + // /*1ST BOTTLE*/ + spot(20, 15), + }; + + int current_spot_i = 0; + spot current_spot; +}; diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h index 77a0a80..2f5c879 100644 --- a/include/systems/position/positionsys_camera.h +++ b/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 diff --git a/src/main.cpp b/src/main.cpp index cf569a5..286fb5d 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -54,8 +54,7 @@ void loop() { // keeper_condition = role == LOW; if(robot_indentifier){ - tc2->play(1); - // if(roller->roller_armed) roller->speed(roller->MAX); + striker->play(1); // Last thing to do: movement and update status vector drive->drivePrepared(); }else{ diff --git a/src/strategy_roles/games.cpp b/src/strategy_roles/games.cpp index 487eede..543d19b 100644 --- a/src/strategy_roles/games.cpp +++ b/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()); } \ No newline at end of file diff --git a/src/strategy_roles/striker.cpp b/src/strategy_roles/striker.cpp index 9833fb2..a7c8f90 100644 --- a/src/strategy_roles/striker.cpp +++ b/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); diff --git a/src/strategy_roles/the_spinner.cpp b/src/strategy_roles/the_spinner.cpp new file mode 100644 index 0000000..3de364e --- /dev/null +++ b/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(); + } + +} \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp index cb110d5..437952b 100644 --- a/src/system/positions/positionsys_camera.cpp +++ b/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) ); - // int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, MAX_VEL); - int speed = hypot(Outputx, Outputy) * DIST_MULT; - // speed = speed > 10 ? speed : 0; - dir = filterDir->calculate(dir); + float distance = hypot(Outputx, Outputy); + float speed = distance > 3 ? 20 + map(distance, 0, MAX_DIST_EXPERIMENTAL, 0, MAX_POSSIBLE_VEL) : 0; + + // DEBUG.print("x: "); + // 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])); diff --git a/utility/OpenMV/conic_eff_h7.py b/utility/OpenMV/conic_eff_h7.py index d5c8b91..c79be2c 100644 --- a/utility/OpenMV/conic_eff_h7.py +++ b/utility/OpenMV/conic_eff_h7.py @@ -45,8 +45,8 @@ blue_led.on() ############################################################################## -thresholds = [ (84, 100, -29, 1, 36, 127), # thresholds yellow goalz - (36, 100, -34, 12, -59, -19)] # thresholds blue goal (6, 31, -15, 4, -35, 0) +thresholds = [ (68, 100, -16, 26, 24, 85), # thresholds yellow goalz + (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())