diff --git a/.vscode/extensions.json b/.vscode/extensions.json index 8281e64..272828b 100755 --- a/.vscode/extensions.json +++ b/.vscode/extensions.json @@ -1,7 +1,7 @@ -{ - // See http://go.microsoft.com/fwlink/?LinkId=827846 - // for the documentation about the extensions.json format - "recommendations": [ - "platformio.platformio-ide" - ] +{ + // See http://go.microsoft.com/fwlink/?LinkId=827846 + // for the documentation about the extensions.json format + "recommendations": [ + "platformio.platformio-ide" + ] } \ No newline at end of file diff --git a/include/data_source_controller.h b/include/data_source_controller.h deleted file mode 100755 index e83332f..0000000 --- a/include/data_source_controller.h +++ /dev/null @@ -1,23 +0,0 @@ -#pragma once -#include "data_source.h" -#include "vars.h" -#include -#include - -using namespace std; -class DataSourceController { - - public: - DataSourceController(); - DataSourceController(vector); - - public: - void update(); - void test(); - void postProcess(); - void readSensor(); - void getValue(); - - vector ds; - -}; \ No newline at end of file diff --git a/include/ds_ctrl.h b/include/ds_ctrl.h index 4db4910..3871999 100755 --- a/include/ds_ctrl.h +++ b/include/ds_ctrl.h @@ -17,6 +17,7 @@ class DataSourceCtrl { virtual void test(); virtual void postProcess(); virtual void read(); + int getValue(int i); vector ds; diff --git a/include/ds_ctrl_position.h b/include/ds_ctrl_position.h deleted file mode 100644 index 2bcad65..0000000 --- a/include/ds_ctrl_position.h +++ /dev/null @@ -1,10 +0,0 @@ -#include "vars.h" -#include "ds_ctrl.h" -#include "ds_ctrl_lines.h" - -class DSCtrlPosition : public DataSourceController { - - public: - DSCtrlPosition(); - -}; \ No newline at end of file diff --git a/include/game.h b/include/game.h index 2776e2e..7acc2f9 100755 --- a/include/game.h +++ b/include/game.h @@ -2,11 +2,16 @@ #include "vars.h" #include "sensors.h" +#include "systems.h" class Game { public: Game(); + Game(LineSystem* ls, PositionSystem* ps); void play(bool condition=true); private: virtual void realPlay() = 0; + virtual void init() = 0; + LineSystem* ls; + PositionSystem* ps; }; \ No newline at end of file diff --git a/include/goalie.h b/include/goalie.h index d6fde1d..619a273 100755 --- a/include/goalie.h +++ b/include/goalie.h @@ -20,12 +20,17 @@ class Goalie : public Game{ public: Goalie(); + Goalie(LineSystem* ls, PositionSystem* ps); + private: void realPlay() override; + void init() override; void goalie(); void ballBack(); void storcimentoPorta(); int atk_speed, atk_direction; float cstorc; + + }; diff --git a/include/keeper.h b/include/keeper.h index d63c850..ec7cfb0 100755 --- a/include/keeper.h +++ b/include/keeper.h @@ -2,22 +2,35 @@ #include "game.h" -//KEEPER -#define KEEPER_ATTACK_DISTANCE 200 +#define KEEPER_ATTACK_DISTANCE 190 #define KEEPER_ALONE_ATTACK_TIME 5000 //in millis #define KEEPER_COMRADE_ATTACK_TIME 3000//in millis #define KEEPER_BASE_VEL 320 #define KEEPER_VEL_MULT 1.4 #define KEEPER_BALL_BACK_ANGLE 30 +#define KEEPER_ANGLE_SX 265 +#define KEEPER_ANGLE_DX 85 +//POSITION +#define CENTERGOALPOST_VEL1 220 +#define CENTERGOALPOST_VEL2 220 +#define CENTERGOALPOST_VEL3 220 +#define CENTERGOALPOST_CAM_MIN -40 +#define CENTERGOALPOST_CAM_MAX 40 +#define CENTERGOALPOST_US_MAX 60 +#define CENTERGOALPOST_US_CRITIC 25 class Keeper : public Game{ public: Keeper(); + Keeper(LineSystem*, PositionSystem*); + private: void realPlay() override; + void init() override; void keeper(); + void centerGoalPostCamera(bool); int defSpeed, defDir; diff --git a/include/line_system.h b/include/line_system.h deleted file mode 100644 index 0f94b68..0000000 --- a/include/line_system.h +++ /dev/null @@ -1,11 +0,0 @@ -#pragma once -#include "ds_ctrl_lines.h" -#include "games.h" -#include "game.h" - -class LineSystem : public DSCtrlLines{ - public: - LineSystem(DSCtrlLines* method, Game* strategy); - virtual void update() = 0; - virtual void test() = 0; -}; \ No newline at end of file diff --git a/include/ds_ctrl_lines.h b/include/linesys_2019.h similarity index 73% rename from include/ds_ctrl_lines.h rename to include/linesys_2019.h index bf8330e..2c15694 100755 --- a/include/ds_ctrl_lines.h +++ b/include/linesys_2019.h @@ -1,6 +1,7 @@ #pragma once + #include "ds_ctrl.h" -#include "data_source.h" +#include "systems.h" #include "vars.h" #include @@ -16,19 +17,17 @@ #define LINE_THRESH 90 #define EXTIME 100 -class DSCtrlLines : public DataSourceCtrl{ +class LineSys2019 : public LineSystem{ public: - DSCtrlLines(); - DSCtrlLines(vector in_, vector out); + LineSys2019(); + LineSys2019(vector in_, vector out); - void read() override; - void postProcess() override; + void update() override; + void test() override; void outOfBounds(); void handleIntern(); void handleExtern(); - int getValue(); - void test() override; private: vector in, out; diff --git a/include/positionsys_zone.h b/include/positionsys_zone.h new file mode 100644 index 0000000..24dedff --- /dev/null +++ b/include/positionsys_zone.h @@ -0,0 +1,120 @@ +#pragma once + +#include "systems.h" + +//POSITION +#define CENTERGOALPOST_VEL1 220 +#define CENTERGOALPOST_VEL2 220 +#define CENTERGOALPOST_VEL3 220 +#define CENTERGOALPOST_CAM_MIN -40 +#define CENTERGOALPOST_CAM_MAX 40 +#define CENTERGOALPOST_US_MAX 60 +#define CENTERGOALPOST_US_CRITIC 25 +#define GOCENTER_VEL 280 + +#define ZONE_MAX_VALUE 150 +#define ZONE_LOOP_DECREASE_VALUE 4 +#define ZONE_US_UNKNOWN_INCREASE_VALUE 4 +#define ZONE_US_INDEX_INCREASE_VALUE 9 +#define ZONE_CAM_INCREASE_VALUE 3 +#define ZONE_CAM_CENTER_RANGE 25 +#define ZONE_LINES_INCREASE_VALUE 100 +#define ZONE_LINES_ERROR_VALUE 30 + +// You can modify this if you need +// LIMITI DEL CAMPO +#define Lx_min 115 // valore minimo accettabile di larghezza +#define Lx_max 195 // valore massimo accettabile di larghezza (larghezza campo) +#define LyF_min 190 // valore minimo accettabile di lunghezza sulle fasce +#define LyF_max 270 // valore massimo accettabile di lunghezza sulle fasce (lunghezza campo) +#define LyP_min 139 // valore minimo accettabile di lunghezza tra le porte +#define LyP_max 250 // valore massimo accettabile di lunghezza tra le porte// con misura x OK con us_dx o us_sx < DxF sto nelle fasce 30 + 30 - 1/2 robot + +#define DyF 91 // con misura y OK e robot al CENTER (tra le porte) con us_fx o us_px < DyP sto a NORTH o a SOUTH era - 22 +#define DyP 69 +#define DxF_Atk 48 // per attaccante, fascia centrale allargata +#define DxF_Def 48 // per portiere, fascia centrale ristretta quEASTa roba viene fatta dentro WhereAmI +#define robot 21 // diametro del robot + + +// ZONE DEL CAMPO +// codici utilizzabili per una matice 3x3 +#define EAST 2 +#define WEST 0 +#define CENTER 1 +#define NORTH 0 +#define SOUTH 2 + +#define NORTH_WEST 1 +#define NORTH_CENTER 2 +#define NORTH_EAST 3 +#define CENTER_WEST 4 +#define CENTER_CENTER 5 // codici zona nuovi +#define CENTER_EAST 6 +#define SOUTH_WEST 7 +#define SOUTH_CENTER 8 +#define SOUTH_EAST 9 + +class PositionSysZone : public PositionSystem{ + public: + PositionSysZone(); + void update() override; + void test() override; + + //returns the zone calculated + int getValue(); + + private: + int zone[3][3]; + + int zoneIndex; + int p; + int camA; + int camD; + unsigned long ao; + + int old_status_x; // posizione precedente nel campo vale EST, OVEST o CENTRO o 255 >USI FUTURI< + int old_status_y; // posizione precedente nel campo vale SUD, NORD o CENTRO o 255 >USI FUTURI< + bool good_field_x; // vedo tutta la larghezza del campo si/no + bool good_field_y; // vedo tutta la lunghezza del campo si/no + int status_x; // posizione nel campo vale EST, OVEST o CENTRO o 255 + int status_y; // posizione nel campo vale SUD, NORD o CENTRO o 255 + int guessed_x, guessed_y; + bool calcPhyZoneCam; + int DxF; // con misura y OK e robot a EST o A OVEST con us_fx o us_px < DyF sto a NORD o a SUD era - 10 + bool goal_zone; + + //calculations + void increaseIndex(int, int, int); + void increaseCol(int, int); + void increaseRow(int, int); + void increaseAll(int); + void decreaseIndex(int, int, int); + void decreaseCol(int, int); + void decreaseRow(int, int); + void decreaseAll(int); + void increaseRowWithLimit(int, int); + void increaseColWithLimit(int, int); + void decreaseRowWithLimit(int, int); + void decreaseColWithLimit(int, int); + + //reading + void readPhyZone(); + void phyZoneDirection(); + void phyZoneCam(); + void phyZoneUS(); + void phyZoneLines(); + + //testing + void testPhyZone(); + void testLogicZone(); + + //movement + void goCenter(); + void goGoalPost(); + void centerGoalPost(); + void centerGoalPostCamera(bool); + void AAANGOLO(); + int diff(int, int); + +}; \ No newline at end of file diff --git a/include/sensors.h b/include/sensors.h index 91251bd..1ef031f 100755 --- a/include/sensors.h +++ b/include/sensors.h @@ -12,11 +12,13 @@ #include "data_source_ball.h" #include "data_source_camera.h" #include "data_source_us.h" -#include "ds_ctrl_lines.h" #include "motor.h" #include "ds_ctrl.h" #include "drivecontroller.h" #include "data_source_bt.h" +#include "systems.h" +#include "linesys_2019.h" +#include "positionsys_zone.h" void initSensors(); void updateSensors(); @@ -26,7 +28,7 @@ s_extr vector lOut; s_extr vector dUs; s_extr DataSourceCtrl* usCtrl; -s_extr DSCtrlLines* linesCtrl; +s_extr LineSys2019* linesCtrl; s_extr DataSourceBNO055* compass; s_extr DataSourceBall* ball; diff --git a/include/systems.h b/include/systems.h new file mode 100644 index 0000000..50d7920 --- /dev/null +++ b/include/systems.h @@ -0,0 +1,13 @@ +#pragma once + +class LineSystem{ + public: + virtual void update() = 0; + virtual void test() = 0; +}; + +class PositionSystem{ + public: + virtual void update() = 0; + virtual void test() = 0; +}; \ No newline at end of file diff --git a/src/ds_ctrl.cpp b/src/ds_ctrl.cpp index c39dc19..07d58e1 100755 --- a/src/ds_ctrl.cpp +++ b/src/ds_ctrl.cpp @@ -23,6 +23,10 @@ void DataSourceCtrl::postProcess(){ } } +int DataSourceCtrl::getValue(int i){ + return this->ds[i]->getValue(); +} + void DataSourceCtrl::test(){ DEBUG.println("========================================"); for(DataSource* d : ds){ diff --git a/src/ds_ctrl_position.cpp b/src/ds_ctrl_position.cpp deleted file mode 100644 index e69de29..0000000 diff --git a/src/game.cpp b/src/game.cpp index dfbd250..28529fd 100644 --- a/src/game.cpp +++ b/src/game.cpp @@ -1,7 +1,14 @@ #include "game.h" Game::Game() {} +Game::Game(LineSystem* ls_, PositionSystem* ps_) { + this->ls = ls_; + this->ps = ps_; +} void Game::play(bool condition){ + ps->update(); if(condition) realPlay(); + ls->update(); + } \ No newline at end of file diff --git a/src/games.cpp b/src/games.cpp index fd07bcb..3a18f44 100644 --- a/src/games.cpp +++ b/src/games.cpp @@ -1,8 +1,10 @@ #define GAMES_CPP #include "games.h" +#include "linesys_2019.h" +#include "positionsys_zone.h" void initGames(){ - goalie = new Goalie(); - keeper = new Keeper(); + goalie = new Goalie(new LineSys2019(), new PositionSysZone()); + keeper = new Keeper(new LineSys2019(), new PositionSysZone()); } \ No newline at end of file diff --git a/src/goalie.cpp b/src/goalie.cpp index 485cc60..807e76a 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -3,8 +3,17 @@ #include "vars.h" Goalie::Goalie() : Game() { + init(); +} + +Goalie::Goalie(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_) { + init(); +} + +void Goalie::init(){ atk_speed = 0; atk_direction = 0; + } void Goalie::realPlay(){ diff --git a/src/keeper.cpp b/src/keeper.cpp index b2dd38f..c4236d4 100755 --- a/src/keeper.cpp +++ b/src/keeper.cpp @@ -4,6 +4,12 @@ #include Keeper::Keeper() : Game() { + init(); +} + +Keeper::Keeper(LineSystem* ls_, PositionSystem* ps_) : Game(ls_, ps_){} + +void Keeper::init(){ defSpeed = 0; defDir = 0; angle = 0; @@ -29,18 +35,37 @@ void Keeper::realPlay() { }else{ - angle = (90 + ball->angle) * M_PI / 180; + angle = (KEEPER_ANGLE_DX + ball->angle) * M_PI / 180; angleX = abs(cos(angle)); - if(ball->angle >= 0 && ball->angle <= 90 && camera->getValueDef(true) < 30) drive->prepareDrive(90, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); - else if(ball->angle >= 270 && ball->angle <= 360 && camera->getValueDef(true) > -30) drive->prepareDrive(270, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); - else if(ball->angle < 270 && ball->angle > 90){ + if(ball->angle >= 0 && ball->angle <= KEEPER_ANGLE_DX && camera->getValueDef(true) < 30) drive->prepareDrive(KEEPER_ANGLE_DX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); + else if(ball->angle >= KEEPER_ANGLE_SX && ball->angle <= 360 && camera->getValueDef(true) > -30) drive->prepareDrive(KEEPER_ANGLE_SX, KEEPER_BASE_VEL*angleX*KEEPER_VEL_MULT); + else if(ball->angle < KEEPER_ANGLE_SX && ball->angle > KEEPER_ANGLE_DX){ int ball_degrees2 = ball->angle > 180? ball->angle-360:ball->angle; int dir = ball_degrees2 > 0 ? ball->angle + KEEPER_BALL_BACK_ANGLE : ball->angle - KEEPER_BALL_BACK_ANGLE; dir = dir < 0? dir + 360: dir; drive->prepareDrive(dir, KEEPER_BASE_VEL); } - //centerGoalPostCamera(false); + } +} + +void Keeper::centerGoalPostCamera(bool checkBack){ + if (camera->getValueDef(true) > CENTERGOALPOST_CAM_MAX) { + drive->prepareDrive(KEEPER_ANGLE_SX, CENTERGOALPOST_VEL1); + } else if (camera->getValueDef(true) < CENTERGOALPOST_CAM_MIN) { + drive->prepareDrive(KEEPER_ANGLE_DX, CENTERGOALPOST_VEL1); + }else if(camera->getValueDef(true) > CENTERGOALPOST_CAM_MIN && camera->getValueDef(true) < CENTERGOALPOST_CAM_MAX){ + if(!ball->ballSeen) drive->prepareDrive(0, 0, 0); + if(checkBack){ + if(usCtrl->getValue(2) > CENTERGOALPOST_US_MAX){ + drive->prepareDrive(180, CENTERGOALPOST_VEL2); + } else{ + if(usCtrl->getValue(2) < CENTERGOALPOST_US_CRITIC) drive->prepareDrive(0, CENTERGOALPOST_VEL3); + + keeper_backToGoalPost = false; + keeper_tookTimer = false; + } + } } } \ No newline at end of file diff --git a/src/ds_ctrl_lines.cpp b/src/linesys_2019.cpp similarity index 92% rename from src/ds_ctrl_lines.cpp rename to src/linesys_2019.cpp index 61cbad4..25e5bdc 100755 --- a/src/ds_ctrl_lines.cpp +++ b/src/linesys_2019.cpp @@ -1,9 +1,9 @@ -#include "ds_ctrl_lines.h" +#include "linesys_2019.h" #include "sensors.h" using namespace std; -DSCtrlLines::DSCtrlLines() {} -DSCtrlLines::DSCtrlLines(vector in_, vector out_){ +LineSys2019::LineSys2019() {} +LineSys2019::LineSys2019(vector in_, vector out_){ this->in = in_; this->out = out_; @@ -24,7 +24,7 @@ DSCtrlLines::DSCtrlLines(vector in_, vector out_){ exitTimer = 0; } -void DSCtrlLines::read(){ +void LineSys2019::update(){ inV = 0; outV = 0; for(DataSource* d : in) d->readSensor(); @@ -47,9 +47,7 @@ void DSCtrlLines::read(){ } inV = inV | outV; -} -void DSCtrlLines::postProcess(){ if ((inV > 0) || (outV > 0)) { fboundsOX = true; fboundsOY = true; @@ -63,12 +61,12 @@ void DSCtrlLines::postProcess(){ outOfBounds(); } -void DSCtrlLines::outOfBounds(){ +void LineSys2019::outOfBounds(){ handleExtern(); handleIntern(); } -void DSCtrlLines::handleIntern(){ +void LineSys2019::handleIntern(){ if(fboundsX == true) { if(inV & 0x02) inVOldX = 2; @@ -162,14 +160,14 @@ void DSCtrlLines::handleIntern(){ else slow = false; } -void DSCtrlLines::handleExtern(){ +void LineSys2019::handleExtern(){ if((outV & 0b00000001) == 1) drive->vyp = 1; // esclusione if((outV & 0b00000100) == 4) drive->vyn = 1; if((outV & 0b00000010) == 2) drive->vxp = 1; if((outV & 0b00001000) == 8) drive->vxn = 1; } -void DSCtrlLines::test(){ +void LineSys2019::test(){ update(); DEBUG.print("In: "); for(DataSource* d : in){ diff --git a/src/positionsys_zone.cpp b/src/positionsys_zone.cpp new file mode 100644 index 0000000..a76f640 --- /dev/null +++ b/src/positionsys_zone.cpp @@ -0,0 +1,428 @@ +#include "positionsys_zone.h" +#include "vars.h" +#include "sensors.h" + +PositionSysZone::PositionSysZone(){ + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + zone[i][j] = 0; + } + } + zoneIndex = 0; + camA = 0; + camD = 0; + ao = 0; + p = 4; +} + +void PositionSysZone::PositionSysZone::update() { + decreaseAll(ZONE_LOOP_DECREASE_VALUE); + + readPhyZone(); + //calculates guessed_x and guessed_y and zoneIndex + //zoneIndex is just 2D to 1D of the guessed x and y + //(y_position * width + x_position) + + int top = 0; + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + if(zone[i][j] > top){ + guessed_x = i; + guessed_y = j; + top = zone[i][j]; + } + } + } + zoneIndex = guessed_y * 3 + guessed_x; +} + +int PositionSysZone::getValue(){ + return zoneIndex; +} + +void PositionSysZone::PositionSysZone::test() { + //outpus the matrix + if (millis() - ao >= 500) { + DEBUG.println("------"); + for (int i = 0; i < 4; i++) { + DEBUG.print("US: "); + usCtrl->test(); + DEBUG.print(" | "); + } + DEBUG.println(); + testPhyZone(); + testLogicZone(); + compass->test(); + + // if (comrade){ + // DEBUG.print("FriendZone: "); + // DEBUG.println(friendZone); + // } + DEBUG.println("------"); + ao = millis(); + } +} + +int zone[3][3]; + +void PositionSysZone::increaseIndex(int i, int j, int ammount){ + if(i < 3 && j < 3){ + zone[i][j] += ammount; + zone[i][j] = constrain(zone[i][j], 0, ZONE_MAX_VALUE); + } +} + +void PositionSysZone::decreaseIndex(int i, int j, int ammount){ + increaseIndex(i, j, -ammount); +} + +void PositionSysZone::increaseRow(int i, int ammount){ + if(i < 3){ + for(int a = 0; a < 3; a++){ + increaseIndex(a, i, ammount); + } + } +} + +void PositionSysZone::decreaseRow(int i, int ammount){ + increaseRow(i, -ammount); +} + +void PositionSysZone::increaseCol(int i, int ammount){ + if(i < 3){ + for(int a = 0; a < 3; a++){ + increaseIndex(i, a, ammount); + } + } +} + +void PositionSysZone::decreaseCol(int i, int ammount){ + increaseCol(i, -ammount); +} + +void PositionSysZone::increaseColWithLimit(int i, int ammount){ + if(zone[i][0] + ammount < ZONE_MAX_VALUE && zone[i][1] + ammount < ZONE_MAX_VALUE && zone[i][2] + ammount < ZONE_MAX_VALUE){ + increaseCol(i, ammount); + } +} + +void PositionSysZone::increaseRowWithLimit(int i, int ammount){ + if(zone[0][1] + ammount < ZONE_MAX_VALUE && zone[1][i] + ammount < ZONE_MAX_VALUE && zone[2][i] + ammount < ZONE_MAX_VALUE){ + increaseRow(i, ammount); + } +} + +void PositionSysZone::decreaseColWithLimit(int i, int ammount){ + if(zone[i][0] - ammount >= 0 && zone[i][1] - ammount >= 0 && zone[i][2] - ammount >= 0){ + decreaseCol(i, ammount); + } +} + +void PositionSysZone::decreaseRowWithLimit(int i, int ammount){ + if(zone[0][i] - ammount >= 0 && zone[1][i] - ammount >= 0 && zone[2][i] - ammount >= 0){ + decreaseRow(i, ammount); + } +} + +void PositionSysZone::increaseAll(int val){ + //decrease all + for(int i = 0; i < 3; i++){ + for(int j = 0; j < 3; j++){ + increaseIndex(i, j, val); + } + } +} + +void PositionSysZone::decreaseAll(int val){ + increaseAll(-val); +} + + +void PositionSysZone::readPhyZone(){ + phyZoneUS(); + phyZoneCam(); + phyZoneLines(); + // phyZoneDirection(); +} + +//old WhereAmI. Renamed to be coerent. Now also adds to the logic zone +void PositionSysZone::phyZoneUS(){ + // decide la posizione in orizzontale e verticale + // Aggiorna i flag : good_field_x good_field_y non utilizzata da altre + // routines + // goal_zone non utilizzata da altre + // routines + // Aggiorna le variabili: + // status_x (con valori CENTER EAST WEST 255 = non lo so) + // status_y (con valori CENTER NORTH SOUTH 255 = non lo so) + + int Lx_mis; // larghezza totale stimata dalle misure + int Ly_mis; // lunghezza totale stimata dalle misure + int Ly_min; // Limite inferiore con cui confrontare la misura y + int Ly_max; // Limite inferiore con cui confrontare la misura y + int Dy; // Limite per decidere NORTH SOUTH in funzione della posizione + // orizzontale + + old_status_x = status_x; + old_status_y = status_y; + good_field_x = false; // non é buona x + good_field_y = false; // non é buona y + goal_zone = false; // non sono davanti alla porta avversaria + + if (role == HIGH) + DxF = DxF_Atk; + else + DxF = DxF_Def; + + Lx_mis = usCtrl->getValue(1) + usCtrl->getValue(3) + robot; // larghezza totale stimata + Ly_mis = usCtrl->getValue(0) + usCtrl->getValue(2) + robot; // lunghezza totale stimata + + // controllo orizzontale + if ((Lx_mis < Lx_max) && (Lx_mis > Lx_min) && (usCtrl->getValue(1) > 25) && (usCtrl->getValue(3) > 25)) { + // se la misura orizzontale é accettabile + good_field_x = true; + status_x = CENTER; + if (usCtrl->getValue(1) < DxF) // robot é vicino al bordo dEASTro + status_x = EAST; + if (usCtrl->getValue(3) < DxF) // robot é vicino al bordo sinistro + status_x = WEST; + + if (status_x == CENTER) { + // imposto limiti di controllo lunghezza verticale tra le porte + Ly_min = LyP_min; + Ly_max = LyP_max; + Dy = DyP; + } else { + // imposto limiti di controllo lunghezza verticale in fascia + Ly_min = LyF_min; + Ly_max = LyF_max; + Dy = DyF; + } + } else { + // la misura non é pulita per la presenza di un ostacolo + if ((usCtrl->getValue(1) >= (DxF + 10)) || (usCtrl->getValue(3) >= (DxF + 10))) { + // se ho abbastanza spazio a dEASTra o a sinistra + // devo stare per forza al cento + status_x = CENTER; + // imposto limiti di controllo lunghezza verticale tra le porte + Ly_min = LyP_min; + Ly_max = LyP_max; + Dy = DyP; + } else { + status_x = 255; + // non so la coordinata x + // imposto i limiti di controllo verticale in base alla posizione + // orizzontale precedente + if (old_status_x == CENTER) { + // controlla la posizione precedente per decidere limiti di controllo y + // imposto limiti di controllo lunghezza verticale tra le porte + Ly_min = LyP_min; + Ly_max = LyP_max; + Dy = DyP; + } else { + // imposto limiti di controllo lunghezza verticale in fascia anche per x + // incognita + Ly_min = LyF_min; + Ly_max = LyF_max; + Dy = DyF; + } + } + } + // controllo verticale + if ((Ly_mis < Ly_max) && (Ly_mis > Ly_min)) { + // se la misura verticale é accettabile + good_field_y = true; + status_y = CENTER; + if (usCtrl->getValue(0) < Dy) { + status_y = NORTH; // robot é vicino alla porta avversaria + if (Dy == DyP) + goal_zone = true; // davanti alla porta in zona goal + } + if (usCtrl->getValue(2) < Dy) + status_y = SOUTH; // robot é vicino alla propria porta + } else { + // la misura non é pulita per la presenza di un ostacolo + status_y = 255; // non so la coordinata y + if (usCtrl->getValue(0) >= (Dy + 0)) + status_y = CENTER; // ma se ho abbastanza spazio dietro o avanti + if (usCtrl->getValue(2) >= (Dy + 0)) + status_y = CENTER; // e'probabile che stia al CENTER + } + + //now operates on the matrix + if (status_x == 255 && status_y != 255) { + increaseRow(status_y, ZONE_US_UNKNOWN_INCREASE_VALUE); + } else if (status_x != 255 && status_y == 255) { + increaseCol(status_x, ZONE_US_UNKNOWN_INCREASE_VALUE); + } else { + increaseIndex(status_x, status_y, ZONE_US_INDEX_INCREASE_VALUE); + } +} + +void PositionSysZone::phyZoneCam(){ + + //IMU-fixed attack angle + camA = camera->getValueAtk(true); + //IMU-fixed defence angle + camD = camera->getValueDef(true); + + //Negative angle means that the robot is positioned to the right of the goalpost + //Positive angle means that the robot is positioned to the left of the goalpost + + p = 4; + + if(abs(diff(camA, camD)) <= ZONE_CAM_CENTER_RANGE){ + //at center row, you can consider both camA and camD + p = 1; + }else if(camA > camD){ + p = 0; + }else if(camD > camA){ + p = 2; + } + + increaseColWithLimit(p, ZONE_CAM_INCREASE_VALUE); + + calcPhyZoneCam = false; +} + + + +void PositionSysZone::phyZoneLines(){ +// //ZONE_LINES_ERROR_VALUE is a random error code not used in line exit direction calculations +// if(lineSensByteBak != ZONE_LINES_ERROR_VALUE){ +// switch(lineSensByteBak) { +// case 1: //NORTH +// increaseRow(0, ZONE_LINES_INCREASE_VALUE); +// decreaseRow(1, ZONE_LINES_INCREASE_VALUE); +// decreaseRow(2, ZONE_LINES_INCREASE_VALUE); +// break; + +// case 2: //EAST +// decreaseCol(0, ZONE_LINES_INCREASE_VALUE); +// decreaseCol(1, ZONE_LINES_INCREASE_VALUE); +// increaseCol(2, ZONE_LINES_INCREASE_VALUE); +// break; + +// case 4: //SOUTH +// decreaseRow(0, ZONE_LINES_INCREASE_VALUE); +// decreaseRow(1, ZONE_LINES_INCREASE_VALUE); +// decreaseRow(2, ZONE_LINES_INCREASE_VALUE); +// break; + +// case 8: //WEST +// increaseCol(0, ZONE_LINES_INCREASE_VALUE); +// decreaseCol(1, ZONE_LINES_INCREASE_VALUE); +// decreaseCol(2, ZONE_LINES_INCREASE_VALUE); +// break; + +// case 3: +// decreaseAll(ZONE_LINES_INCREASE_VALUE); +// increaseIndex(0, 2, 2*ZONE_LINES_INCREASE_VALUE); +// break; + +// case 6: +// decreaseAll(ZONE_LINES_INCREASE_VALUE); +// increaseIndex(2, 2, 2*ZONE_LINES_INCREASE_VALUE); +// break; + +// case 9: +// decreaseAll(ZONE_LINES_INCREASE_VALUE); +// increaseIndex(0, 0, 2*ZONE_LINES_INCREASE_VALUE); +// break; + +// default: +// break; +// } +// //Last thing to do, sets the var to an error code, so next time it will be called will be because of the outOfBounds function being called +// lineSensByteBak = ZONE_LINES_ERROR_VALUE; +// } +} + + +void PositionSysZone::testPhyZone(){ + updateSensors(); + + readPhyZone(); + DEBUG.print("Measured US location:\t"); + DEBUG.print(status_x); + DEBUG.print(" | "); + DEBUG.println(status_y); + DEBUG.print("Measured Cam Column (4 is error):\t"); + DEBUG.println(p); +} + +void PositionSysZone::testLogicZone(){ + update(); + DEBUG.println("-----------------"); + + for (int j = 0; j < 3; j++) { + for (int i = 0; i < 3; i++) { + DEBUG.print(zone[i][j]); + DEBUG.print(" | "); + } + DEBUG.println(); + } + DEBUG.println("-----------------"); + DEBUG.print("Guessed location:\t"); + DEBUG.print(guessed_x); + DEBUG.print(" | "); + DEBUG.println(guessed_y); + DEBUG.print("Zone Index: "); + DEBUG.println(zoneIndex); +} + + +void PositionSysZone::goCenter() { + if (zoneIndex == 8) + drive->prepareDrive(330, GOCENTER_VEL); + if (zoneIndex == 7) + drive->prepareDrive(0, GOCENTER_VEL); + if (zoneIndex == 6) + drive->prepareDrive(45, GOCENTER_VEL); + if (zoneIndex == 5) + drive->prepareDrive(270, GOCENTER_VEL); + if (zoneIndex == 4) + drive->prepareDrive(0, 0); + if (zoneIndex == 3) + drive->prepareDrive(90, GOCENTER_VEL); + if (zoneIndex == 2) + drive->prepareDrive(255, GOCENTER_VEL); + if (zoneIndex == 1) + drive->prepareDrive(180, GOCENTER_VEL); + if (zoneIndex == 0) + drive->prepareDrive(135, GOCENTER_VEL); +} + + +// int vel = 160; +// int usDist = 70; +// void PositionSysZone::centerGoalPost() { +// x = 1; +// y = 1; +// int vel = 255; +// if ((zoneIndex >= 0 && zoneIndex <= 2) || zoneIndex == 4) { +// drive->prepareDrive(180, vel); +// } else if (zoneIndex == 3 || zoneIndex == 6) { +// drive->prepareDrive(90, vel); +// } else if (zoneIndex == 5 || zoneIndex == 8) { +// drive->prepareDrive(270, vel); +// } else { +// stop_menamoli = false; +// if (usCtrl->getValue(2) >= 25) +// drive->prepareDrive(180, vel); +// else +// drive->prepareDrive(0, 0); +// } +// } + + +void PositionSysZone::AAANGOLO() { + if((usCtrl->getValue(2) <= 45) && ((usCtrl->getValue(1) <= 50) || (usCtrl->getValue(3) <= 50))) drive->prepareDrive(0, 350, 0); +} + +int PositionSysZone::diff(int a, int b){ + int diffB = abs(min(a, b) - max(a, b)); + int diffB1 = 360-diffB; + int diff = min(diffB, diffB1); + return diff; +} \ No newline at end of file diff --git a/src/sensors.cpp b/src/sensors.cpp index 3597fe6..0b46aaa 100755 --- a/src/sensors.cpp +++ b/src/sensors.cpp @@ -16,13 +16,9 @@ void initSensors(){ ball = new DataSourceBall(&Serial4, 57600); camera = new DataSourceCamera(&Serial2, 19200); usCtrl = new DataSourceCtrl(dUs); - linesCtrl = new DSCtrlLines(lIn, lOut); + linesCtrl = new LineSys2019(lIn, lOut); bt = new DataSourceBT(&Serial3, 115200); - - /*game = new Game(); - goalie = new Goalie(); - keeper = new Keeper();*/ -} + } void updateSensors(){ role = digitalRead(SWITCH_DX);