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