ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

define tau

Browse source
This commit is contained in:
Andy Killorin 2026-02-04 22:49:13 -05:00
parent ff16e9026d
commit 324a51cf4c
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 7 additions and 9 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
src/robot-nav.cpp
View file

@ -46,17 +46,17 @@ float Robot::SquareDistance(void) {
return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
}
float Robot::RadError(float current, float destination) {
#define TAU (float) TWO_PI
float Robot::RadError(float current, float destination) {
float diff = destination-current;
diff += PI_2;
diff %= TAU;
diff -= PI_2;
diff += HALF_PI;
diff = fmod(diff,TAU); // floating point modulo
diff -= HALF_PI;
if (diff > 180) {
diff -= TAU;
}
return diff;
}
@ -66,8 +66,6 @@ bool Robot::CheckReachedDestination(void)
* TODO: Add code to check if you've reached destination here.
*/
float rad_error = abs(destPose.theta - currPose.theta);
return SquareDistance() < TARGET_SQ_DIST
&& RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
}
@ -84,8 +82,8 @@ void Robot::DriveToPoint(void)
target_heading = atan2(destPose.x - currPose.x, destPose.y - currPose.y);
}
float heading_err = destPose.theta - target_heading;
float forward_effort = cos(heading_err);
float heading_err = RadError(currPose.theta, target_heading);
float forward_effort = cos(heading_err) * sqrt(SquareDistance());
/**