ank/RBE2002
1
Fork 0
Code Issues Activity

added constant term

Browse source
This commit is contained in:
Andy Killorin 2024-10-24 13:40:57 -04:00
parent 3f2727464b
commit 043bb622fb
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 15 additions and 3 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

18
lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
View file

@ -24,11 +24,11 @@ protected:
enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED}; enum CTRL_MODE : uint8_t {CTRL_DIRECT, CTRL_SPEED};
volatile CTRL_MODE ctrlMode = CTRL_DIRECT; volatile CTRL_MODE ctrlMode = CTRL_DIRECT;
// TODO: After you tune your motors, set the gains here. float Kp = 2.50; // proportional to error
float Kp = 2.50;
float Ki = 0.03; float Ki = 0.03;
float Kd = 0; float Kd = 0;
float Kf = 3.00; float Kf = 3.00; // proportional to setpoint
int16_t C = 22; // constant added to effort
// Used to keep track of the target speed, in counts / interval. // Used to keep track of the target speed, in counts / interval.
float targetSpeed = 0; float targetSpeed = 0;
@ -140,6 +140,18 @@ protected:
// Calculate the effort from the PID gains // Calculate the effort from the PID gains
int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed; int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed;
// increase effort to match static friction
if (effort > 0) {
effort += C;
} else if (effort < 0) {
effort -= C;
}
// stop motor if halted and stopped
if (abs(error) < 3. && targetSpeed == 0) {
effort = 0;
}
// Set the effort for the motor // Set the effort for the motor
SetEffort(effort); SetEffort(effort);