ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

possible DriveToPoint impl

Browse source
This commit is contained in:
Andy Killorin 2026-02-04 22:57:08 -05:00
parent 324a51cf4c
commit e4b4072fd9
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 8 additions and 12 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
src/robot-nav.cpp
View file

@ -62,10 +62,6 @@ float Robot::RadError(float current, float destination) {
bool Robot::CheckReachedDestination(void)
{
/**
* TODO: Add code to check if you've reached destination here.
*/
return SquareDistance() < TARGET_SQ_DIST
&& RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
}
@ -83,20 +79,16 @@ void Robot::DriveToPoint(void)
}
float heading_err = RadError(currPose.theta, target_heading);
float forward_effort = cos(heading_err) * sqrt(SquareDistance());
float forward_err = cos(heading_err) * sqrt(SquareDistance());
/**
* TODO: Add your IK algorithm here.
*/
float turn_effort = heading_err * LATERAL_kP;
float forward_effort = forward_err * LONGITUDINAL_kP;
#ifdef __NAV_DEBUG__
// Print useful stuff here.
#endif
/**
* TODO: Call chassis.SetMotorEfforts() to command the motion, based on your calculations above.
*/
chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort);
}
}
@ -105,4 +97,5 @@ void Robot::HandleDestination(void)
/**
* TODO: Stop and change state. Turn off LED.
*/
EnterIdleState(); // TODO: sequence
}

3
src/robot.h
View file

@ -5,6 +5,9 @@
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)
const float LONGITUDINAL_kP = 3.0;
const float LATERAL_kP = 3.0 * DEG_TO_RAD;
class Robot
{
protected: