ank/RBE2002
1
Fork 0
Code Issues Activity

added feed forward term to pid

Browse source
This commit is contained in:
Andy Killorin 2024-10-23 14:29:57 -04:00
parent 6066594f6a
commit 0336639c9c
Signed by: ank
GPG key ID: 23F9463ECB67FE8C
4 changed files with 10 additions and 1 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

1
lib/Chassis/src/chassis.cpp
View file

@ -7,6 +7,7 @@ Romi32U4EncodedMotor<RIGHT_XOR, RIGHT_B, PWM_R, DIR_R, OCR_R> rightMotor;
void Chassis::SetMotorKp(float kp) {leftMotor.Kp = kp; rightMotor.Kp = kp;}
void Chassis::SetMotorKi(float ki) {leftMotor.Ki = ki; rightMotor.Ki = ki; leftMotor.sumError = 0; rightMotor.sumError = 0;}
void Chassis::SetMotorKd(float kd) {leftMotor.Kd = kd; rightMotor.Kd = kd;}
void Chassis::SetMotorKf(float kf) {leftMotor.Kf = kf; rightMotor.Kf = kf;}
/**
* Because it's declared static, we initialize Chassis::loopFlag here.

1
lib/Chassis/src/chassis.h
View file

@ -42,6 +42,7 @@ public:
void SetMotorKp(float kp);
void SetMotorKi(float ki);
void SetMotorKd(float kd);
void SetMotorKf(float kf);
bool CheckChassisTimer(void);

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

@ -28,6 +28,7 @@ protected:
float Kp = 1;
float Ki = 0;
float Kd = 0;
float Kf = 0;
// Used to keep track of the target speed, in counts / interval.
float targetSpeed = 0;
@ -129,7 +130,7 @@ protected:
prevError = error;
// Calculate the effort from the PID gains
int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff;
int16_t effort = Kp * error + Ki * sumError + Kd * errorDiff + Kf * targetSpeed;
// Set the effort for the motor
SetEffort(effort);

6
src/robot-remote.cpp
View file

@ -119,6 +119,12 @@ void Robot::HandleKeyCode(int16_t keyCode)
chassis.SetMotorKd(k);
keyString = "";
break;
case RIGHT_ARROW:
k = keyString.toInt() / 100.0;
Serial.print("Kf = "); Serial.println(k);
chassis.SetMotorKf(k);
keyString = "";
break;
case UP_ARROW:
if(!keyString.length()) chassis.SetWheelSpeeds(60, 0);
break;