reimproted the zone position system

.vscode/extensions.json

@@ -1,7 +1,7 @@
-{
+{
-	// See http://go.microsoft.com/fwlink/?LinkId=827846
+	// See http://go.microsoft.com/fwlink/?LinkId=827846
-	// for the documentation about the extensions.json format
+	// for the documentation about the extensions.json format
-	"recommendations": [
+	"recommendations": [
-		"platformio.platformio-ide"
+		"platformio.platformio-ide"
-	]
+	]
 }

include/data_source_controller.h

@@ -1,23 +0,0 @@
-#pragma once
-#include "data_source.h"
-#include "vars.h"
-#include <Arduino.h>
-#include <vector>
-
-using namespace std;
-class DataSourceController {
-
-    public:
-        DataSourceController();
-        DataSourceController(vector<DataSource*>);
-
-    public:
-        void update();
-        void test();
-        void postProcess();
-        void readSensor();
-        void getValue();
-
-        vector<DataSource*> ds;
-
-};

include/ds_ctrl.h

@@ -17,6 +17,7 @@ class DataSourceCtrl {
         virtual void test();
         virtual void postProcess();
         virtual void read();
+        int getValue(int i);
 
         vector<DataSource*> ds;
 

include/ds_ctrl_position.h

@@ -1,10 +0,0 @@
-#include "vars.h"
-#include "ds_ctrl.h"
-#include "ds_ctrl_lines.h"
-
-class DSCtrlPosition : public DataSourceController {
-
-    public:
-        DSCtrlPosition();
-
-};

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;
 };

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;
+
+    
 };

include/keeper.h

@@ -2,22 +2,35 @@
 
 #include "game.h"
 
-//KEEPER
+#define KEEPER_ATTACK_DISTANCE 190
-#define KEEPER_ATTACK_DISTANCE 200
 #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;
 

include/line_system.h

@@ -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;
-};

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 <Arduino.h>
 
@@ -16,19 +17,17 @@
 #define LINE_THRESH 90
 #define EXTIME 100
 
-class DSCtrlLines : public DataSourceCtrl{
+class LineSys2019 : public LineSystem{
 
     public:
-        DSCtrlLines();
+        LineSys2019();
-        DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> out);
+        LineSys2019(vector<DataSource*> in_, vector<DataSource*> out);
 
-        void read() override;
+        void update() override;
-        void postProcess() override;
+        void test() override;
         void outOfBounds();
         void handleIntern();
         void handleExtern();
-        int getValue();
-        void test() override;
         
     private:
         vector<DataSource*> in, out;

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);
+
+};

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<DataSource*> lOut;
 s_extr vector<DataSource*> dUs;
 
 s_extr DataSourceCtrl* usCtrl;
-s_extr DSCtrlLines* linesCtrl;
+s_extr LineSys2019* linesCtrl;
 
 s_extr DataSourceBNO055* compass;
 s_extr DataSourceBall* ball;

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;
+};

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){

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();
+
 }

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();
+    goalie = new Goalie(new LineSys2019(), new PositionSysZone());
-    keeper = new Keeper();
+    keeper = new Keeper(new LineSys2019(), new PositionSysZone());
 }

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(){

src/keeper.cpp

@@ -4,6 +4,12 @@
 #include <Arduino.h>
 
 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);
+        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 >= 270 && ball->angle <= 360  && camera->getValueDef(true) > -30) drive->prepareDrive(270, 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 < 270 && ball->angle > 90){
+        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;
+            }
+        }
     }
 }

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() {}
+LineSys2019::LineSys2019() {}
-DSCtrlLines::DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> out_){
+LineSys2019::LineSys2019(vector<DataSource*> in_, vector<DataSource*> out_){
     this->in = in_;
     this->out = out_;
 
@@ -24,7 +24,7 @@ DSCtrlLines::DSCtrlLines(vector<DataSource*> in_, vector<DataSource*> 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){

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;
+}

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);