diff --git a/lib/Chassis/src/constants.h b/lib/Chassis/src/constants.h index 435a7d2..3be7c34 100644 --- a/lib/Chassis/src/constants.h +++ b/lib/Chassis/src/constants.h @@ -7,8 +7,8 @@ * tune them. */ const float ROBOT_RADIUS = 14.7 / 2; -const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0); -const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 4.0); +const float LEFT_TICKS_PER_CM = 1440.0 / (3.1416 * 7); +const float RIGHT_TICKS_PER_CM = 1440.0 / (3.1416 * 7); /** * You can change the control loop period, but you should use multiples of 4 ms to diff --git a/lib/LineSensor/src/LineSensor.cpp b/lib/LineSensor/src/LineSensor.cpp index 2a800e5..879fee1 100644 --- a/lib/LineSensor/src/LineSensor.cpp +++ b/lib/LineSensor/src/LineSensor.cpp @@ -2,7 +2,7 @@ #define DARK_THRESHOLD 500; -#define LINE_THRESHOLD 200; +#define LINE_THRESHOLD 110; #define INTERSECTION_THRESHOLD 500; void LineSensor::Initialize(void) diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h index 5469c4f..c347b63 100644 --- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h +++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h @@ -34,7 +34,7 @@ protected: // Used to keep track of the target speed, in counts / interval. float targetSpeed = 0; - float TRAPEZOIDAL_RAMP_RATE_CM_S = 30.; + float TRAPEZOIDAL_RAMP_RATE_CM_S = 10.; // maximum accelration in counts/interval float TRAPEZOIDAL_RAMP_RATE = TRAPEZOIDAL_RAMP_RATE_CM_S / LEFT_TICKS_PER_CM / CONTROL_LOOP_PERIOD_S; float currentSetpoint = 0; diff --git a/src/robot.cpp b/src/robot.cpp index cf0a9c3..5f23335 100644 --- a/src/robot.cpp +++ b/src/robot.cpp @@ -123,6 +123,18 @@ void Robot::LineFollowingUpdate(void) float turnEffort = powErr * lining_kP2 + lineError * lining_kP + errDiff * lining_kD; +#ifdef __LINING_DEBUG__ + Serial.print(lineSensor.CalcError()); + Serial.print(' '); + Serial.print(lineError * lining_kP); + Serial.print(' '); + Serial.print(errDiff * lining_kD); + Serial.print(' '); + Serial.print(turnEffort); + Serial.print(' '); + Serial.println(lineSensor.AverageReflectance()); +#endif + rollingTurnRate = rollingTurnRate * turnRateDamp + turnEffort * (1-turnRateDamp); float speed = baseSpeed; diff --git a/src/robot.h b/src/robot.h index 1823c2a..99def59 100644 --- a/src/robot.h +++ b/src/robot.h @@ -3,7 +3,9 @@ #include #include -static float lining_kP = 1.24; +static float lining_kP2 = 0.67; +static float lining_kP = 0.72; +static float lining_kD = -1.2; class Robot { @@ -58,8 +60,8 @@ protected: 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 + float turnRateDamp = 0.975; // the proportion of the turn rate retained each loop + float KTurnRate = 0.18; // slowdown coefficient /** * For tracking the motion of the Romi. We keep track of the intersection we came