Add ff settings

src/simulator/Character.java

@@ -1,9 +1,15 @@
 package simulator;
 
+import simulator.control.FeedForwardController;
+import simulator.control.FeedForwardSettings;
+import simulator.control.PIDController;
+import simulator.control.PIDSettings;
+
 import java.awt.*;
 
 public class Character extends Rectangle {
 
+    private static final int MILLISECONDS_IN_SECOND = 1000;
     private static final int WIDTH = 32;
     private static final int HEIGHT = 32;
 
@@ -14,21 +20,17 @@ public class Character extends Rectangle {
     private static final int MAX_ACCELERATION = 2;
     private static final int FRICTION = 3;
 
-    private double kP;
-    private double kI;
-    private double kD;
-
-    private static final int I_ZONE = 200;
+    private final PIDSettings pidSettings;
+    private final PIDController pidController;
+    private final FeedForwardSettings feedForwardSettings;
+    private final FeedForwardController feedForwardController;
 
-    private static Character instance;
 
-    private double errorSum;
-    private double lastTimestamp;
-    private double lastError;
+    private double lastTimeNotOnTarget;
     private double lastSpeed;
-    private double error;
-    private double dt;
-    private double errorRate;
+    private boolean commandFinished;
+
+    private static Character instance;
 
     public static Character getInstance() {
         if (instance == null) {
@@ -39,37 +41,39 @@ public static Character getInstance() {
 
     private Character(int width, int height, int sourceX, int sourceY) {
         super(sourceX, sourceY, width, height);
-        errorSum = 0;
-        lastTimestamp = System.currentTimeMillis();
-        lastError = Setpoint.getInstance().x - this.x;
         lastSpeed = 0;
-        kP = 0;
-        kI = 0;
-        kD = 0;
+        pidSettings = new PIDSettings(0, 0, 0, 10, 1);
+        feedForwardSettings = new FeedForwardSettings(0, 0, 0);
+        pidController = new PIDController(pidSettings);
+        feedForwardController = new FeedForwardController(feedForwardSettings);
+        lastTimeNotOnTarget = System.currentTimeMillis();
     }
 
     public void update() {
-        error = Setpoint.getInstance().x - this.x;
-        dt = System.currentTimeMillis() - lastTimestamp;
-        errorRate = (error - lastError) / dt;
-        if (Math.abs(error) < I_ZONE) errorSum += error;
-        int moveValue = (int) (error * kP + errorSum * kI + errorRate * kD);
-        moveValue = (int) normalizeSpeed(moveValue);
-        if (Math.abs(moveValue - lastSpeed) > MAX_ACCELERATION) {
-            if (lastSpeed > moveValue) moveValue = (int) (lastSpeed - MAX_ACCELERATION);
-            if (lastSpeed < moveValue) moveValue = (int) (lastSpeed + MAX_ACCELERATION);
+        if (!commandFinished) {
+            commandFinished = (System.currentTimeMillis() - lastTimeNotOnTarget >=
+                    pidSettings.getWaitTime() * MILLISECONDS_IN_SECOND && pidController.isOnTarget());
+            int moveValue = pidController.calculate(this.x, Setpoint.getInstance().x) +
+                    feedForwardController.calculate(this.x, Setpoint.getInstance().x);
+            moveValue = (int) normalizeSpeed(moveValue);
+            if (Math.abs(moveValue - lastSpeed) > MAX_ACCELERATION) {
+                if (lastSpeed > moveValue) moveValue = (int) (lastSpeed - MAX_ACCELERATION);
+                if (lastSpeed < moveValue) moveValue = (int) (lastSpeed + MAX_ACCELERATION);
+            }
+            this.translate(moveValue, 0);
+            lastSpeed = moveValue;
+        }
+        if (!pidController.isOnTarget()) {
+            lastTimeNotOnTarget = System.currentTimeMillis();
         }
-        this.translate(moveValue, 0);
-        lastError = error;
-        lastTimestamp = System.currentTimeMillis();
-        lastSpeed = normalizeNoFriction(moveValue);
     }
 
     private double normalizeSpeed(double speed) {
-        if (speed > 0) speed -= FRICTION;
-        if (speed < 0) speed += FRICTION;
-        if (Math.abs(speed) > MAX_SPEED) speed = (int) (MAX_SPEED * Math.signum(speed));
-        if (Math.abs(speed) < FRICTION) speed = 0;
+        if (speed != 0) {
+            if (speed > 0) speed -= FRICTION;
+            if (speed < 0) speed += FRICTION;
+            if (Math.abs(speed) > MAX_SPEED) speed = (int) (MAX_SPEED * Math.signum(speed));
+        } else if (Math.abs(speed) < FRICTION) speed = 0;
         return speed;
     }
 
@@ -80,37 +84,33 @@ private double normalizeNoFriction(double speed) {
 
     public static void reset() {
         Character character = getInstance();
-        character.errorSum = 0;
-        character.lastTimestamp = System.currentTimeMillis();
-        character.lastError = Setpoint.getInstance().x - character.x;
         character.lastSpeed = 0;
         character.setLocation(SOURCE_X, SOURCE_Y);
+        character.commandFinished = false;
+        character.lastTimeNotOnTarget = System.currentTimeMillis();
+        character.pidController.reset();
+        character.feedForwardController.reset();
     }
 
-    public void setP(double kP) {
-        this.kP = kP;
+    public void setPID(double kP, double kI, double kD, double tolerance, double waitTime) {
+        pidController.setPID(kP, kI, kD);
+        pidController.setTolerance(tolerance);
+        pidSettings.setWaitTime(waitTime);
     }
 
-    public void setI(double kI) {
-        this.kI = kI;
+    public void setIZone(int iZone) {
+        pidController.setIZone(iZone);
     }
 
-    public void setD(double kD) {
-        this.kD = kD;
-    }
-
-    public void setPID(double kP, double kI, double kD) {
-        setP(kP);
-        setI(kI);
-        setD(kD);
+    public PIDController getPIDController() {
+        return pidController;
     }
 
     public double getError() {
-        return error;
+        return Setpoint.getInstance().x - this.x;
     }
 
-    public double getRate() {
-        return errorRate;
+    public void setFF(double kS, double kV, double kA) {
+        feedForwardController.setGains(kS, kV, kA);
     }
-
 }

src/simulator/Setpoint.java

@@ -22,4 +22,8 @@ public static Setpoint getInstance() {
     private Setpoint(int width, int height, int sourceX, int sourceY) {
         super(sourceX, sourceY, width, height);
     }
+
+    public void setPosition(int position) {
+        this.x = position;
+    }
 }

src/simulator/Window.java

@@ -8,9 +8,10 @@
 
 public class Window extends JPanel {
 
+    public static final double PERIODIC_FRAME = 0.02;
+
     private static final int WINDOW_WIDTH = 1280;
     private static final int WINDOW_HEIGHT = 792;
-
     private static final boolean IS_DOUBLE_BUFFERED = true;
 
     private final Character character;
@@ -19,6 +20,13 @@ public class Window extends JPanel {
     private final BaseTextField kPField;
     private final BaseTextField kIField;
     private final BaseTextField kDField;
+    private final BaseTextField iZoneField;
+    private final BaseTextField toleranceField;
+    private final BaseTextField waitTimeField;
+    private final BaseTextField kSField;
+    private final BaseTextField kVField;
+    private final BaseTextField kAField;
+    private final BaseTextField setpointField;
     private final Status status;
 
     private Window() {
@@ -34,11 +42,15 @@ private Window() {
         kPField = new BaseTextField("kP", 300, 30);
         kIField = new BaseTextField("kI", 600, 30);
         kDField = new BaseTextField("kD", 900, 30);
+        iZoneField = new BaseTextField("i zone", 30, 30);
+        toleranceField = new BaseTextField("tolerance", 450, 65);
+        waitTimeField = new BaseTextField("wait time", 750, 65);
+        kSField = new BaseTextField("kS", 300, 100);
+        kVField = new BaseTextField("kV", 600, 100);
+        kAField = new BaseTextField("kA", 900, 100);
+        setpointField = new BaseTextField("setpoint", 30, 722);
+        configureTextFields();
         this.add(rerunButton);
-        this.add(kPField);
-        this.add(kIField);
-        this.add(kDField);
-        this.add(status);
     }
 
     @Override
@@ -61,13 +73,31 @@ private void delay(double seconds) {
     }
 
     private void update() {
-        delay(0.02);
+        delay(PERIODIC_FRAME);
+        setpoint.setPosition((int) setpointField.getValue());
+        character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue(), toleranceField.getValue(),
+                waitTimeField.getValue());
+        character.setIZone((int) iZoneField.getValue());
+        character.setFF(kSField.getValue(), kVField.getValue(), kAField.getValue());
         character.update();
-        character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue());
         status.update();
         repaint();
     }
 
+    private void configureTextFields() {
+        this.add(kPField);
+        this.add(kIField);
+        this.add(kDField);
+        this.add(toleranceField);
+        this.add(waitTimeField);
+        this.add(iZoneField);
+        this.add(kSField);
+        this.add(kVField);
+        this.add(kAField);
+        this.add(status);
+        this.add(setpointField);
+    }
+
     public static void initGame() {
         JFrame frame = new JFrame();
         Window window = new Window();

src/simulator/control/FeedForwardController.java

@@ -0,0 +1,68 @@
+package simulator.control;
+
+import simulator.Window;
+
+public class FeedForwardController {
+
+    private double kS;
+    private double kV;
+    private double kA;
+    private double previousTarget;
+
+    public FeedForwardController(double kS, double kV, double kA) {
+        this.kS = kS;
+        this.kV = kV;
+        this.kA = kA;
+        this.previousTarget = 0;
+    }
+
+    public FeedForwardController(FeedForwardSettings feedForwardSettings) {
+        this(feedForwardSettings.getS(), feedForwardSettings.getV(), feedForwardSettings.getA());
+    }
+
+    public void setGains(double kS, double kV, double kA) {
+        this.kS = kS;
+        this.kV = kV;
+        this.kA = kA;
+    }
+
+    public void setGains(FeedForwardSettings feedForwardSettings) {
+        setGains(feedForwardSettings.getS(), feedForwardSettings.getV(), feedForwardSettings.getA());
+    }
+
+    public double getS() {
+        return kS;
+    }
+
+    public void setS(double kS) {
+        this.kS = kS;
+    }
+
+    public double getV() {
+        return kV;
+    }
+
+    public void setV(double kV) {
+        this.kV = kV;
+    }
+
+    public double getA() {
+        return kA;
+    }
+
+    public void setA(double kA) {
+        this.kA = kA;
+    }
+
+
+    public void reset() {
+        this.previousTarget = 0;
+    }
+
+    public int calculate(double source, double setpoint) {
+        double error = setpoint - source;
+        double targetDerivative = (setpoint - previousTarget) / Window.PERIODIC_FRAME;
+        previousTarget = setpoint;
+        return (int) (kS * Math.signum(error) + kV * setpoint + kA * targetDerivative);
+    }
+}

src/simulator/control/FeedForwardSettings.java

@@ -0,0 +1,44 @@
+package simulator.control;
+
+public class FeedForwardSettings {
+
+    private double kS;
+    private double kV;
+    private double kA;
+
+    public FeedForwardSettings(double kS, double kV, double kA) {
+        this.kS = kS;
+        this.kV = kV;
+        this.kA = kA;
+    }
+
+    public double getS() {
+        return kS;
+    }
+
+    public void setS(double kS) {
+        this.kS = kS;
+    }
+
+    public double getV() {
+        return kV;
+    }
+
+    public void setV(double kV) {
+        this.kV = kV;
+    }
+
+    public double getA() {
+        return kA;
+    }
+
+    public void setA(double kA) {
+        this.kA = kA;
+    }
+
+    public void setFF(double kS, double kV, double kA) {
+        setS(kS);
+        setV(kV);
+        setA(kA);
+    }
+}