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

Camera angle fix using IMU working once for all

code_midgen
EmaMaker 2020-02-03 14:12:04 +01:00
parent b87488b1c1
commit 73299bc6c4
2 changed files with 5 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/data_source_camera_conicmirror.cpp
View File

@ -46,9 +46,11 @@ void DataSourceCameraConic :: readSensor(){
yAngle = (yAngle+360)%360; yAngle = (yAngle+360)%360;
bAngle = (bAngle+360)%360; bAngle = (bAngle+360)%360;
int angleFix = compass->getValue() > 180 ? compass->getValue() - 360 : compass->getValue();
//Fixes with IMU //Fixes with IMU
yAngleFix = ((int)(yAngle + compass->getValue()*0.8) + 360) % 360 ; yAngleFix = ((int) ((yAngle + angleFix*0.8)) + 360) % 360 ;
bAngleFix = ((int)(bAngle + compass->getValue()*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) ); 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) ); bDist = sqrt( (true_yb-50)*(true_yb-50) + (50-true_xb)*(50-true_xb) );

2
src/main.cpp
View File

@ -16,7 +16,7 @@ void setup() {
void loop() { void loop() {
updateSensors(); updateSensors();
// camera->test(); camera->test();
// compass->test(); // compass->test();
goalie->play(role==1); goalie->play(role==1);
keeper->play(role==0); keeper->play(role==0);