diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h index f20048d..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: @@ -43,7 +43,8 @@ typedef struct data{ speed, tilt, dir, axisBlock[4], USfr, USsx, USdx, USrr, lineOutDir, matePos, role, cam_xb_fixed, - cam_xy_fixed, cam_yb_fixed, cam_yy_fixed; + cam_xy_fixed, cam_yb_fixed, cam_yy_fixed, + posx, posy, addvx, addvy; Game* game; LineSystem* lineSystem; PositionSystem* posSystem; diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h index a951194..280d851 100644 --- a/include/motors_movement/drivecontroller.h +++ b/include/motors_movement/drivecontroller.h @@ -51,6 +51,4 @@ class DriveController{ double input, output, setpoint; - float sins[360], cosins[360]; - }; \ No newline at end of file diff --git a/include/sensors/sensors.h b/include/sensors/sensors.h index fc19a93..ef73bd4 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 78% rename from include/strategy_roles/goalie.h rename to include/strategy_roles/striker.h index e75f3fd..6500e71 100644 --- a/include/strategy_roles/goalie.h +++ b/include/strategy_roles/striker.h @@ -5,9 +5,9 @@ #include "strategy_roles/game.h" #define TILT_MULT 1.8 -#define TILT_DIST 180 +#define TILT_DIST 170 #define CATCH_DIST 150 -#define GOALIE_ATKSPD_LAT 255 +#define GOALIE_ATKSPD_LAT 320 //255 #define GOALIE_ATKSPD_BAK 350 #define GOALIE_ATKSPD_FRT 345 #define GOALIE_ATKSPD_STRK 355 @@ -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(int); + 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 eeafd82..355c5d5 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 94% rename from include/sensors/linesys_camera.h rename to include/systems/lines/linesys_camera.h index 0ab82ab..f0fd759 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" @@ -17,7 +17,7 @@ #define S4O 22 //A2 #define LINE_THRESH_CAM 90 -#define EXIT_TIME 100 +#define EXIT_TIME 125 #define LINES_EXIT_SPD 350 class LineSysCamera : public LineSystem{ diff --git a/include/systems/lines/linesys_camera_new.h b/include/systems/lines/linesys_camera_new.h new file mode 100644 index 0000000..8de96bd --- /dev/null +++ b/include/systems/lines/linesys_camera_new.h @@ -0,0 +1,40 @@ +// #pragma once + +// #include + +// #include "behaviour_control/ds_ctrl.h" +// #include "position/systems.h" + +// #include "vars.h" + +// #define S1I A14 +// #define S1O A15 +// #define S2I A16 +// #define S2O A17 +// #define S3I A20 +// #define S3O A0 +// #define S4I A1 +// #define S4O A2 + +// #define LINE_THRESH_CAM 90 +// #define EXTIME_CAM 75 +// #define LINES_EXIT_SPD_CAM 300 + +// class LineSysCamera : public LineSystem{ + +// public: +// LineSysCamera(); +// LineSysCamera(vector in_, vector out); + +// void update() override; +// void test() override; +// void outOfBounds(); + +// private: +// vector in, out; +// DataSource* ds; +// bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow; +// int inV, outV, linesensOldX, linesensOldY, value, linetriggerI[4], linetriggerO[4], i; +// unsigned long exitTimer; +// byte linesens; +// }; \ No newline at end of file diff --git a/include/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h similarity index 53% rename from include/position/positionsys_camera.h rename to include/systems/position/positionsys_camera.h index ad3073b..a099743 100644 --- a/include/position/positionsys_camera.h +++ b/include/systems/position/positionsys_camera.h @@ -1,11 +1,17 @@ #include "PID_v2.h" -#include "position/systems.h" +#include "systems/systems.h" -#define CAMERA_CENTER_X 0 -#define CAMERA_CENTER_Y 0 +//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 -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 -#define CAMERA_GOAL_Y -13 +#define CAMERA_GOAL_Y -20 + #define CAMERA_CENTER_Y_ABS_SUM 72 //Actually it's ± MAX_VAL #define MAX_X 25 @@ -24,6 +30,9 @@ class PositionSysCamera : public PositionSystem{ public: PositionSysCamera(); void goCenter(); + void centerGoal(); + void setMoveSetpoints(int x, int y); + void addMoveOnAxis(int x, int y); void update() override; void test() override; void setCameraPID(); @@ -31,7 +40,8 @@ class PositionSysCamera : public PositionSystem{ int calcOtherGoalY(int goalY); double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy; - int MAX_DIST; + int MAX_DIST, vx, vy, axisx, axisy; + bool givenMovement; PID* X; PID* Y; 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/include/vars.h b/include/vars.h index e808e4a..673f35f 100755 --- a/include/vars.h +++ b/include/vars.h @@ -1,6 +1,12 @@ #pragma once #define DEBUG Serial +#ifdef VARS +#define extr +#else +#define extr extern +#endif + #define GLOBAL_SPD_MULT 1.0 /*SWS and LEDS are to be tested and implemented in the code. @@ -19,3 +25,5 @@ in the 32U4 code*/ #define SWITCH_1 39 #define SWITCH_2 38 #define SWITCH_3 33*/ + +extr float sins[360], cosins[360]; diff --git a/src/main.cpp b/src/main.cpp index 223d48f..dd0da0c 100755 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,15 +1,25 @@ #include -#include "test_menu.h" +#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" +#include "test_menu.h" + +TestMenu* testmenu; void setup() { delay(1500); - DEBUG.begin(9600); + + for(int i = 0; i < 360; i++){ + sins[i] = (float) sin((i*3.14/180)); + cosins[i] = (float) cos((i*3.14/180)); + } + + testmenu = new TestMenu(); initStatusVector(); initSensors(); initGames(); @@ -20,14 +30,12 @@ void setup() { void loop() { updateSensors(); -/* TestMenu testmenu; - testmenu.testMenu(); - goalie->play(role==1); + if(DEBUG.available()) testmenu->testMenu(); + + striker->play(role==1); keeper->play(role==0); - camera->test(); - // Last thing to do: movement and update status vector drive->drivePrepared(); - updateStatusVector(); */ -} + updateStatusVector(); +} \ No newline at end of file diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp index 98773b5..45df50e 100644 --- a/src/motors_movement/drivecontroller.cpp +++ b/src/motors_movement/drivecontroller.cpp @@ -1,17 +1,13 @@ #include "motors_movement/drivecontroller.h" #include "sensors/sensors.h" #include "behaviour_control/status_vector.h" +#include "vars.h" DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){ m1 = m1_; m2 = m2_; m3 = m3_; m4 = m4_; - - for(int i = 0; i < 360; i++){ - sins[i] = (float) sin(torad(i)); - cosins[i] = (float) cos(torad(i)); - } pDir = 0; pSpeed = 0; @@ -69,6 +65,11 @@ void DriveController::drive(int dir, int speed, int tilt){ speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT; tilt = tilt > 180 ? tilt - 360 : tilt; + //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE? + // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines + // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here + // vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx; + // vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy; vx = ((speed * cosins[dir])); vy = ((-speed * sins[dir])); @@ -87,7 +88,7 @@ void DriveController::drive(int dir, int speed, int tilt){ speed3 = -(speed1); speed4 = -(speed2); - // calcola l'errore di posizione rispetto allo 0 + // Calculate position error relative to the 0 delta = CURRENT_DATA_READ.IMUAngle; if(delta > 180) delta = delta - 360; diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp deleted file mode 100644 index ba28ab8..0000000 --- a/src/position/positionsys_camera.cpp +++ /dev/null @@ -1,120 +0,0 @@ -#include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" -#include "sensors/sensors.h" -#include "vars.h" - -PositionSysCamera::PositionSysCamera() { - setCameraPID(); -} - -void PositionSysCamera::update(){ -} - -void PositionSysCamera::test(){ -} - -void PositionSysCamera::goCenter(){ - /*WORKS BUT CAN BE BETTER*/ - //Y - /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0); - else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0); - //X - else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0); - else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0); - else drive->prepareDrive(0, 0, 0); */ - - - /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/ - /* int x = 1; - int y = 1; - ; - //Trying using an angle - if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ - if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) - y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; - if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb; - if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy; - - int dir = -90-(atan2(y*1.5,x)*180/3.14); - dir = (dir+360) % 360; - drive->prepareDrive(dir, 100, 0); - } */ - CameraPID(); -} - -//using a pid controller for the movement, or trying at least -void PositionSysCamera :: setCameraPID(){ - MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y); - Inputx = 0; - Outputx = 0; - Setpointx = 0; - Inputy = 0; - Outputy = 0; - Setpointy = 0; - - X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); - X->SetOutputLimits(-50,50); - X->SetMode(AUTOMATIC); - X->SetDerivativeLag(1); - X->SetSampleTime(2); - Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); - Y->SetOutputLimits(-50,50); - Y->SetMode(AUTOMATIC); - Y->SetDerivativeLag(1); - Y->SetSampleTime(2); -} - -/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one -We know the sum of the absolute values is a fixed number. -By♦ subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y -The sign of the goal y we found is simply the reverse of the one we got -*/ -int PositionSysCamera::calcOtherGoalY(int goalY){ - int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY); - otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY; - return otherGoalY; -} - -void PositionSysCamera :: CameraPID(){ - if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){ - Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2; - Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy; - }else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){ - Inputx = CURRENT_DATA_READ.cam_xb; - Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb); - }else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){ - Inputx = CURRENT_DATA_READ.cam_xy; - Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy); - //Setpointy todo - }else{ - //TODO: no goal seen - } - - //To have the axis corresponding to the real cartesian plane Inputx and InputY must be multiplied by -1 - //This is because the coordinates given as input are relative to the robot, while the coordinates used in the setpoint - //are on an absolute cartesian plane with it's center in the center of the field - Inputx *= -1; - Inputy *= -1; - - Setpointx = CAMERA_CENTER_X; - Setpointy = CAMERA_CENTER_Y; - - X->Compute(); - Y->Compute(); - - // DEBUG.print(CURRENT_DATA_READ.cam_yb); - // DEBUG.print(" "); - // DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb)); - - /*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){ - drive->prepareDrive(0,0,0); - }else{*/ - int dir = -90-(atan2(Outputy,Outputx)*180/3.14); - dir = (dir+360) % 360; - - int dist = sqrt(Outputx*Outputx + Outputy*Outputy); - int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350); - drive->prepareDrive(dir, speed, 0); - //DEBUG.println(dir); - //} -} \ No newline at end of file diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp index d4b7b4e..36f4e18 100644 --- a/src/sensors/data_source_camera_conicmirror.cpp +++ b/src/sensors/data_source_camera_conicmirror.cpp @@ -45,6 +45,8 @@ void DataSourceCameraConic ::readSensor() { true_yb = yb; true_xy = xy; true_yy = yy; + + computeCoordsAngles(); } end = true; start = false; diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp index 397c8b3..2b51478 100644 --- a/src/strategy_roles/game.cpp +++ b/src/strategy_roles/game.cpp @@ -1,5 +1,6 @@ #include "behaviour_control/status_vector.h" #include "strategy_roles/game.h" +#include "systems/position/positionsys_camera.h" Game::Game() {} Game::Game(LineSystem* ls_, PositionSystem* ps_) { @@ -8,14 +9,14 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) { } void Game::play(bool condition){ - ps->update(); if(condition) { + ps->update(); realPlay(); ls->update(); 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 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/goalie.cpp b/src/strategy_roles/goalie.cpp deleted file mode 100644 index cbcc3d7..0000000 --- a/src/strategy_roles/goalie.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include "behaviour_control/status_vector.h" -#include "position/positionsys_camera.h" -#include "sensors/sensors.h" -#include "strategy_roles/goalie.h" -#include "vars.h" - -#include "math.h" - - -Goalie::Goalie() : Game() { - init(); -} - -Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { - init(); -} - -void Goalie::init(){ - atk_speed = 0; - atk_direction = 0; - cstorc = 0; -} - -void Goalie::realPlay(){ - if(ball->ballSeen) this->goalie(50); - else ((PositionSysCamera*)ps)->goCenter(); -} - -int dir, degrees2; -void Goalie::goalie(int plusang) { - if(ball->distance < CATCH_DIST) drive->prepareDrive(ball->angle, 350, 0); - else{ -/* if(ball->angle > 345 || ball->angle < 15) plusang *= 0.15; - FRONT */ - if (ball->angle > 345 && ball->angle < 15) plusang *= 0.15; - if(ball->angle > 180) degrees2 = ball->angle - 360; - else degrees2 = ball->angle; - - if(degrees2 > 0) dir = ball->angle + plusang; //45 con 8 ruote - else dir = ball->angle - plusang; //45 con 8 ruote - - if(dir < 0) dir = dir + 360; - else dir = dir; - - storcimentoPorta(); - if(ball->distance > TILT_DIST && (ball->angle > 340 || ball->angle < 20)){ - plusang -= 20; - drive->prepareDrive(dir, 350, cstorc); - } else { - drive->prepareDrive(dir, 350, 0); - cstorc = 0; - } - } -} - -void Goalie::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; - cstorc = constrain(cstorc, -45, 45); -} \ No newline at end of file diff --git a/src/strategy_roles/keeper.cpp b/src/strategy_roles/keeper.cpp index 6d0040b..120ef0f 100644 --- a/src/strategy_roles/keeper.cpp +++ b/src/strategy_roles/keeper.cpp @@ -1,10 +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 "systems/position/positionsys_camera.h" Keeper::Keeper() : Game() { @@ -28,14 +29,14 @@ void Keeper::init(){ void Keeper::realPlay() { if(ball->ballSeen) keeper(); - else drive->prepareDrive(0,0,0); + else ((PositionSysCamera*)ps)->centerGoal(); } 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/striker.cpp b/src/strategy_roles/striker.cpp new file mode 100644 index 0000000..591a641 --- /dev/null +++ b/src/strategy_roles/striker.cpp @@ -0,0 +1,99 @@ +#include "behaviour_control/status_vector.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "strategy_roles/striker.h" +#include "vars.h" + +#include "math.h" + + +Striker::Striker() : Game() { + init(); +} + +Striker::Striker(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { + init(); +} + +void Striker::init(){ + atk_speed = 0; + atk_direction = 0; + cstorc = 0; +} + +void Striker::realPlay(){ + if(CURRENT_DATA_READ.ballSeen) this->striker(); + else ((PositionSysCamera*)ps)->goCenter(); +} + +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; + atk_speed = GOALIE_ATKSPD_FRT; + } + + if(CURRENT_DATA_READ.ballAngle>= 90 && CURRENT_DATA_READ.ballAngle<= 270) { + ballBack(); + 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; + } + + // if((CURRENT_DATA_READ.ballAngle>= 330 || CURRENT_DATA_READ.ballAngle<= 30) && CURRENT_DATA_READ.ballDistance > 190) { //storcimento + // atk_speed = GOALIE_ATKSPD_STRK; //dove i gigahertz hanno fallito + // drive->prepareDrive(atk_direction, atk_speed, cstorc); + // } + // else + drive->prepareDrive(atk_direction, atk_speed); +} + + + +void Striker::ballBack() { + 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; +} + + +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; + cstorc = constrain(cstorc, -45, 45); +} \ No newline at end of file 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/system/lines/linesys_camera_new.cpp b/src/system/lines/linesys_camera_new.cpp new file mode 100644 index 0000000..5a906fe --- /dev/null +++ b/src/system/lines/linesys_camera_new.cpp @@ -0,0 +1,116 @@ +// #include "sensors/linesys_camera.h" +// #include "position/positionsys_camera.h" +// #include "sensors/sensors.h" +// #include "strategy_roles/games.h" + +// using namespace std; +// LineSysCamera::LineSysCamera() {} +// LineSysCamera::LineSysCamera(vector in_, vector out_){ +// this->in = in_; +// this->out = out_; + +// fboundsX = false; +// fboundsY = false; +// slow = false; + +// linesensOldX = 0; +// linesensOldY = 0; + +// tookLine = false; + +// for(int i = 0; i < 4; i++){ +// linetriggerI[i] = 0; +// linetriggerO[i] = 0; +// } + +// exitTimer = 0; +// linesens = 0; +// } + +// void LineSysCamera::update(){ +// inV = 0; +// outV = 0; +// tookLine = false; + +// for(DataSource* d : in) d->readSensor(); +// for(DataSource* d : out) d->readSensor(); + +// for(auto it = in.begin(); it != in.end(); it++){ +// i = it - in.begin(); +// ds = *it; +// linetriggerI[i] = ds->getValue() > LINE_THRESH_CAM; +// } +// for(auto it = out.begin(); it != out.end(); it++){ +// i = it - out.begin(); +// ds = *it; +// linetriggerO[i] = ds->getValue() > LINE_THRESH_CAM; +// } + +// for(int i = 0; i < 4; i++){ +// inV = inV | (linetriggerI[i] << i); +// outV = outV | (linetriggerO[i] << i); +// } + + +// if ((inV > 0) || (outV > 0)) { +// if(millis() - exitTimer > EXTIME_CAM) { +// fboundsX = true; +// fboundsY = true; +// } +// exitTimer = millis(); +// } + +// linesens |= inV | outV; +// outOfBounds(); +// } + +// void LineSysCamera::outOfBounds(){ + +// if(fboundsX == true) { +// if(linesens & 0x02) linesensOldX = 2; +// else if(linesens & 0x08) linesensOldX = 8; +// if(linesensOldX != 0) fboundsX = false; +// } +// if(fboundsY == true) { +// if(linesens & 0x01) linesensOldY = 1; +// else if(linesens & 0x04) linesensOldY = 4; +// if(linesensOldY != 0) fboundsY = false; +// } + +// if (millis() - exitTimer <= EXTIME_CAM){ +// if(linesens > 0) ((PositionSysCamera*)goalie->ps)->goCenter(); + +// tookLine = true; +// }else{ +// linesens = 0; +// linesensOldY = 0; +// linesensOldX = 0; +// } +// } + +// void LineSysCamera::test(){ +// update(); +// DEBUG.print("In: "); +// for(DataSource* d : in){ +// d->update(); +// DEBUG.print(d->getValue()); +// DEBUG.print(" | "); +// } +// DEBUG.print(" |---| "); +// DEBUG.print("Out: "); +// for(DataSource* d : out){ +// d->update(); +// DEBUG.print(d->getValue()); +// DEBUG.print(" | "); +// } +// DEBUG.println(); +// for(int i = 0; i < 4; i++){ +// DEBUG.print(linetriggerI[i]); +// DEBUG.print(" | "); +// DEBUG.println(linetriggerO[i]); +// } + +// DEBUG.println(inV); +// DEBUG.println(outV); +// DEBUG.println(); +// } \ No newline at end of file diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp new file mode 100644 index 0000000..52a074d --- /dev/null +++ b/src/system/positions/positionsys_camera.cpp @@ -0,0 +1,135 @@ +#include "behaviour_control/status_vector.h" +#include "systems/position/positionsys_camera.h" +#include "sensors/sensors.h" +#include "vars.h" + +PositionSysCamera::PositionSysCamera() { + MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y); + Inputx = 0; + Outputx = 0; + Setpointx = 0; + Inputy = 0; + Outputy = 0; + Setpointy = 0; + + vx = 0; + vy = 0; + axisx = 0; + axisy = 0; + givenMovement = false; + + X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE); + X->SetOutputLimits(-50,50); + X->SetMode(AUTOMATIC); + X->SetDerivativeLag(1); + X->SetSampleTime(2); + Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE); + Y->SetOutputLimits(-50,50); + Y->SetMode(AUTOMATIC); + Y->SetDerivativeLag(1); + Y->SetSampleTime(2); +} + +void PositionSysCamera::update(){ + int posx, posy; + + //Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync + //Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot + if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){ + posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2; + posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy; + }else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){ + posx = CURRENT_DATA_WRITE.cam_xb; + posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb); + }else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){ + posx = CURRENT_DATA_WRITE.cam_xy; + posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy); + }else{ + //TODO: no goal seen ? + } + //IMPORTANT STEP: or the direction of the plane will be flipped + posx *= -1; + posy *= -1; + + CURRENT_DATA_WRITE.posx = posx; + CURRENT_DATA_WRITE.posy = posy; + Inputx = posx; + Inputy = posy; + + //Prepare for receiving information about movement + //Starting setpoint position as current position + Setpointx = posx; + Setpointy = posy; + axisx = 0; + axisy = 0; + givenMovement = false; +} + +//This means the last time this is called has the biggest priority, has for prepareDrive +void PositionSysCamera::setMoveSetpoints(int x, int y){ + Setpointx = x; + Setpointy = y; + givenMovement = true; +} + +void PositionSysCamera::addMoveOnAxis(int x, int y){ + axisx += x; + axisy += y; + givenMovement = true; +} + +void PositionSysCamera::goCenter(){ + setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y); +} + +void PositionSysCamera::centerGoal(){ + setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y); +} + +/*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one +We know the sum of the absolute values is a fixed number. +By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y +The sign of the goal y we found is simply the reverse of the one we got +*/ +int PositionSysCamera::calcOtherGoalY(int goalY){ + int otherGoalY = CAMERA_CENTER_Y_ABS_SUM - abs(goalY); + otherGoalY = goalY < 0 ? otherGoalY : -otherGoalY; + return otherGoalY; +} + +void PositionSysCamera::CameraPID(){ + if(givenMovement){ + + vx = 0; + vy = 0; + + Setpointx += axisx; + Setpointy += axisy; + + X->Compute(); + Y->Compute(); + + //Compute an X and Y to give to the PID later + //There's surely a better way to do this + int dir = -90-(atan2(Outputy,Outputx)*180/3.14); + dir = (dir+360) % 360; + + int dist = sqrt(Outputx*Outputx + Outputy*Outputy); + int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350); + drive->prepareDrive(dir, speed, 0); + + + //Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above + //and check the notes in drivecontroller for the other stuff to comment and uncomment + + //TODO: add complementary filter on this speed if we keep using it + // vx = ((speed * cosins[dir])); + // vy = ((-speed * sins[dir])); + + // CURRENT_DATA_WRITE.addvx = vx; + // CURRENT_DATA_WRITE.addvy = vy; + } +} + +void PositionSysCamera::test(){ +} 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 0fdfd21..2db0126 100644 --- a/utility/OpenMV/conic_eff.py +++ b/utility/OpenMV/conic_eff.py @@ -38,8 +38,8 @@ blue_led.on() ############################################################################## -thresholds = [ (50, 98, -2, 30, 57, 113), # thresholds yellow goal - (22, 45, -32, -6, -16, 10)] # 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) @@ -59,12 +59,12 @@ clock = time.clock()''' sensor.reset() sensor.set_pixformat(sensor.RGB565) sensor.set_framesize(sensor.QQVGA) -sensor.set_contrast(1) -sensor.set_saturation(1) +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, 6500) +sensor.set_auto_exposure(False, 5500) sensor.set_auto_gain(True) sensor.skip_frames(time = 300) @@ -84,7 +84,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=40, area_threshold=50, merge = True): + for blob in img.find_blobs(thresholds, pixels_threshold=30, area_threshold=40, merge = True): img.draw_rectangle(blob.rect()) img.draw_cross(blob.cx(), blob.cy()) 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) +