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;
float lastError = 0;
void Robot::LineFollowingUpdate(void)
{
if(robotState == ROBOT_LINING)
{
// TODO: calculate the error in CalcError(), calc the effort, and update the motion
float lineError = 3.5 - lineSensor.CalcError();
Serial.println(lineError);
float turnEffort = lineError * lining_kP;
float setpoint = 3.5;
float lineError = setpoint - lineSensor.CalcError();
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;
speed *= 1 - (abs(rollingTurnRate) * KTurnRate);
if (!lineSensor.LineDetected()) {
lineLostFrames -= 1;
if (lineLostFrames < 0) {

6
src/robot.h
View file

@ -3,7 +3,7 @@
#include <LineSensor.h>
#include <LSM6.h>
static float lining_kP = 1.0;
static float lining_kP = 1.24;
class Robot
{
@ -57,6 +57,10 @@ protected:
/* baseSpeed is used to drive at a given speed while, say, line following.*/
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
* from and the one we're headed to. You'll program in the map in handleIntersection()