diff --git a/.vscode/settings.json b/.vscode/settings.json
index ec0f11d..4f2a5b6 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -20,6 +20,29 @@
         "vector": "cpp",
         "string_view": "cpp",
         "initializer_list": "cpp",
-        "*.tcc": "cpp"
+        "*.tcc": "cpp",
+        "cctype": "cpp",
+        "clocale": "cpp",
+        "cmath": "cpp",
+        "cstdarg": "cpp",
+        "cstdint": "cpp",
+        "cstdio": "cpp",
+        "cstdlib": "cpp",
+        "cstring": "cpp",
+        "cwchar": "cpp",
+        "cwctype": "cpp",
+        "exception": "cpp",
+        "fstream": "cpp",
+        "iosfwd": "cpp",
+        "iostream": "cpp",
+        "istream": "cpp",
+        "limits": "cpp",
+        "new": "cpp",
+        "ostream": "cpp",
+        "sstream": "cpp",
+        "stdexcept": "cpp",
+        "streambuf": "cpp",
+        "cinttypes": "cpp",
+        "typeinfo": "cpp"
     }
 }
\ No newline at end of file
diff --git a/include/README b/include/README
deleted file mode 100644
index 194dcd4..0000000
--- a/include/README
+++ /dev/null
@@ -1,39 +0,0 @@
-
-This directory is intended for project header files.
-
-A header file is a file containing C declarations and macro definitions
-to be shared between several project source files. You request the use of a
-header file in your project source file (C, C++, etc) located in `src` folder
-by including it, with the C preprocessing directive `#include'.
-
-```src/main.c
-
-#include "header.h"
-
-int main (void)
-{
- ...
-}
-```
-
-Including a header file produces the same results as copying the header file
-into each source file that needs it. Such copying would be time-consuming
-and error-prone. With a header file, the related declarations appear
-in only one place. If they need to be changed, they can be changed in one
-place, and programs that include the header file will automatically use the
-new version when next recompiled. The header file eliminates the labor of
-finding and changing all the copies as well as the risk that a failure to
-find one copy will result in inconsistencies within a program.
-
-In C, the usual convention is to give header files names that end with `.h'.
-It is most portable to use only letters, digits, dashes, and underscores in
-header file names, and at most one dot.
-
-Read more about using header files in official GCC documentation:
-
-* Include Syntax
-* Include Operation
-* Once-Only Headers
-* Computed Includes
-
-https://gcc.gnu.org/onlinedocs/cpp/Header-Files.html
diff --git a/include/data_source_controller.h b/include/data_source_controller.h
deleted file mode 100644
index 90b6c3c..0000000
--- a/include/data_source_controller.h
+++ /dev/null
@@ -1,22 +0,0 @@
-#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;
-
-};
\ No newline at end of file
diff --git a/include/drivecontroller.h b/include/drivecontroller.h
index 93dc6eb..87785e4 100644
--- a/include/drivecontroller.h
+++ b/include/drivecontroller.h
@@ -4,10 +4,10 @@
 #include "motor.h"
 
 //PID Constants
-#define KP 0.9
-#define KI 0 
-#define KD 1.1
-#define DEADZONE_MIN 25
+
+#define KP 1.2    
+#define KI 0   
+#define KD 0.7 
 
 class DriveController{
 
@@ -21,6 +21,10 @@ class DriveController{
         float updatePid();
         float torad(float f);
 
+        int vxp, vyp, vxn, vyn;
+        bool canUnlock;
+        elapsedMillis unlockTime;
+
     private:
         Motor* m1;
         Motor* m2;
diff --git a/include/ds_ctrl.h b/include/ds_ctrl.h
new file mode 100644
index 0000000..4db4910
--- /dev/null
+++ b/include/ds_ctrl.h
@@ -0,0 +1,23 @@
+#pragma once
+
+#include "data_source.h"
+#include "vars.h"
+#include <Arduino.h>
+#include <vector>
+
+using namespace std;
+class DataSourceCtrl {
+
+    public:
+        DataSourceCtrl();
+        DataSourceCtrl(vector<DataSource*>);
+
+    public:
+        virtual void update();
+        virtual void test();
+        virtual void postProcess();
+        virtual void read();
+
+        vector<DataSource*> ds;
+
+};
\ No newline at end of file
diff --git a/include/ds_ctrl_lines.h b/include/ds_ctrl_lines.h
new file mode 100644
index 0000000..fa330fd
--- /dev/null
+++ b/include/ds_ctrl_lines.h
@@ -0,0 +1,40 @@
+#pragma once
+#include "ds_ctrl.h"
+#include "data_source.h"
+#include "vars.h" 
+#include <Arduino.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 90
+#define EXTIME 100
+
+class DataSourceCtrlLines : public DataSourceCtrl {
+
+    public:
+        DataSourceCtrlLines();
+        DataSourceCtrlLines(vector<DataSource*> in_, vector<DataSource*> out);
+
+        void read() override;
+        void postProcess() override;
+        void outOfBounds();
+        void handleIntern();
+        void handleExtern();
+        int getValue();
+        void test() override;
+        
+    private:
+        vector<DataSource*> in, out;
+        DataSource* ds;
+        bool fboundsX, fboundsY, fboundsOX, fboundsOY, slow;
+        int inV, outV, inVOldX, inVOldY, value, linetriggerI[4], linetriggerO[4], i;
+        elapsedMillis exitTimer;
+        int outDir, outVel;
+};
\ No newline at end of file
diff --git a/include/sensors.h b/include/sensors.h
index b5e1ad1..3057e06 100644
--- a/include/sensors.h
+++ b/include/sensors.h
@@ -3,8 +3,9 @@
 #include "data_source_ball.h"
 #include "data_source_camera.h"
 #include "data_source_us.h"
+#include "ds_ctrl_lines.h"
 #include "motor.h"
-#include "data_source_controller.h"
+#include "ds_ctrl.h"
 #include "drivecontroller.h"
 
 #ifdef SENSORS_CPP
@@ -19,6 +20,10 @@ void updateSensors();
 extr DataSource* compass;
 extr DataSource* ball;
 extr DataSource* camera;
-//extr DataSource* us;
-extr DataSourceController* usCtrl;
+extr DataSourceCtrl* usCtrl;
+extr DataSourceCtrlLines* linesCtrl;
 extr DriveController* drive;
+
+extr vector<DataSource*> lIn;
+extr vector<DataSource*> lOut;
+extr vector<DataSource*> dUs;
diff --git a/include/vars.h b/include/vars.h
index cfb863d..e8fec13 100644
--- a/include/vars.h
+++ b/include/vars.h
@@ -1,8 +1,10 @@
 #pragma once
-#define DEBUG_PRINT Serial
+#define DEBUG Serial
+
 #define LED_R 20
 #define LED_Y 17
 #define LED_G 13
+
 #define BUZZER 30
 #define SWITCH_SX 28
 #define SWITCH_DX 29
\ No newline at end of file
diff --git a/lib/Adafruit-GFX/.gitignore b/lib/Adafruit-GFX/.gitignore
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/.travis.yml b/lib/Adafruit-GFX/.travis.yml
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Adafruit_GFX.cpp b/lib/Adafruit-GFX/Adafruit_GFX.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Adafruit_GFX.h b/lib/Adafruit-GFX/Adafruit_GFX.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Adafruit_SPITFT.cpp b/lib/Adafruit-GFX/Adafruit_SPITFT.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Adafruit_SPITFT.h b/lib/Adafruit-GFX/Adafruit_SPITFT.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Adafruit_SPITFT_Macros.h b/lib/Adafruit-GFX/Adafruit_SPITFT_Macros.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMono12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMono12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMono18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMono18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMono24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMono24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMono9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMono9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBold12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBold12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBold18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBold18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBold24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBold24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBold9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBold9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoBoldOblique9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoOblique12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoOblique12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoOblique18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoOblique18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoOblique24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoOblique24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeMonoOblique9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeMonoOblique9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSans12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSans12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSans18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSans18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSans24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSans24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSans9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSans9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBold12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBold12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBold18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBold18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBold24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBold24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBold9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBold9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansBoldOblique9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansOblique12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansOblique12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansOblique18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansOblique18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansOblique24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansOblique24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSansOblique9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSansOblique9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerif12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerif12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerif18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerif18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerif24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerif24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerif9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerif9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBold12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBold12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBold18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBold18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBold24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBold24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBold9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBold9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifBoldItalic9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifItalic12pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifItalic12pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifItalic18pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifItalic18pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifItalic24pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifItalic24pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/FreeSerifItalic9pt7b.h b/lib/Adafruit-GFX/Fonts/FreeSerifItalic9pt7b.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/Org_01.h b/lib/Adafruit-GFX/Fonts/Org_01.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/Picopixel.h b/lib/Adafruit-GFX/Fonts/Picopixel.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/Tiny3x3a2pt7b b/lib/Adafruit-GFX/Fonts/Tiny3x3a2pt7b
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/Fonts/TomThumb.h b/lib/Adafruit-GFX/Fonts/TomThumb.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/README.md b/lib/Adafruit-GFX/README.md
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/examples/mock_ili9341/mock_ili9341.ino b/lib/Adafruit-GFX/examples/mock_ili9341/mock_ili9341.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/fontconvert/Makefile b/lib/Adafruit-GFX/fontconvert/Makefile
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/fontconvert/fontconvert.c b/lib/Adafruit-GFX/fontconvert/fontconvert.c
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/fontconvert/fontconvert_win.md b/lib/Adafruit-GFX/fontconvert/fontconvert_win.md
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/fontconvert/makefonts.sh b/lib/Adafruit-GFX/fontconvert/makefonts.sh
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/gfxfont.h b/lib/Adafruit-GFX/gfxfont.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/glcdfont.c b/lib/Adafruit-GFX/glcdfont.c
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/library.properties b/lib/Adafruit-GFX/library.properties
old mode 100755
new mode 100644
diff --git a/lib/Adafruit-GFX/license.txt b/lib/Adafruit-GFX/license.txt
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.cpp b/lib/Adafruit_BNO055/Adafruit_BNO055.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/Adafruit_BNO055.h b/lib/Adafruit_BNO055/Adafruit_BNO055.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/OBJLoader/OBJLoader.txt b/lib/Adafruit_BNO055/OBJLoader/OBJLoader.txt
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/OBJLoader/OBJLoader.zip b/lib/Adafruit_BNO055/OBJLoader/OBJLoader.zip
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/README.md b/lib/Adafruit_BNO055/README.md
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/bunny/bunny.ino b/lib/Adafruit_BNO055/examples/bunny/bunny.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/cuberotate.pde b/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/cuberotate.pde
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.mtl b/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.mtl
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.obj b/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/data/bunny.obj
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/serialconfig.txt b/lib/Adafruit_BNO055/examples/bunny/processing/cuberotate/serialconfig.txt
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/rawdata/rawdata.ino b/lib/Adafruit_BNO055/examples/rawdata/rawdata.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/restore_offsets/restore_offsets.ino b/lib/Adafruit_BNO055/examples/restore_offsets/restore_offsets.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/examples/sensorapi/sensorapi.ino b/lib/Adafruit_BNO055/examples/sensorapi/sensorapi.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/library.properties b/lib/Adafruit_BNO055/library.properties
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/utility/imumaths.h b/lib/Adafruit_BNO055/utility/imumaths.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/utility/matrix.h b/lib/Adafruit_BNO055/utility/matrix.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/utility/quaternion.h b/lib/Adafruit_BNO055/utility/quaternion.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_BNO055/utility/vector.h b/lib/Adafruit_BNO055/utility/vector.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_Sensor-master/.gitignore b/lib/Adafruit_Sensor-master/.gitignore
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_Sensor-master/Adafruit_Sensor.h b/lib/Adafruit_Sensor-master/Adafruit_Sensor.h
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_Sensor-master/README.md b/lib/Adafruit_Sensor-master/README.md
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_Sensor-master/library.properties b/lib/Adafruit_Sensor-master/library.properties
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/README.md b/lib/Adafruit_VL53L0X/README.md
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/doxyfile b/lib/Adafruit_VL53L0X/doxyfile
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/examples/vl53l0x/vl53l0x.ino b/lib/Adafruit_VL53L0X/examples/vl53l0x/vl53l0x.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/examples/vl53l0x_oled/vl53l0x_oled.ino b/lib/Adafruit_VL53L0X/examples/vl53l0x_oled/vl53l0x_oled.ino
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/library.properties b/lib/Adafruit_VL53L0X/library.properties
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api.cpp b/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_calibration.cpp b/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_calibration.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_core.cpp b/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_core.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_ranging.cpp b/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_ranging.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_strings.cpp b/lib/Adafruit_VL53L0X/src/core/src/vl53l0x_api_strings.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/platform/src/vl53l0x_i2c_comms.cpp b/lib/Adafruit_VL53L0X/src/platform/src/vl53l0x_i2c_comms.cpp
old mode 100755
new mode 100644
diff --git a/lib/Adafruit_VL53L0X/src/platform/src/vl53l0x_platform.cpp b/lib/Adafruit_VL53L0X/src/platform/src/vl53l0x_platform.cpp
old mode 100755
new mode 100644
diff --git a/src/data_source.cpp b/src/data_source.cpp
index e629df8..a4d65b3 100644
--- a/src/data_source.cpp
+++ b/src/data_source.cpp
@@ -2,6 +2,8 @@
 
 DataSource::DataSource(void){
     protocol = P_NULL;
+    
+    value = 0;
 }
 
 DataSource::DataSource(TwoWire* i2c_, int addr){
@@ -11,12 +13,16 @@ DataSource::DataSource(TwoWire* i2c_, int addr){
 
     this->i2c->end();
     this->i2c->begin();
+
+    value = 0;
 }
 
 DataSource::DataSource(HardwareSerial* ser_, int baud){
     protocol = P_RXTX;
     this->ser = ser_;    
     ser->begin(baud);
+    
+    value = 0;
 }
 
 DataSource::DataSource(int pin_, bool analog){
@@ -24,8 +30,10 @@ DataSource::DataSource(int pin_, bool analog){
     if(analog) protocol = P_APIN;
     else {
         protocol = P_DPIN;
-        digitalWrite(pin, OUTPUT);
+        pinMode(pin, INPUT);
     }
+    
+    value = 0;
 }
 
 int DataSource::getValue(){
@@ -40,13 +48,13 @@ void DataSource::update(){
 void DataSource::postProcess(){ }
 
 void DataSource::readSensor(){
-    if(protocol == 1) value = i2c->read();
-    else if(protocol == 2) while(ser->available() > 0) value = ser->read();
-    else if(protocol == 3) analogRead(pin);
-    else if(protocol == 4) digitalRead(pin);
+    if(protocol == P_I2C) value = i2c->read();
+    else if(protocol == P_RXTX) while(ser->available() > 0) value = ser->read();
+    else if(protocol == P_APIN) value = analogRead(pin);
+    else if(protocol == P_DPIN) value = digitalRead(pin);
 }
 
 void DataSource::test(){
     this->update();
-    DEBUG_PRINT.println(this->getValue());
+    DEBUG.println(this->getValue());
 }
\ No newline at end of file
diff --git a/src/data_source_ball.cpp b/src/data_source_ball.cpp
index 83dda85..89c99aa 100644
--- a/src/data_source_ball.cpp
+++ b/src/data_source_ball.cpp
@@ -2,6 +2,9 @@
 #include "vars.h"
 
 DataSourceBall :: DataSourceBall(HardwareSerial* ser_, int baud) : DataSource(ser_, baud) {
+  ballSeen = false;
+  distance = 0;
+  angle = 0;
 }
 
 void DataSourceBall :: postProcess(){
@@ -16,10 +19,10 @@ void DataSourceBall :: postProcess(){
 void DataSourceBall :: test(){
     this->update();
     if(ballSeen){
-    DEBUG_PRINT.print(angle); 
-    DEBUG_PRINT.print(" | ");
-    DEBUG_PRINT.println(distance); 
+    DEBUG.print(angle); 
+    DEBUG.print(" | ");
+    DEBUG.println(distance); 
     }else{
-    DEBUG_PRINT.println("Not seeing ball");
+    DEBUG.println("Not seeing ball");
     }
 }
\ No newline at end of file
diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index d4f2bd6..887f841 100644
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -122,14 +122,14 @@ void DataSourceCamera :: test(){
     goalOrientation = digitalRead(SWITCH_SX);     //se HIGH attacco gialla, difendo blu
     
 
-    DEBUG_PRINT.print(pAtk);
-    DEBUG_PRINT.print(" | ");
-    DEBUG_PRINT.print(fixCamIMU(pAtk));
-    DEBUG_PRINT.print(" --- ");
+    DEBUG.print(pAtk);
+    DEBUG.print(" | ");
+    DEBUG.print(fixCamIMU(pAtk));
+    DEBUG.print(" --- ");
 
-    DEBUG_PRINT.print(pDef);
-    DEBUG_PRINT.print(" | ");
-    DEBUG_PRINT.println(fixCamIMU(pDef));
+    DEBUG.print(pDef);
+    DEBUG.print(" | ");
+    DEBUG.println(fixCamIMU(pDef));
     delay(100);
 }
 
diff --git a/src/data_source_controller.cpp b/src/data_source_controller.cpp
deleted file mode 100644
index 88db1f8..0000000
--- a/src/data_source_controller.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-#include "data_source_controller.h"
-
-using namespace std;
-
-DataSourceController::DataSourceController() {}
-DataSourceController::DataSourceController(vector<DataSource*> ds_){
-    this->ds = ds_;
-}
-
-void DataSourceController::readSensor(){
-    for(DataSource* d : ds){
-        d->readSensor();
-    }
-}
-void DataSourceController::update(){
-    for(DataSource* d : ds){
-        d->update();
-    }
-}
-void DataSourceController::postProcess(){
-    for(DataSource* d : ds){
-        d->postProcess();
-    }
-}
-void DataSourceController::getValue(){
-    for(DataSource* d : ds){
-        d->getValue();
-    }
-}
-void DataSourceController::test(){
-    DEBUG_PRINT.println("========================================");
-    for(DataSource* d : ds){
-        d->test();
-    }
-    DEBUG_PRINT.println("========================================");
-
-}
\ No newline at end of file
diff --git a/src/data_source_us.cpp b/src/data_source_us.cpp
index a214de9..63a325c 100644
--- a/src/data_source_us.cpp
+++ b/src/data_source_us.cpp
@@ -3,6 +3,7 @@
 
 DataSourceUS::DataSourceUS(TwoWire* i2c_, int addr) : DataSource(i2c_, addr){
   us_flag = false;
+  value = 0;
 }
 
 void DataSourceUS::test(){
@@ -45,7 +46,7 @@ void DataSourceUS::test(){
       value = reading;
     }
 
-  DEBUG_PRINT.println(value);
+  DEBUG.println(value);
 }
 
 void DataSourceUS::readSensor(){
@@ -93,5 +94,4 @@ void DataSourceUS::usReceive() {
     // receive low byte as lower 8 bit
     reading |= i2c->read();
     value = reading;
-    DEBUG_PRINT.println(reading);
   }
\ No newline at end of file
diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp
index 85fa612..6d1d526 100644
--- a/src/drivecontroller.cpp
+++ b/src/drivecontroller.cpp
@@ -11,6 +11,23 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
         sins[i] = (float) sin(torad(i));
         cosins[i] =  (float) cos(torad(i));
     }
+
+    pDir = 0;
+    pSpeed = 0;
+    pTilt = 0;
+
+    vx = 0;
+    vy = 0;
+
+    speed1 = 0;
+    speed2 = 0;
+    speed3 = 0;
+    speed4 = 0;
+
+    delta = 0;
+    errorP = 0;
+    errorI = 0;
+    errorD = 0;
 }
 
 void DriveController::prepareDrive(int dir, int speed, int tilt){
@@ -62,18 +79,7 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed1 = constrain(speed1, -255, 255);
     speed2 = constrain(speed2, -255, 255);
     speed3 = constrain(speed3, -255, 255);
-    speed4 = constrain(speed4, -255, 255);
-
-    //-almost- eliminating motor deadzones, should increase motor efficiency
-    speed1 = (int)speed1 > 0 ? map((int)speed1, 1, 255, DEADZONE_MIN, 255) : speed1;        
-    speed2 = (int)speed2 > 0 ? map((int)speed2, 1, 255, DEADZONE_MIN, 255) : speed2;        
-    speed3 = (int)speed3 > 0 ? map((int)speed3, 1, 255, DEADZONE_MIN, 255) : speed3;        
-    speed4 = (int)speed4 > 0 ? map((int)speed4, 1, 255, DEADZONE_MIN, 255) : speed4;        
-
-    speed1 = (int)speed1 < 0 ? map((int)speed1, -255, -1, -255, -DEADZONE_MIN) : speed1;        
-    speed2 = (int)speed2 < 0 ? map((int)speed2, -255, -1, -255, -DEADZONE_MIN) : speed2;        
-    speed3 = (int)speed3 < 0 ? map((int)speed3, -255, -1, -255, -DEADZONE_MIN) : speed3;        
-    speed4 = (int)speed4 < 0 ? map((int)speed4, -255, -1, -255, -DEADZONE_MIN) : speed4;        
+    speed4 = constrain(speed4, -255, 255);     
 
     m1->drive((int) speed1);
     m2->drive((int) speed2);
diff --git a/src/ds_ctrl.cpp b/src/ds_ctrl.cpp
new file mode 100644
index 0000000..c39dc19
--- /dev/null
+++ b/src/ds_ctrl.cpp
@@ -0,0 +1,33 @@
+#include "ds_ctrl.h"
+
+using namespace std;
+
+DataSourceCtrl::DataSourceCtrl() {}
+DataSourceCtrl::DataSourceCtrl(vector<DataSource*> ds_){
+    this->ds = ds_;
+}
+
+void DataSourceCtrl::read(){
+    for(DataSource* d : ds){
+        d->readSensor();
+    }
+}
+void DataSourceCtrl::update(){
+    read();
+    postProcess();
+}
+
+void DataSourceCtrl::postProcess(){
+    for(DataSource* d : ds){
+        d->postProcess();
+    }
+}
+
+void DataSourceCtrl::test(){
+    DEBUG.println("========================================");
+    for(DataSource* d : ds){
+        d->test();
+    }
+    DEBUG.println("========================================");
+
+}
\ No newline at end of file
diff --git a/src/ds_ctrl_lines.cpp b/src/ds_ctrl_lines.cpp
new file mode 100644
index 0000000..c3672c4
--- /dev/null
+++ b/src/ds_ctrl_lines.cpp
@@ -0,0 +1,197 @@
+#include "ds_ctrl_lines.h"
+#include "sensors.h"
+
+using namespace std;
+DataSourceCtrlLines::DataSourceCtrlLines() {}
+DataSourceCtrlLines::DataSourceCtrlLines(vector<DataSource*> in_, vector<DataSource*> out_){
+    this->in = in_;
+    this->out = out_;
+
+    fboundsOX = false;
+    fboundsOY = false;
+    fboundsX = false;
+    fboundsY = false;
+    slow = false;
+
+    inVOldX = 0;
+    inVOldY = 0;
+
+    for(int i = 0; i < 4; i++){
+      linetriggerI[i] = 0;
+      linetriggerO[i] = 0;
+    }
+
+    exitTimer = 0;
+}
+
+void DataSourceCtrlLines::read(){
+  inV = 0;
+  outV = 0;
+  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;
+  }
+  for(auto it = out.begin(); it != out.end(); it++){
+    i = it - out.begin();
+    ds = *it;
+    linetriggerO[i] = ds->getValue() > LINE_THRESH;
+  }
+
+  for(int i = 0; i < 4; i++){
+    inV = inV | (linetriggerI[i] << i);
+    outV = outV | (linetriggerO[i] << i);
+  }
+
+  inV = inV | outV;
+}
+
+void DataSourceCtrlLines::postProcess(){
+    if ((inV > 0) || (outV > 0)) {
+      fboundsOX = true;
+      fboundsOY = true;
+    if(exitTimer > EXTIME) {
+      fboundsX = true;
+      fboundsY = true;
+    }
+    exitTimer = 0;
+  }
+
+  outOfBounds();
+}
+
+void DataSourceCtrlLines::outOfBounds(){
+    //handleExtern();
+    handleIntern();
+}
+
+void DataSourceCtrlLines::handleIntern(){
+
+  if(fboundsX == true) {
+    if(inV & 0x02) inVOldX = 2;
+    else if(inV & 0x08) inVOldX = 8;
+    if(inVOldX != 0) fboundsX = false;
+  }
+  if(fboundsY == true) {
+    if(inV & 0x01) inVOldY = 1;
+    else if(inV & 0x04) inVOldY = 4;
+    if(inVOldY != 0) fboundsY = false;
+  }
+  if (exitTimer <= EXTIME){
+    drive->canUnlock = false;
+    drive->unlockTime = millis();
+
+    //fase di rientro
+    if(inV == 15) {
+      inV = inVOldY | inVOldX;        //ZOZZATA MAXIMA
+      //digitalWrite(Y, HIGH);
+    }
+
+    switch(inV){
+      case 1:
+        outDir = 180;
+        break;
+      case 2:
+        outDir = 270;
+        break;
+      case 4:
+        outDir = 0;
+        break;
+      case 8:
+        outDir = 90;
+        break;
+      case 3:
+        outDir = 225;
+        break;
+      case 6:
+        outDir = 315;
+        break;
+      case 12:
+        outDir = 45;
+        break;
+      case 9:
+        outDir = 135;
+        break;
+      case 7:
+        outDir = 270;
+        break;
+      case 13:
+        outDir = 90;
+        break;
+      case 11:
+        outDir = 180;
+        break;
+      case 14:
+        outDir = 0;
+        break;
+      case 5:
+        //digitalWrite(R, HIGH);
+        if(inVOldX == 2) outDir = 270;
+        if(inVOldX == 8) outDir = 90;
+        break;
+      case 10:
+        if(inVOldY == 4) outDir = 0;
+        if(inVOldY == 1)outDir = 180;
+        break;
+      case 15:
+        break;
+      case 0:
+      default:
+        //;)
+        break;
+    }
+
+    if(exitTimer < 45) outVel = 200;
+    else outVel = 150;
+    drive->prepareDrive(outDir, outVel, 0);
+    // keeper_backToGoalPost = true;
+    // keeper_tookTimer = true;
+  }else{
+    inV = 0;
+    inVOldX = 0;
+    inVOldY = 0;
+    // lineSensByteBak = 30;
+    //drive->canUnlock = true;
+  }
+
+//    lineSensByteBak = linesensbyteI;
+   if(exitTimer == 99) slow = true;
+   else slow = false;
+}
+
+void DataSourceCtrlLines::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 DataSourceCtrlLines::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/main.cpp b/src/main.cpp
index cc0a32e..d31801d 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -6,17 +6,19 @@
 
 
 void setup() {
-  delay(1000);
+  delay(500);
 
-  DEBUG_PRINT.begin(9600);
-  
+  DEBUG.begin(9600);
   initSensors();
+  delay(2000);
 }
 
 void loop() {
   updateSensors();
-  /*if(millis() % 100 == 0)
-  DEBUG_PRINT.println(us->getValue());*/
-  usCtrl->test();
-  delay(200);
-}
\ No newline at end of file
+
+  drive->prepareDrive(0,0,0);
+  linesCtrl->update();
+ 
+  drive->drivePrepared();
+
+} 
\ No newline at end of file
diff --git a/src/motor.cpp b/src/motor.cpp
index 2c94a36..0bf5f13 100644
--- a/src/motor.cpp
+++ b/src/motor.cpp
@@ -11,11 +11,11 @@ Motor::Motor(int a, int b, int pwm, int angle_){
     pinMode(pinA, OUTPUT);
     pinMode(pinB, OUTPUT);
     pinMode(pinPwm, OUTPUT);
+
+    angle = 0;
 }
 
-Motor::Motor(){
-    
-}
+Motor::Motor(){ }
 
 void Motor::drive(int speed){
     byte VAL_INA, VAL_INB;
diff --git a/src/sensors.cpp b/src/sensors.cpp
index b14aeec..c156675 100644
--- a/src/sensors.cpp
+++ b/src/sensors.cpp
@@ -4,19 +4,24 @@
 void initSensors(){
     pinMode(SWITCH_DX, INPUT);
     pinMode(SWITCH_SX, INPUT);
-    vector<DataSource*> dUs { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), 
+
+    dUs = { new DataSourceUS(&Wire1, int(112)), new DataSourceUS(&Wire1, int(113)), 
     new DataSourceUS(&Wire1, int(114)), new DataSourceUS(&Wire1, int(115)) };
 
+    lIn = { new DataSource(S1I, true), new DataSource(S2I, true), new DataSource(S3I, true), new DataSource(S4I, true) };
+    lOut = { new DataSource(S1O, true), new DataSource(S2O, true), new DataSource(S3O, true), new DataSource(S4O, true) };
+
     drive = new DriveController(new Motor(12, 11, 2, 45),new  Motor(25, 24, 5, 135), new Motor(27, 26, 6, 225), new Motor(21, 22, 23, 315));
     compass = new DataSourceBNO055();
     ball = new DataSourceBall(&Serial4, 57600);
     camera = new DataSourceCamera(&Serial2, 19200);
-    usCtrl = new DataSourceController(dUs);
+    usCtrl = new DataSourceCtrl(dUs);
+    linesCtrl = new DataSourceCtrlLines(lIn, lOut);
 }
 
 void updateSensors(){
     compass->update();
     ball->update();
     camera->update();
-    // usCtrl->test();
+    usCtrl->update();
 }
\ No newline at end of file