EmaMaker
/
SPQR-Team1-2020
1
1
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Angles finally working

code_midgen
EmaMaker 2020-01-27 17:47:36 +01:00
parent e84292568e
commit 9a8830f275
2 changed files with 16 additions and 15 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/data_source_camera.cpp
View File

@ -15,6 +15,7 @@ void DataSourceCamera :: readSensor(){
data_received=false; data_received=false;
if(count=4 && start==true) { if(count=4 && start==true) {
data_received=true; data_received=true;
true_xb = xb; true_xb = xb;
true_yb = yb; true_yb = yb;
true_xy = xy; true_xy = xy;
@ -24,14 +25,14 @@ void DataSourceCamera :: readSensor(){
yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14; yAngle = atan2(50-true_yy, 50-true_xy) * 180 / 3.14;
bAngle = atan2(50-true_yb, 50-true_xb) * 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) //Subtract 90 to bring the angles back to euler angles (0 in front)
yAngle = -90 + yAngle; yAngle = 90 - yAngle;
bAngle = -90 + bAngle; bAngle = 90 - bAngle;
//Now cast angles to [0, 359] domain angle flip them //Now cast angles to [0, 359] domain angle flip them
yAngle = (yAngle + 360) % 360; yAngle = (yAngle + 360) % 360;
bAngle = (bAngle + 360) % 360; bAngle = (bAngle + 360) % 360;
yAngleFix = yAngle - compass->getValue() ; yAngleFix = yAngle - compass->getValue()*0.9 ;
bAngleFix = bAngle - compass->getValue() ; bAngleFix = bAngle - compass->getValue()*0.9 ;
yDist = sqrt( (50-true_yy)*(50-true_yy) + (50-true_xy)*(50-true_xy) ); 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) ); bDist = sqrt( (50-true_yb)*(50-true_yb) + (50-true_xb)*(50-true_xb) );
@ -74,13 +75,13 @@ void DataSourceCamera::test(){
DEBUG.print(" | "); DEBUG.print(" | ");
DEBUG.println(yDist); DEBUG.println(yDist);
DEBUG.println("---------------"); DEBUG.println("---------------");
DEBUG.print(xb); DEBUG.print(true_xb);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.print(yb); DEBUG.print(true_yb);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.print(xy); DEBUG.print(true_xy);
DEBUG.print("|"); DEBUG.print("|");
DEBUG.println(yy); DEBUG.println(true_yy);
DEBUG.println("---------------"); DEBUG.println("---------------");
delay(150); delay(150);
} }

14
utility/OpenMV/conic_eff.py
View File

@ -39,8 +39,8 @@ blue_led.on()
thresholds = [ (49, 81, -18, 29, 22, 83), # thresholds yellow goal thresholds = [ (44, 100, -4, 48, 11, 74), # thresholds yellow goal
(0, 61, -4, 55, -81, -12)] # thresholds blue goal (6, 31, -15, 4, -35, 0) (16, 60, -4, 48, -73, -22)] # thresholds blue goal (6, 31, -15, 4, -35, 0)
roi = (0, 6, 318, 152) roi = (0, 6, 318, 152)
@ -61,10 +61,10 @@ sensor.reset()
sensor.set_pixformat(sensor.RGB565) sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QQVGA) sensor.set_framesize(sensor.QQVGA)
sensor.set_contrast(+3) sensor.set_contrast(+3)
sensor.set_saturation(+3) sensor.set_saturation(+1)
sensor.set_brightness(-1) sensor.set_brightness(-1)
sensor.set_quality(0) sensor.set_quality(0)
sensor.set_auto_exposure(False, 5000) sensor.set_auto_exposure(False, 12000)
sensor.set_auto_gain(True) sensor.set_auto_gain(True)
sensor.skip_frames(time = 300) sensor.skip_frames(time = 300)
@ -111,7 +111,7 @@ while(True):
#Normalize data between 0 and 100 #Normalize data between 0 and 100
if y_found == True: 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) y_cy = val_map(y_cy, -img.height() / 2, img.height() / 2, 0, 100)
#Prepare for send as a list of characters #Prepare for send as a list of characters
s_ycx = chr(y_cx) s_ycx = chr(y_cx)
@ -124,7 +124,7 @@ while(True):
s_ycy = y_cy s_ycy = y_cy
if b_found == True: 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) b_cy = val_map(b_cy, -img.height() / 2, img.height() / 2, 0, 100)
#Prepare for send as a list of characters #Prepare for send as a list of characters
@ -137,7 +137,7 @@ while(True):
s_bcx = b_cx s_bcx = b_cx
s_bcy = b_cy 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) uart.write(START_BYTE)