ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

impl CheckReachedDestination

Browse source
This commit is contained in:
Andy Killorin 2026-02-04 14:50:41 -05:00
parent dd538d7185
commit f4373cdc59
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 7 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

6
src/robot-nav.cpp
View file

@ -44,12 +44,14 @@ void Robot::SetDestination(const Pose& dest)
bool Robot::CheckReachedDestination(void)
{
bool retVal = false;
/**
* TODO: Add code to check if you've reached destination here.
*/
return retVal;
float sq_dist = pow((destPose.x - currPose.x),2) + pow((destPose.y-currPose.y),2);
float rad_error = abs(destPose.theta - currPose.theta);
return sq_dist < TARGET_SQ_DIST && rad_error < TARGET_RAD_ERR;
}
void Robot::DriveToPoint(void)

3
src/robot.h
View file

@ -2,6 +2,9 @@
#include "chassis.h"
const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (cm)
const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
class Robot
{
protected: