ank/RBE2002
1
Fork 0
Code Issues Activity

naive pid impl

Browse source
This commit is contained in:
Andy Killorin 2024-10-23 11:49:39 -04:00
parent 0534981d4b
commit 4ca507605a
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
1 changed files with 7 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

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

@ -122,8 +122,14 @@ protected:
// Calculate the error in speed // Calculate the error in speed
float error = targetSpeed - speed; float error = targetSpeed - speed;
sumError += error;
float errorDiff = error - prevError;
prevError = error;
// Calculate the effort from the PID gains // Calculate the effort from the PID gains
int16_t effort = Kp * error; int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff;
// Set the effort for the motor // Set the effort for the motor
SetEffort(effort); SetEffort(effort);