diff --git a/include/positionsys_camera.h b/include/positionsys_camera.h
index 59dce44..7878308 100644
--- a/include/positionsys_camera.h
+++ b/include/positionsys_camera.h
@@ -1,7 +1,15 @@
+#include "PID_v2.h"
 #include "systems.h"
 
 #define CAMERA_CENTER_X 3
-#define CAMERA_CENTER_Y 6
+#define CAMERA_CENTER_Y 3
+
+#define Kpx 1
+#define Kix 0
+#define Kdx 0
+#define Kpy 1
+#define Kiy 0
+#define Kdy 0
 
 class PositionSysCamera : public PositionSystem{
 
@@ -10,5 +18,12 @@ class PositionSysCamera : public PositionSystem{
         void goCenter();
         void update() override;
         void test() override;
+        void setCameraPID();
+        void CameraPID();
+
+        double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
+
+        PID* X;
+        PID* Y;
 
 };
diff --git a/lib/Arduino-PID-Library b/lib/Arduino-PID-Library
new file mode 160000
index 0000000..9b4ca0e
--- /dev/null
+++ b/lib/Arduino-PID-Library
@@ -0,0 +1 @@
+Subproject commit 9b4ca0e5b6d7bab9c6ac023e249d6af2446d99bb
diff --git a/src/drivecontroller.cpp b/src/drivecontroller.cpp
index 71f28ee..9a38054 100755
--- a/src/drivecontroller.cpp
+++ b/src/drivecontroller.cpp
@@ -47,7 +47,7 @@ DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_)
     vyn = 0;
 }
 
-void DriveController::prepareDrive(int dir, int speed, int tilt=0){
+void DriveController::prepareDrive(int dir, int speed, int tilt){
     pDir = dir;
     pSpeed = speed;
     pTilt = tilt;
diff --git a/src/main.cpp b/src/main.cpp
index ca69d9f..077a193 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,6 +25,7 @@ void loop() {
 
   // Last thing to do: movement and update status vector
   // drive->prepareDrive(0,0,  CURRENT_DATA_READ.angleAtkFix);
+
   drive->drivePrepared();  
   updateStatusVector();
 } 
diff --git a/src/positionsys_camera.cpp b/src/positionsys_camera.cpp
index 765e0c7..73bbe13 100644
--- a/src/positionsys_camera.cpp
+++ b/src/positionsys_camera.cpp
@@ -3,7 +3,9 @@
 #include "vars.h"
 #include "sensors.h"
 
-PositionSysCamera::PositionSysCamera() {}
+PositionSysCamera::PositionSysCamera() {
+    setCameraPID();
+}
 
 void PositionSysCamera::update(){
 }
@@ -14,26 +16,70 @@ 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);
+    /* 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);
+    else drive->prepareDrive(0, 0, 0); */
 
 
     /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
-    // int x = 1;
-    // int y = 1;
+    /* int x = 1;
+    int y = 1;
     
-    // //Trying using an angle
-    // 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;
+    //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);
+    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(){
+    X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
+    X->SetOutputLimits(-50,50);
+    X->SetMode(AUTOMATIC);
+    X->SetDerivativeLag(1);
+    X->SetControllerDirection(DIRECT);
+    X->SetSampleTime(1.5);
+    Y = new PID(&Inputy, &Outputy, &Setpointy, Kpy, Kiy, Kdy, REVERSE);
+    Y->SetOutputLimits(-50,50);
+    Y->SetMode(AUTOMATIC);
+    Y->SetDerivativeLag(1);
+    Y->SetControllerDirection(DIRECT);
+    Y->SetSampleTime(1.5);
+}
+
+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;
+        Setpointx = 0;
+        Setpointy = 0;
+    }
+    if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
+        Inputx = CURRENT_DATA_READ.cam_xb;
+        Inputy = CURRENT_DATA_READ.cam_yb;
+    }
+    if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
+        Inputx = CURRENT_DATA_READ.cam_xy;
+        Inputy = CURRENT_DATA_READ.cam_yy;
+        //Setpointy todo
+    }
+    //TODO: no goal seen
+    X->Compute();
+    Y->Compute();
+    
+    int dir = -90-(atan2(Outputy*1.5,Outputx)*180/3.14);
+    dir = (dir+360) % 360;
+    if(Outputx > 0) drive->prepareDrive(90, 100, 0);
+    else drive->prepareDrive(270, 100, 0);
 }
\ No newline at end of file
diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py
index 1bd4edf..a7d5194 100644
--- a/utility/OpenMV/conic_eff.py
+++ b/utility/OpenMV/conic_eff.py
@@ -38,8 +38,8 @@ blue_led.on()
 ##############################################################################
 
 
-thresholds = [  (72, 100, -18, 11, 12, 65)  ,    # thresholds yellow goal
-                (39, 61, -18, 11, -47, -16)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+thresholds = [  (45, 71, 0, 20, 56, 84)  ,    # thresholds yellow goal
+                (16, 27, -29, -7, -10, 11)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -60,10 +60,11 @@ sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
-sensor.set_saturation(0)
-sensor.set_brightness(-2)
+sensor.set_saturation(2)
+sensor.set_brightness(0)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 6000)
+sensor.set_auto_whitebal(False)
+sensor.set_auto_exposure(False, 2000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
diff --git a/utility/OpenMV/conic_eff.py.autosave b/utility/OpenMV/conic_eff.py.autosave
new file mode 100644
index 0000000..af4518c
--- /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 = [  (40, 98, -26, 2, 68, 110)  ,    # thresholds yellow goal
+                (26, 100, -38, -4, -20, 16)]  # 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(2)
+sensor.set_brightness(0)
+sensor.set_quality(0)
+sensor.set_auto_whitebal(False)
+sensor.set_auto_exposure(False, 6000)
+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=80, 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)
+