ank/RBE2002
1
Fork 0
Code Issues Activity

slow down on turns

Browse source
This commit is contained in:
Andy Killorin 2024-10-25 13:29:23 -04:00
parent ba1dee7913
commit cbf38e4fd6
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
2 changed files with 17 additions and 7 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
src/robot.cpp
View file

@ -98,20 +98,26 @@ void Robot::EnterLineFollowing(float speed)
} }
int lineLostFrames = 7; int lineLostFrames = 7;
float lastError = 0;
void Robot::LineFollowingUpdate(void) void Robot::LineFollowingUpdate(void)
{ {
if(robotState == ROBOT_LINING) if(robotState == ROBOT_LINING)
{ {
// TODO: calculate the error in CalcError(), calc the effort, and update the motion float setpoint = 3.5;
float lineError = 3.5 - lineSensor.CalcError();
Serial.println(lineError); float lineError = setpoint - lineSensor.CalcError();
float turnEffort = lineError * lining_kP;
float powErr = (lineError*lineError);
powErr = (lineError < 0 ? -powErr : powErr);
float turnEffort = powErr * lining_kP;
rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp);
chassis.SetTwist(baseSpeed, turnEffort);
float speed = baseSpeed; float speed = baseSpeed;
speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
if (!lineSensor.LineDetected()) { if (!lineSensor.LineDetected()) {
lineLostFrames -= 1; lineLostFrames -= 1;
if (lineLostFrames < 0) { if (lineLostFrames < 0) {

6
src/robot.h
View file

@ -3,7 +3,7 @@
#include <LineSensor.h> #include <LineSensor.h>
#include <LSM6.h> #include <LSM6.h>
static float lining_kP = 1.0; static float lining_kP = 1.24;
class Robot class Robot
{ {
@ -57,6 +57,10 @@ protected:
/* baseSpeed is used to drive at a given speed while, say, line following.*/ /* baseSpeed is used to drive at a given speed while, say, line following.*/
float baseSpeed = 0; float baseSpeed = 0;
float rollingTurnRate = 0;
float turnRateDamp = 0.96; // the proportion of the turn rate retained each loop
float KTurnRate = 0.25; // slowdown coefficient
/** /**
* For tracking the motion of the Romi. We keep track of the intersection we came * For tracking the motion of the Romi. We keep track of the intersection we came
* from and the one we're headed to. You'll program in the map in handleIntersection() * from and the one we're headed to. You'll program in the map in handleIntersection()