diff --git a/include/behaviour_control/status_vector.h b/include/behaviour_control/status_vector.h
index f20048d..3a67b28 100644
--- a/include/behaviour_control/status_vector.h
+++ b/include/behaviour_control/status_vector.h
@@ -43,7 +43,8 @@ typedef struct data{
         speed, tilt, dir, axisBlock[4],
         USfr, USsx, USdx, USrr, 
         lineOutDir, matePos, role, cam_xb_fixed,
-        cam_xy_fixed, cam_yb_fixed, cam_yy_fixed;
+        cam_xy_fixed, cam_yb_fixed, cam_yy_fixed,
+        posx, posy, addvx, addvy;
     Game* game;
     LineSystem* lineSystem;
     PositionSystem* posSystem;
diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h
index a951194..280d851 100644
--- a/include/motors_movement/drivecontroller.h
+++ b/include/motors_movement/drivecontroller.h
@@ -51,6 +51,4 @@ class DriveController{
 
         double input, output, setpoint;
         
-        float sins[360], cosins[360];
-
 };
\ No newline at end of file
diff --git a/include/position/positionsys_camera.h b/include/position/positionsys_camera.h
index ad3073b..d75673f 100644
--- a/include/position/positionsys_camera.h
+++ b/include/position/positionsys_camera.h
@@ -24,6 +24,9 @@ class PositionSysCamera : public PositionSystem{
     public:
         PositionSysCamera();
         void goCenter();
+        void centerGoal();
+        void setMoveSetpoints(int x, int y);
+        void addMoveOnAxis(int x, int y);
         void update() override;
         void test() override;
         void setCameraPID();
@@ -31,7 +34,8 @@ class PositionSysCamera : public PositionSystem{
         int calcOtherGoalY(int goalY);
 
         double Inputx, Outputx, Setpointx, Inputy, Outputy, Setpointy;
-        int MAX_DIST;
+        int MAX_DIST, vx, vy, axisx, axisy;
+        bool givenMovement;
         PID* X;
         PID* Y;
 
diff --git a/include/vars.h b/include/vars.h
index 4c4e00f..4c42379 100755
--- a/include/vars.h
+++ b/include/vars.h
@@ -1,6 +1,12 @@
 #pragma once
 #define DEBUG Serial3
 
+#ifdef VARS
+#define extr
+#else
+#define extr extern
+#endif
+
 #define GLOBAL_SPD_MULT 1.0
 
 #define LED_R 20
@@ -9,4 +15,7 @@
 
 #define BUZZER 30
 #define SWITCH_SX 28
-#define SWITCH_DX 29
\ No newline at end of file
+#define SWITCH_DX 29
+
+
+extr float sins[360], cosins[360];
diff --git a/src/main.cpp b/src/main.cpp
index 1045fff..d44002f 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -1,14 +1,22 @@
 #include <Arduino.h>
 
+#define VARS
+
 #include "behaviour_control/status_vector.h"
 #include "position/positionsys_zone.h"
 #include "sensors/sensors.h"
 #include "strategy_roles/games.h"
+#include "vars.h"
 
 void setup() {
   delay(1500);
-
   DEBUG.begin(9600);
+    
+  for(int i = 0; i < 360; i++){
+      sins[i] = (float) sin((i*3.14/180));
+      cosins[i] =  (float) cos((i*3.14/180));
+  }
+
   initStatusVector();
   initSensors();
   initGames();
diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp
index 98773b5..23991a0 100644
--- a/src/motors_movement/drivecontroller.cpp
+++ b/src/motors_movement/drivecontroller.cpp
@@ -1,17 +1,13 @@
 #include "motors_movement/drivecontroller.h"
 #include "sensors/sensors.h"
 #include "behaviour_control/status_vector.h"
+#include "vars.h"
 
 DriveController::DriveController(Motor* m1_, Motor* m2_, Motor* m3_, Motor* m4_){
     m1 = m1_;
     m2 = m2_;
     m3 = m3_;
     m4 = m4_;
-    
-    for(int i = 0; i < 360; i++){
-        sins[i] = (float) sin(torad(i));
-        cosins[i] =  (float) cos(torad(i));
-    }
 
     pDir = 0;
     pSpeed = 0;
@@ -69,8 +65,9 @@ void DriveController::drive(int dir, int speed, int tilt){
     speed = speedFilter->calculate(speed)*GLOBAL_SPD_MULT;
     tilt = tilt > 180 ? tilt - 360 : tilt;
 
-    vx = ((speed * cosins[dir]));
-    vy = ((-speed * sins[dir]));
+    //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
+    vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
+    vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
 
     if((((vy < 0 && vxn == 1) || (vy > 0 && vxp == 1) || (vx < 0 && vyp == 1) || (vx > 0 && vyn == 1)) && canUnlock) || (millis() > this->unlockTime+UNLOCK_THRESH)) {
         vxn = 0;
diff --git a/src/position/positionsys_camera.cpp b/src/position/positionsys_camera.cpp
index ba28ab8..7fd606b 100644
--- a/src/position/positionsys_camera.cpp
+++ b/src/position/positionsys_camera.cpp
@@ -4,46 +4,6 @@
 #include "vars.h"
 
 PositionSysCamera::PositionSysCamera() {
-    setCameraPID();
-}
-
-void PositionSysCamera::update(){
-}
-
-void PositionSysCamera::test(){
-}
-
-void PositionSysCamera::goCenter(){
-    /*WORKS BUT CAN BE BETTER*/
-    //Y
-    /* if((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y) drive->prepareDrive(180, 75, 0);
-    else if ((CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) drive->prepareDrive(0, 75, 0);
-    //X
-    else if(CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X) drive->prepareDrive(90, 75, 0);
-    else if(CURRENT_DATA_READ.cam_xb > CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > CAMERA_CENTER_X) drive->prepareDrive(270, 75, 0);
-    else drive->prepareDrive(0, 0, 0); */
-
-
-    /*MAKING A SINGLE LINE HERE, DOESN'T WORK FOR NOW*/
-    /* int x = 1;
-    int y = 1;
-    ;
-    //Trying using an angle
-    if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
-    if((CURRENT_DATA_READ.cam_yy) > CAMERA_CENTER_Y || (CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy) < -CAMERA_CENTER_Y) 
-    y = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
-    if(CURRENT_DATA_READ.bSeen && (CURRENT_DATA_READ.cam_xb < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xb > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xb;
-    if(CURRENT_DATA_READ.ySeen && (CURRENT_DATA_READ.cam_xy < -CAMERA_CENTER_X || CURRENT_DATA_READ.cam_xy > -CAMERA_CENTER_X) ) x = CURRENT_DATA_READ.cam_xy;
-
-    int dir = -90-(atan2(y*1.5,x)*180/3.14);
-    dir = (dir+360) % 360;
-    drive->prepareDrive(dir, 100, 0);
-    } */
-    CameraPID();
-}
-
-//using a pid controller for the movement, or trying at least
-void PositionSysCamera :: setCameraPID(){
     MAX_DIST = sqrt(MAX_X*MAX_X + MAX_Y*MAX_Y);
     Inputx = 0;
     Outputx = 0;
@@ -51,6 +11,12 @@ void PositionSysCamera :: setCameraPID(){
     Inputy = 0;
     Outputy = 0;
     Setpointy = 0;
+
+    vx = 0;
+    vy = 0;
+    axisx = 0;
+    axisy = 0;
+    givenMovement = false;
     
     X = new PID(&Inputx, &Outputx, &Setpointx, Kpx, Kix, Kdx, REVERSE);
     X->SetOutputLimits(-50,50);
@@ -64,9 +30,65 @@ void PositionSysCamera :: setCameraPID(){
     Y->SetSampleTime(2);
 }
 
+void PositionSysCamera::update(){
+    int posx, posy;
+
+    //Calculate robot position based on just-read coordinates for camera. Using CURRENT_DATA_WRITE instead of CURRENT_DATA_READ othwerise we would be late by 1 loop cycle, but the calculations have to stay in sync
+    //Coordinates are referred to a cartesian plane with the origin at the center of the field. Angles starting at the north of the robot
+    if(CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == true){
+        posx = (CURRENT_DATA_WRITE.cam_xy + CURRENT_DATA_WRITE.cam_xb) / 2;
+        posy = CURRENT_DATA_WRITE.cam_yb + CURRENT_DATA_WRITE.cam_yy;
+    }else if (CURRENT_DATA_WRITE.bSeen == true && CURRENT_DATA_WRITE.ySeen == false){
+        posx = CURRENT_DATA_WRITE.cam_xb;
+        posy = CURRENT_DATA_WRITE.cam_yb + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yb);
+    }else if (CURRENT_DATA_WRITE.bSeen == false && CURRENT_DATA_WRITE.ySeen == true){
+        posx = CURRENT_DATA_WRITE.cam_xy;
+        posy = CURRENT_DATA_WRITE.cam_yy + calcOtherGoalY(CURRENT_DATA_WRITE.cam_yy);
+    }else{
+        //TODO: no goal seen ?
+    }
+    //IMPORTANT STEP: or the direction of the plane will be flipped
+    posx *= -1;
+    posy *= -1;
+
+    CURRENT_DATA_WRITE.posx = posx;
+    CURRENT_DATA_WRITE.posy = posy;
+    Inputx = posx;
+    Inputy = posy;
+
+    //Prepare for receiving information about movement
+    //Starting setpoint position as current position
+    Setpointx = posx;
+    Setpointy = posy;
+    axisx = 0;
+    axisy = 0;
+    givenMovement = false;
+}
+
+//This means the last time this is called has the biggest priority, has for prepareDrive
+void PositionSysCamera::setMoveSetpoints(int x, int y){
+    Setpointx = x;
+    Setpointy = y;
+    givenMovement = true;
+}
+
+void PositionSysCamera::addMoveOnAxis(int x, int y){
+    axisx += x;
+    axisy += y;
+    givenMovement = true;
+}
+
+void PositionSysCamera::goCenter(){
+    setMoveSetpoints(CAMERA_CENTER_X, CAMERA_CENTER_Y);
+}
+
+void PositionSysCamera::centerGoal(){
+    setMoveSetpoints(CAMERA_GOAL_X, CAMERA_GOAL_Y);
+}
+
 /*Knowing the sum of the absolute values of the y position of the goals, it calculates the missing goal y knowing the other one
 We know the sum of the absolute values is a fixed number.
-By♦ subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
+By subtracting the absolute value of the goal y we know to the sum of the absolute values, we get the absolute value of the missing goal y
 The sign of the goal y we found is simply the reverse of the one we got
 */
 int PositionSysCamera::calcOtherGoalY(int goalY){
@@ -75,46 +97,36 @@ int PositionSysCamera::calcOtherGoalY(int goalY){
     return otherGoalY;
 }
 
-void PositionSysCamera :: CameraPID(){   
-    if(CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == true){
-        Inputx = (CURRENT_DATA_READ.cam_xy + CURRENT_DATA_READ.cam_xb) / 2;
-        Inputy = CURRENT_DATA_READ.cam_yb + CURRENT_DATA_READ.cam_yy;
-    }else if (CURRENT_DATA_READ.bSeen == true && CURRENT_DATA_READ.ySeen == false){
-        Inputx = CURRENT_DATA_READ.cam_xb;
-        Inputy = CURRENT_DATA_READ.cam_yb + calcOtherGoalY(CURRENT_DATA_READ.cam_yb);
-    }else if (CURRENT_DATA_READ.bSeen == false && CURRENT_DATA_READ.ySeen == true){
-        Inputx = CURRENT_DATA_READ.cam_xy;
-        Inputy = CURRENT_DATA_READ.cam_yy + calcOtherGoalY(CURRENT_DATA_READ.cam_yy);
-        //Setpointy todo
-    }else{
-        //TODO: no goal seen
-    }
+void PositionSysCamera::CameraPID(){   
+    if(givenMovement){
 
-    //To have the axis corresponding to the real cartesian plane Inputx and InputY must be multiplied by -1
-    //This is because the coordinates given as input are relative to the robot, while the coordinates used in the setpoint
-    //are on an absolute cartesian plane with it's center in the center of the field
-    Inputx *= -1;
-    Inputy *= -1;
+        vx = 0;
+        vy = 0;
 
-    Setpointx = CAMERA_CENTER_X;
-    Setpointy = CAMERA_CENTER_Y;
+        Setpointx += axisx;
+        Setpointy += axisy;
+
+        X->Compute();
+        Y->Compute();
         
-    X->Compute();
-    Y->Compute();
-    
-    // DEBUG.print(CURRENT_DATA_READ.cam_yb);
-    // DEBUG.print(" ");
-    // DEBUG.println(calcOtherGoalY(CURRENT_DATA_READ.cam_yb));
-
-    /*if(abs(Outputx) <= 1 && abs(Outputy) <= 1){
-        drive->prepareDrive(0,0,0);
-    }else{*/
+        //Compute an X and Y to give to the PID later
+        //There's surely a better way to do this
         int dir = -90-(atan2(Outputy,Outputx)*180/3.14);
         dir = (dir+360) % 360;
 
         int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
         int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0, 350);
         drive->prepareDrive(dir, speed, 0);
-        //DEBUG.println(dir);
-    //}
-}
\ No newline at end of file
+
+        //TODO: add complementary filter on this speed if we keep using it
+
+        vx = ((speed * cosins[dir]));
+        vy = ((-speed * sins[dir]));
+
+        CURRENT_DATA_WRITE.addvx = vx;
+        CURRENT_DATA_WRITE.addvy = vy;
+    }
+}
+
+void PositionSysCamera::test(){
+}
diff --git a/src/sensors/data_source_camera_conicmirror.cpp b/src/sensors/data_source_camera_conicmirror.cpp
index d4b7b4e..36f4e18 100644
--- a/src/sensors/data_source_camera_conicmirror.cpp
+++ b/src/sensors/data_source_camera_conicmirror.cpp
@@ -45,6 +45,8 @@ void DataSourceCameraConic ::readSensor() {
         true_yb = yb;
         true_xy = xy;
         true_yy = yy;
+
+        computeCoordsAngles();
       }
       end = true;
       start = false;
diff --git a/src/strategy_roles/game.cpp b/src/strategy_roles/game.cpp
index 397c8b3..f9b5b05 100644
--- a/src/strategy_roles/game.cpp
+++ b/src/strategy_roles/game.cpp
@@ -1,5 +1,6 @@
 #include "behaviour_control/status_vector.h"
 #include "strategy_roles/game.h"
+#include "position/positionsys_camera.h"
 
 Game::Game() {}
 Game::Game(LineSystem* ls_, PositionSystem* ps_) {
@@ -8,14 +9,14 @@ Game::Game(LineSystem* ls_, PositionSystem* ps_) {
 }
 
 void Game::play(bool condition){
-    ps->update();
     if(condition) {
+        ps->update();
         realPlay();
         ls->update();
 
         CURRENT_DATA_WRITE.posSystem = (this->ps);
         CURRENT_DATA_WRITE.lineSystem = (this->ls);
         CURRENT_DATA_WRITE.game = this;
-
+        ((PositionSysCamera*)ps)->CameraPID();
     }
 }
\ No newline at end of file