ank/RBE2002
1
Fork 0
Code Issues Activity

swerve termporarily on tracking loss

Browse source
This commit is contained in:
Andy Killorin 2024-10-25 16:35:38 -04:00
parent dd7ff23be4
commit d3ed3f4093
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 13 additions and 2 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

15
src/robot.cpp
View file

@ -99,18 +99,29 @@ void Robot::EnterLineFollowing(float speed)
int lineLostFrames = 24;
float lastError = 0;
void Robot::LineFollowingUpdate(void)
{
if(robotState == ROBOT_LINING)
{
float setpoint = 3.5;
float setpoint = rollingTurnRate > 0.1 ? 4 : 3;
float lineError = setpoint - lineSensor.CalcError();
float powErr = (lineError*lineError);
powErr = (lineError < 0 ? -powErr : powErr);
float turnEffort = powErr * lining_kP;
float errDiff = lastError - lineError;
if (lineSensor.LineDetected()) {
lastError = lineError;
} else {
errDiff = 0;
lineError = lastError > 0 ? 7:-7;
}
float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD;
rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);