ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

impl roatation-aware raddiff

Browse source
This commit is contained in:
Andy Killorin 2026-02-04 22:33:52 -05:00
parent 936b160f2c
commit ff16e9026d
Signed by: ank
GPG key ID: D96FEC2847FBAFA0
2 changed files with 19 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
src/robot-nav.cpp
View file

@ -46,6 +46,20 @@ float Robot::SquareDistance(void) {
return pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
}
float Robot::RadError(float current, float destination) {
float diff = destination-current;
diff += PI_2;
diff %= TAU;
diff -= PI_2;
if (diff > 180) {
diff -= TAU;
}
return diff;
}
bool Robot::CheckReachedDestination(void)
{
/**
@ -54,7 +68,8 @@ bool Robot::CheckReachedDestination(void)
float rad_error = abs(destPose.theta - currPose.theta);
return SquareDistance() < TARGET_SQ_DIST && rad_error < TARGET_RAD_ERR;
return SquareDistance() < TARGET_SQ_DIST
&& RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
}
void Robot::DriveToPoint(void)

3
src/robot.h
View file

@ -55,4 +55,7 @@ protected:
// Square distance between current and target pose
float SquareDistance(void);
// minimum radians to transform current to setpoint
float RadError(float current, float setpoint);
};