added feed forward term to pid

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.

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);
 

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);

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;