EmaMaker
/
SPQR-Team-2019-REVAMPED
1
1
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

Camera angle fix using IMU working once for all

pull/1/head
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;
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) );

2
src/main.cpp
View File

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