ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

follow sa path

Browse source
This commit is contained in:
Andy Killorin 2026-02-05 09:46:03 -05:00
parent b2d0b01232
commit bb33056071
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
3 changed files with 9 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

12
src/robot-nav.cpp
View file

@ -50,7 +50,7 @@ void Robot::SetDestination(const Pose& dest)
Pose transformed; Pose transformed;
// rotate clockwise //// rotate clockwise
transformed.x = dest.y; transformed.x = dest.y;
transformed.y = -dest.x; transformed.y = -dest.x;
transformed.theta = dest.theta; transformed.theta = dest.theta;
@ -104,12 +104,12 @@ float boost(float input, float boost) {
// y left // y left
// //
// goal y forward x right // goal y forward x right
const int PATH_LEN=4; const int PATH_LEN=3;
Pose PATH[PATH_LEN] = { Pose PATH[PATH_LEN] = {
Pose(0.0, 43.0, 0.0), Pose(0.0, 81.0, 0.0),
Pose(43.0, 0.0, 0.0), Pose(40.0, 40.0, M_PI/ 2.0),
Pose(43.0, 43.0, 0.0),
Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0),
}; };
@ -136,7 +136,7 @@ void Robot::DriveToPoint(void)
+ sin(currPose.theta) * (destPose.y - currPose.y); + sin(currPose.theta) * (destPose.y - currPose.y);
if (heading_err > 45.0) { if (heading_err > 15.0) {
forward_err = 0.0; forward_err = 0.0;
} }

1
src/robot.cpp
View file

@ -80,6 +80,7 @@ void Robot::RobotLoop(void)
if (chassis.buttonA.isPressed()) { if (chassis.buttonA.isPressed()) {
//Pose dest = Pose(0.0,43.0, 0.0); //Pose dest = Pose(0.0,43.0, 0.0);
//SetDestination(dest); //SetDestination(dest);
delay(1000);
DrivePath(); DrivePath();
} }
if (chassis.buttonB.isPressed()) { if (chassis.buttonB.isPressed()) {

2
src/robot.h
View file

@ -6,7 +6,7 @@ const float TARGET_SQ_DIST = pow(3.0, 2); // distance from setpoint that is ok (
const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad) const float TARGET_RAD_ERR = 0.25; // angle from setpoint that is ok (rad)
const float LONGITUDINAL_kP = 4.0; const float LONGITUDINAL_kP = 4.0;
const float LATERAL_kP = 1.5 * RAD_TO_DEG; const float LATERAL_kP = 1.7 * RAD_TO_DEG;
const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives const Pose offsetPose = Pose(100.0, 100.0, 0.0); // hack to avoid negatives