From b87488b1c1af215a9f6211db313deeaa747afb05 Mon Sep 17 00:00:00 2001 From: EmaMaker Date: Mon, 3 Feb 2020 10:30:00 +0100 Subject: [PATCH] Camera angle correction using IMU should now work correctly --- src/data_source_camera_conicmirror.cpp | 4 ++-- src/goalie.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/data_source_camera_conicmirror.cpp b/src/data_source_camera_conicmirror.cpp index 0013a0e..7732c4c 100755 --- a/src/data_source_camera_conicmirror.cpp +++ b/src/data_source_camera_conicmirror.cpp @@ -47,8 +47,8 @@ void DataSourceCameraConic :: readSensor(){ bAngle = (bAngle+360)%360; //Fixes with IMU - yAngleFix = ((int)(yAngle - compass->getValue()*0.35) + 360) % 360 ; - bAngleFix = ((int)(bAngle - compass->getValue()*0.35) + 360) % 360 ; + yAngleFix = ((int)(yAngle + compass->getValue()*0.8) + 360) % 360 ; + bAngleFix = ((int)(bAngle + compass->getValue()*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) ); diff --git a/src/goalie.cpp b/src/goalie.cpp index 740ea29..912087a 100755 --- a/src/goalie.cpp +++ b/src/goalie.cpp @@ -36,9 +36,9 @@ void Goalie::goalie(int plusang) { ball->b = ball->dir; storcimentoPorta(); - if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 150, cstorc); + if(ball->distance > 200 && (ball->angle > 340 || ball->angle < 20)) drive->prepareDrive(ball->dir, 350, cstorc); else { - drive->prepareDrive(ball->dir, 150, 0); + drive->prepareDrive(ball->dir, 350, 0); cstorc = 0; } }