diff --git a/include/motors_movement/drivecontroller.h b/include/motors_movement/drivecontroller.h
index 1dce17b..78f2bb9 100644
--- a/include/motors_movement/drivecontroller.h
+++ b/include/motors_movement/drivecontroller.h
@@ -18,11 +18,13 @@
 
 #define UNLOCK_THRESH 800
 
+//Max possible vel 310
+
 #define MAX_VEL 150
-#define MAX_VEL_EIGTH 120
-#define MAX_VEL_HALF 75
-#define MAX_VEL_3QUARTERS 112
-#define MAX_VEL_QUARTER 38
+#define MAX_VEL_EIGTH ((int)MAX_VEL*0.8)
+#define MAX_VEL_HALF ((int)MAX_VEL*0.5)
+#define MAX_VEL_3QUARTERS ((int)MAX_VEL*0.75)
+#define MAX_VEL_QUARTER ((int)MAX_VEL*0.25)
 
 class DriveController{
 
@@ -35,6 +37,7 @@ class DriveController{
         void drivePrepared();
         float updatePid();
         float torad(float f);
+        void resetDrive();
 
         int vxp, vyp, vxn, vyn;
         bool canUnlock;
diff --git a/include/systems/lines/linesys_camera.h b/include/systems/lines/linesys_camera.h
index d0ba408..7bb1a18 100644
--- a/include/systems/lines/linesys_camera.h
+++ b/include/systems/lines/linesys_camera.h
@@ -16,7 +16,7 @@
 #define S4I A9
 #define S4O A8
 
-#define LINE_THRESH_CAM 300
+#define LINE_THRESH_CAM 350
 #define EXIT_TIME 250
 #define LINES_EXIT_SPD 350
 
diff --git a/include/systems/position/positionsys_camera.h b/include/systems/position/positionsys_camera.h
index 65ff45b..cc7cbf9 100644
--- a/include/systems/position/positionsys_camera.h
+++ b/include/systems/position/positionsys_camera.h
@@ -2,23 +2,23 @@
 
 #include "PID_v2.h"
 #include "systems/systems.h"
+#include "behaviour_control/complementary_filter.h"
 
 /*Camera translation: because of mechanical imprecision, the center of the camera and the center of the cone mirror may not coincide
 To overcome this, each coordinate needs to be shifted by some amount, defined on a per-robot basis that needs to be recalibrated each time.
 These values need to be subtracted from the coords used in setMoveSetpoints*/
-#define CAMERA_TRANSLATION_X 0
-#define CAMERA_TRANSLATION_Y 3
-
+#define CAMERA_TRANSLATION_X 8
+#define CAMERA_TRANSLATION_Y 10
 //Camera center: those setpoints correspond to what we consider the center of the field
 #define CAMERA_CENTER_X 0
 #define CAMERA_CENTER_Y 0
 
 //Camera goal: those setpoints correspond to the position of the center of the goal on the field
 #define CAMERA_GOAL_X 0
-#define  CAMERA_GOAL_Y -12
+#define  CAMERA_GOAL_Y -14
 
-#define CAMERA_GOAL_MIN_X -14
-#define CAMERA_GOAL_MAX_X 14
+#define CAMERA_GOAL_MIN_X -16
+#define CAMERA_GOAL_MAX_X 16
 
 #define CAMERA_CENTER_Y_ABS_SUM 60
 //Actually it's ± MAX_VAL
@@ -52,5 +52,7 @@ class PositionSysCamera : public PositionSystem{
         bool givenMovement;
         PID* X;
         PID* Y;
+        ComplementaryFilter* filterDir;
+        ComplementaryFilter* filterSpeed;
 
 };
diff --git a/src/main.cpp b/src/main.cpp
index 4bdce9c..7659cd4 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -15,7 +15,7 @@ bool keeper_condition = false;
 
 void setup() {
   delay(1500);
-  DEBUG.begin(9600);
+  DEBUG.begin(115200);
     
   for(int i = 0; i < 360; i++){
       sins[i] = (float) sin((i*3.14/180));
@@ -38,7 +38,8 @@ void setup() {
 
 void loop() {
   updateSensors();
-  if(DEBUG.available()) testmenu->testMenu();
+
+  drive->resetDrive();
   
   striker_condition = role == HIGH || ((Keeper*)keeper)->shouldStrike;
   keeper_condition = role == LOW;
@@ -46,6 +47,8 @@ void loop() {
   striker->play(striker_condition);
   keeper->play(keeper_condition);
 
+  testmenu->testMenu();
+
   // Last thing to do: movement and update status vector
   drive->drivePrepared();  
   updateStatusVector();
diff --git a/src/motors_movement/drivecontroller.cpp b/src/motors_movement/drivecontroller.cpp
index ef8816b..8cfef60 100644
--- a/src/motors_movement/drivecontroller.cpp
+++ b/src/motors_movement/drivecontroller.cpp
@@ -68,11 +68,11 @@ void DriveController::drive(int dir, int speed, int tilt){
     //TODO: Changing CURRENT_DATA_READ to CURRENT_DATA_WRITE?
     // Disable vector sum because calculations are a bitty crappy imho. Will have to test if it's what makes the robot act strange with lines
     // Re enabling the below lines requires to comment out drive->prepareDrive and uncommenting the lines relative to vector sum inside positionsys_camera and comment out the other lines here
-    // vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
-    // vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
+    vx = ((speed * cosins[dir])) + CURRENT_DATA_READ.addvx;
+    vy = ((-speed * sins[dir])) + CURRENT_DATA_READ.addvy;
 
-    vx = ((speed * cosins[dir]));
-    vy = ((-speed * sins[dir]));
+    // vx = ((speed * cosins[dir]));
+    // vy = ((-speed * sins[dir]));
 
     // 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;
@@ -151,4 +151,10 @@ void DriveController::drive(int dir, int speed, int tilt){
     CURRENT_DATA_WRITE.axisBlock[1] = vxn;
     CURRENT_DATA_WRITE.axisBlock[2] = vyp;
     CURRENT_DATA_WRITE.axisBlock[3] = vyn;
-} 
\ No newline at end of file
+} 
+
+void DriveController::resetDrive(){
+    CURRENT_DATA_WRITE.addvx = 0;
+    CURRENT_DATA_WRITE.addvy = 0;
+    prepareDrive(0,0,0);
+}
\ No newline at end of file
diff --git a/src/motors_movement/motor.cpp b/src/motors_movement/motor.cpp
index 0e5792f..4629a47 100644
--- a/src/motors_movement/motor.cpp
+++ b/src/motors_movement/motor.cpp
@@ -41,9 +41,20 @@ void Motor::test(){
     analogWrite(pinPwm, 255);
     delay(1500);
 
+
+    digitalWriteFast(pinA, 0);
+    digitalWriteFast(pinB, 0);
+    analogWrite(pinPwm, 0);
+    delay(500);
+
     digitalWriteFast(pinA, 1);
     digitalWriteFast(pinB, 0);
     analogWrite(pinPwm, 255);
     delay(1500);
 
+    digitalWriteFast(pinA, 0);
+    digitalWriteFast(pinB, 0);
+    analogWrite(pinPwm, 0);
+    delay(500);
+
 }
\ No newline at end of file
diff --git a/src/system/positions/positionsys_camera.cpp b/src/system/positions/positionsys_camera.cpp
index da249d5..321aa71 100644
--- a/src/system/positions/positionsys_camera.cpp
+++ b/src/system/positions/positionsys_camera.cpp
@@ -28,6 +28,9 @@ PositionSysCamera::PositionSysCamera() {
     Y->SetMode(AUTOMATIC);
     Y->SetDerivativeLag(1);
     Y->SetSampleTime(2);
+
+    filterDir = new ComplementaryFilter(0.65);
+    filterSpeed = new ComplementaryFilter(0.65);
 }
 
 void PositionSysCamera::update(){
@@ -118,19 +121,21 @@ void PositionSysCamera::CameraPID(){
 
         int dist = sqrt(Outputx*Outputx + Outputy*Outputy);
         int speed = map(dist*DIST_MULT, 0, MAX_DIST, 0,  MAX_VEL);
-        speed = speed > 25 ? speed : 0;
-        drive->prepareDrive(dir, speed, 0);
+        speed = filterSpeed->calculate(speed);
+        speed = speed > 35 ? speed : 0;
+        dir = filterDir->calculate(dir);
+        // drive->prepareDrive(dir, speed, 0);
 
 
         //Disable below lines for now because they probably result in unexpected behaviour on lines. Re enabling them requires to comment out the drive->prepareDrive above
         //and check the notes in drivecontroller for the other stuff to comment and uncomment
 
         //TODO: add complementary filter on this speed if we keep using it
-        // vx = ((speed * cosins[dir]));
-        // vy = ((-speed * sins[dir]));
+        vx = ((speed * cosins[dir]));
+        vy = ((-speed * sins[dir]));
 
-        // CURRENT_DATA_WRITE.addvx = vx;
-        // CURRENT_DATA_WRITE.addvy = vy;
+        CURRENT_DATA_WRITE.addvx = vx;
+        CURRENT_DATA_WRITE.addvy = vy;
     }
 }