From 0336639c9c5cf5fdd62b106cc9844550b48faa97 Mon Sep 17 00:00:00 2001
From: Andy Killorin <37423245+Speedy6451@users.noreply.github.com>
Date: Wed, 23 Oct 2024 14:29:57 -0400
Subject: [PATCH] added feed forward term to pid

---
 lib/Chassis/src/chassis.cpp                    | 1 +
 lib/Chassis/src/chassis.h                      | 1 +
 lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h | 3 ++-
 src/robot-remote.cpp                           | 6 ++++++
 4 files changed, 10 insertions(+), 1 deletion(-)

diff --git a/lib/Chassis/src/chassis.cpp b/lib/Chassis/src/chassis.cpp
index ba1d62f..fb0200b 100644
--- a/lib/Chassis/src/chassis.cpp
+++ b/lib/Chassis/src/chassis.cpp
@@ -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.
diff --git a/lib/Chassis/src/chassis.h b/lib/Chassis/src/chassis.h
index 1acbc8b..ddb109e 100644
--- a/lib/Chassis/src/chassis.h
+++ b/lib/Chassis/src/chassis.h
@@ -42,6 +42,7 @@ public:
     void SetMotorKp(float kp);
     void SetMotorKi(float ki);
     void SetMotorKd(float kd);
+    void SetMotorKf(float kf);
 
     bool CheckChassisTimer(void);
 
diff --git a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
index c065b29..781b0a4 100644
--- a/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
+++ b/lib/Romi32U4Motors/src/Romi32U4MotorTemplate.h
@@ -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);
diff --git a/src/robot-remote.cpp b/src/robot-remote.cpp
index 1b2a86c..67adbc4 100644
--- a/src/robot-remote.cpp
+++ b/src/robot-remote.cpp
@@ -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;