From 9a8830f2758cf00138ae2d838600b1ea0423b379 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 27 Jan 2020 17:47:36 +0100 Subject: [PATCH] Angles finally working --- src/data_source_camera.cpp | 17 +++++++++-------- utility/OpenMV/conic_eff.py | 14 +++++++------- 2 files changed, 16 insertions(+), 15 deletions(-) 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)