added velocity control

src/simulator/Character.java

@@ -1,9 +1,6 @@
 package simulator;
 
-import simulator.control.FeedForwardController;
-import simulator.control.FeedForwardSettings;
-import simulator.control.PIDController;
-import simulator.control.PIDSettings;
+import simulator.control.*;
 
 import java.awt.*;
 
@@ -13,8 +10,11 @@ public class Character extends Rectangle {
     private static final int WIDTH = 32;
     private static final int HEIGHT = 32;
 
-    private static final int SOURCE_X = 0;
-    private static final int SOURCE_Y = 320;
+    private static final int POSITION_SOURCE_X = 0;
+    private static final int POSITION_SOURCE_Y = 320;
+
+    private static final int VELOCITY_SOURCE_X = 640;
+    private static final int VELOCITY_SOURCE_Y = 320;
 
     private static final int MAX_SPEED = 120;
     private static final int MAX_ACCELERATION = 2;
@@ -25,7 +25,6 @@ public class Character extends Rectangle {
     private final FeedForwardSettings feedForwardSettings;
     private final FeedForwardController feedForwardController;
 
-
     private double lastTimeNotOnTarget;
     private double lastSpeed;
     private boolean commandFinished;
@@ -34,7 +33,7 @@ public class Character extends Rectangle {
 
     public static Character getInstance() {
         if (instance == null) {
-            instance = new Character(WIDTH, HEIGHT, SOURCE_X, SOURCE_Y);
+            instance = new Character(WIDTH, HEIGHT, POSITION_SOURCE_X, POSITION_SOURCE_Y);
         }
         return instance;
     }
@@ -49,49 +48,35 @@ private Character(int width, int height, int sourceX, int sourceY) {
         lastTimeNotOnTarget = System.currentTimeMillis();
     }
 
-    public void update() {
-        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();
-        }
-    }
-
-    private double normalizeSpeed(double speed) {
-        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;
-    }
-
-    private double normalizeNoFriction(double speed) {
-        if (Math.abs(speed) > MAX_SPEED) speed = (int) (MAX_SPEED * Math.signum(speed));
-        return speed;
-    }
-
     public static void reset() {
         Character character = getInstance();
         character.lastSpeed = 0;
-        character.setLocation(SOURCE_X, SOURCE_Y);
+        if (ControlType.getInstance().getSelectedIndex() == 0) {
+            character.setLocation(POSITION_SOURCE_X, POSITION_SOURCE_Y);
+        } else {
+            if (ControlType.getInstance().getSelectedIndex() == 1) {
+                character.setLocation(VELOCITY_SOURCE_X, VELOCITY_SOURCE_Y);
+            }
+        }
         character.commandFinished = false;
         character.lastTimeNotOnTarget = System.currentTimeMillis();
         character.pidController.reset();
         character.feedForwardController.reset();
     }
 
+    public void update() {
+        if (!commandFinished) {
+            if (ControlType.getInstance().getSelectedIndex() == 0) {
+                runPosition();
+            } else if (ControlType.getInstance().getSelectedIndex() == 1) {
+                runVelocity();
+            }
+        }
+        if (!pidController.isOnTarget()) {
+            lastTimeNotOnTarget = System.currentTimeMillis();
+        }
+    }
+
     public void setPID(double kP, double kI, double kD, double tolerance, double waitTime) {
         pidController.setPID(kP, kI, kD);
         pidController.setTolerance(tolerance);
@@ -113,4 +98,47 @@ public double getError() {
     public void setFF(double kS, double kV, double kA) {
         feedForwardController.setGains(kS, kV, kA);
     }
+
+    public double getLastSpeed() {
+        return lastSpeed;
+    }
+
+    private void runPosition() {
+        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;
+    }
+
+    private void runVelocity() {
+        int moveValue = pidController.calculate(lastSpeed, Setpoint.getInstance().x) +
+                feedForwardController.calculate(lastSpeed, 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);
+        }
+        lastSpeed = moveValue;
+    }
+
+    private double normalizeSpeed(double speed) {
+        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;
+    }
+
+    private double normalizeNoFriction(double speed) {
+        if (Math.abs(speed) > MAX_SPEED) speed = (int) (MAX_SPEED * Math.signum(speed));
+        return speed;
+    }
 }

src/simulator/Setpoint.java

@@ -1,5 +1,7 @@
 package simulator;
 
+import simulator.control.ControlType;
+
 import java.awt.*;
 
 public class Setpoint extends Rectangle {
@@ -23,7 +25,12 @@ private Setpoint(int width, int height, int sourceX, int sourceY) {
         super(sourceX, sourceY, width, height);
     }
 
-    public void setPosition(int position) {
+    public void setSetpoint(int position) {
         this.x = position;
+        if (ControlType.getInstance().getSelectedIndex() != 0) {
+            this.setSize(-1, -1);
+        } else {
+            this.setSize(WIDTH, HEIGHT);
+        }
     }
 }

src/simulator/Window.java

@@ -1,5 +1,6 @@
 package simulator;
 
+import simulator.control.ControlType;
 import simulator.information.Status;
 import simulator.textfields.BaseTextField;
 
@@ -27,6 +28,7 @@ public class Window extends JPanel {
     private final BaseTextField kVField;
     private final BaseTextField kAField;
     private final BaseTextField setpointField;
+    private final ControlType controlType;
     private final Status status;
 
     private Window() {
@@ -49,8 +51,22 @@ private Window() {
         kVField = new BaseTextField("kV", 600, 100);
         kAField = new BaseTextField("kA", 900, 100);
         setpointField = new BaseTextField("setpoint", 30, 722);
+        controlType = ControlType.getInstance();
         configureTextFields();
         this.add(rerunButton);
+        this.add(controlType);
+    }
+
+    public static void initGame() {
+        JFrame frame = new JFrame();
+        Window window = new Window();
+        frame.add(window);
+        frame.pack();
+        frame.setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE);
+        frame.setVisible(true);
+        while (true) {
+            window.update();
+        }
     }
 
     @Override
@@ -59,6 +75,9 @@ public void paintComponent(Graphics g) {
         Graphics2D g2 = (Graphics2D) g;
         g2.setColor(Color.WHITE);
         g2.fill(character);
+        if (ControlType.getInstance().getSelectedIndex() == 1) {
+            g2.drawString("Velocity: " + character.getLastSpeed(), character.x, character.y);
+        }
         g2.setColor(Color.RED);
         g2.fill(setpoint);
     }
@@ -74,14 +93,18 @@ private void delay(double seconds) {
 
     private void update() {
         delay(PERIODIC_FRAME);
-        setpoint.setPosition((int) setpointField.getValue());
+        configurePIDF();
+        character.update();
+        status.update();
+        repaint();
+    }
+
+    private void configurePIDF() {
+        setpoint.setSetpoint((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();
-        status.update();
-        repaint();
     }
 
     private void configureTextFields() {
@@ -97,16 +120,4 @@ private void configureTextFields() {
         this.add(status);
         this.add(setpointField);
     }
-
-    public static void initGame() {
-        JFrame frame = new JFrame();
-        Window window = new Window();
-        frame.add(window);
-        frame.pack();
-        frame.setDefaultCloseOperation(WindowConstants.EXIT_ON_CLOSE);
-        frame.setVisible(true);
-        while (true) {
-            window.update();
-        }
-    }
 }

src/simulator/control/ControlType.java

@@ -0,0 +1,37 @@
+package simulator.control;
+
+import simulator.Character;
+
+import javax.swing.*;
+import java.awt.*;
+import java.awt.event.ActionEvent;
+import java.awt.event.ActionListener;
+
+public class ControlType extends JComboBox<String> {
+
+    private static final int WIDTH = 140;
+    private static final int HEIGHT = 65;
+
+    private static final int X = 1100;
+    private static final int Y = 65;
+    private static final int FONT_SIZE = 20;
+
+    private static final String[] OPTIONS = new String[]{"Position", "Velocity"};
+
+    private static ControlType instance;
+
+    public static ControlType getInstance() {
+        if (instance == null) {
+            instance = new ControlType();
+        }
+        return instance;
+    }
+
+    private ControlType() {
+        super(OPTIONS);
+        this.setBounds(X, Y, WIDTH, HEIGHT);
+        this.setBackground(Color.GRAY);
+        this.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE));
+        addActionListener(e -> Character.reset());
+    }
+}

src/simulator/control/FeedForwardController.java

@@ -63,6 +63,11 @@ 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);
+        double staticVal;
+        if (ControlType.getInstance().getSelectedIndex() == 1) {
+            return (int) (kS + kV * setpoint + kA * targetDerivative);
+        } else {
+            return (int) (kS * Math.signum(error) + kV * setpoint + kA * targetDerivative);
+        }
     }
 }

src/simulator/information/Status.java

@@ -2,6 +2,7 @@
 
 import simulator.Character;
 import simulator.Setpoint;
+import simulator.control.ControlType;
 
 import javax.swing.*;
 import javax.swing.border.LineBorder;
@@ -40,7 +41,14 @@ private Status() {
     }
 
     public void update() {
-        characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().x));
-        errorRate.setText("Error rate: " + (Character.getInstance().getPIDController().getErrorRate()));
+        if (ControlType.getInstance().getSelectedIndex() == 0) {
+            characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().x));
+            errorRate.setText("Error rate: " + (Character.getInstance().getPIDController().getErrorRate()));
+        } else {
+            if (ControlType.getInstance().getSelectedIndex() == 1) {
+                characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().getLastSpeed()));
+                errorRate.setText("Error rate: " + (Character.getInstance().getPIDController().getErrorRate()));
+            }
+        }
     }
 }