ank/RBE2001-lab2
1
Fork 0
Code Releases Activity

add static friction feedforward

Browse source
This commit is contained in:
Andy Killorin 2026-02-05 08:35:43 -05:00
parent 90acef4110
commit ab507bb68a
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 15 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

16
src/robot-nav.cpp
View file

@ -73,6 +73,18 @@ bool Robot::CheckReachedDestination(void)
&& RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR; && RadError(currPose.theta, destPose.theta) < TARGET_RAD_ERR;
} }
float boost(float input, float boost) {
if (input == 0.0) {
return input;
}
if (input > 0.0) {
input += boost;
} else {
input -= boost;
}
return input;
}
void Robot::DriveToPoint(void) void Robot::DriveToPoint(void)
{ {
if(robotState == ROBOT_DRIVE_TO_POINT) if(robotState == ROBOT_DRIVE_TO_POINT)
@ -100,7 +112,9 @@ void Robot::DriveToPoint(void)
TeleplotPrint("linear_dist", SquareDistance()); TeleplotPrint("linear_dist", SquareDistance());
#endif #endif
chassis.SetMotorEfforts(forward_effort - turn_effort, forward_effort + turn_effort); //chassis.SetMotorEfforts(20, 20);
float boost_f = 15.0;
chassis.SetMotorEfforts(boost(forward_effort - turn_effort,boost_f), boost(forward_effort + turn_effort, boost_f));
} }
} }