diff --git a/src/data_source_camera.cpp b/src/data_source_camera.cpp
index 5fc8b4a..565c084 100755
--- a/src/data_source_camera.cpp
+++ b/src/data_source_camera.cpp
@@ -15,6 +15,7 @@ void DataSourceCamera :: readSensor(){
       data_received=false;
       if(count=4 && start==true) {
         data_received=true;
+
         true_xb = xb;
         true_yb = yb;
         true_xy = xy;
@@ -24,14 +25,14 @@ void DataSourceCamera :: readSensor(){
         yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
         bAngle = atan2(50-true_yb, 50-true_xb) * 180 / 3.14;
         //Subtract 90 to bring the angles back to euler angles (0 in front)
-        yAngle = -90 + yAngle;
-        bAngle = -90 + bAngle;
+        yAngle = 90 - yAngle;
+        bAngle = 90 - bAngle;
         //Now cast angles to [0, 359] domain angle flip them
         yAngle = (yAngle + 360) % 360;
         bAngle = (bAngle + 360) % 360;
 
-        yAngleFix = yAngle - compass->getValue() ;
-        bAngleFix = bAngle - compass->getValue() ;
+        yAngleFix = yAngle - compass->getValue()*0.9 ;
+        bAngleFix = bAngle - compass->getValue()*0.9  ;
 
         yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) );
         bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
@@ -74,13 +75,13 @@ void DataSourceCamera::test(){
     DEBUG.print(" | ");
     DEBUG.println(yDist);
     DEBUG.println("---------------");
-    DEBUG.print(xb);
+    DEBUG.print(true_xb);
     DEBUG.print("|");
-    DEBUG.print(yb);
+    DEBUG.print(true_yb);
     DEBUG.print("|");
-    DEBUG.print(xy);
+    DEBUG.print(true_xy);
     DEBUG.print("|");
-    DEBUG.println(yy); 
+    DEBUG.println(true_yy); 
     DEBUG.println("---------------");
     delay(150);
 }
diff --git a/utility/OpenMV/conic_eff.py b/utility/OpenMV/conic_eff.py
index d673585..677bb8a 100644
--- a/utility/OpenMV/conic_eff.py
+++ b/utility/OpenMV/conic_eff.py
@@ -39,8 +39,8 @@ blue_led.on()
 
 
 
-thresholds = [  (49, 81, -18, 29, 22, 83),    # thresholds yellow goal
-                (0, 61, -4, 55, -81, -12)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
+thresholds = [  (44, 100, -4, 48, 11, 74),    # thresholds yellow goal
+                (16, 60, -4, 48, -73, -22)]  # thresholds blue goal (6, 31, -15, 4, -35, 0)
 
 roi = (0, 6, 318, 152)
 
@@ -61,10 +61,10 @@ sensor.reset()
 sensor.set_pixformat(sensor.RGB565)
 sensor.set_framesize(sensor.QQVGA)
 sensor.set_contrast(+3)
-sensor.set_saturation(+3)
+sensor.set_saturation(+1)
 sensor.set_brightness(-1)
 sensor.set_quality(0)
-sensor.set_auto_exposure(False, 5000)
+sensor.set_auto_exposure(False, 12000)
 sensor.set_auto_gain(True)
 sensor.skip_frames(time = 300)
 
@@ -111,7 +111,7 @@ while(True):
 
     #Normalize data between 0 and 100
     if y_found == True:
-        y_cx = val_map(y_cx, -img.width() / 2, img.width() / 2, 0, 100)
+        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)
@@ -124,7 +124,7 @@ while(True):
         s_ycy = y_cy
 
     if b_found == True:
-        b_cx = val_map(b_cx, -img.width() / 2, img.width() / 2, 0, 100)
+        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
@@ -137,7 +137,7 @@ while(True):
         s_bcx = b_cx
         s_bcy = b_cy
 
-    #print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
+    print(str(y_cx) + " | " + str(y_cy) + "  ---  " + str(b_cx) + " | " + str(b_cy))
 
 
     uart.write(START_BYTE)