diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp
index 7732c4c..a0dbecb 100755
--- a/src/data_source_camera_conicmirror.cpp
+++ b/src/data_source_camera_conicmirror.cpp
@@ -46,9 +46,11 @@ void DataSourceCameraConic :: readSensor(){
         yAngle = (yAngle+360)%360;
         bAngle = (bAngle+360)%360;
 
+        int angleFix = compass->getValue() > 180 ? compass->getValue() - 360 : compass->getValue();
+
         //Fixes with IMU
-        yAngleFix = ((int)(yAngle + compass->getValue()*0.8) + 360) % 360  ;
-        bAngleFix = ((int)(bAngle + compass->getValue()*0.8) + 360) % 360 ;
+        yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ;
+        bAngleFix = ((int) ((bAngle + angleFix*0.8)) + 360) % 360;
 
         yDist = sqrt( (true_yy-50)*(true_yy-50) + (50-true_xy)*(50-true_xy) );
         bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) );
diff --git a/src/main.cpp b/src/main.cpp
index fbb18fa..a9f117f 100755
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -16,7 +16,7 @@ void setup() {
 
 void loop() {
   updateSensors();
-  // camera->test();
+  camera->test();
   // compass->test();
   goalie->play(role==1);
   keeper->play(role==0);