diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 829c149..27ce108 100644 --- a/src/simulator/Character.java +++ b/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); } - } diff --git a/src/simulator/Setpoint.java b/src/simulator/Setpoint.java index e3d7051..a058335 100644 --- a/src/simulator/Setpoint.java +++ b/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; + } } diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 15db388..a0a2f9a 100644 --- a/src/simulator/Window.java +++ b/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(); diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java new file mode 100644 index 0000000..920ea35 --- /dev/null +++ b/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); + } +} diff --git a/src/simulator/control/FeedForwardSettings.java b/src/simulator/control/FeedForwardSettings.java new file mode 100644 index 0000000..8331868 --- /dev/null +++ b/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); + } +} diff --git a/src/simulator/control/PIDController.java b/src/simulator/control/PIDController.java new file mode 100644 index 0000000..903dac0 --- /dev/null +++ b/src/simulator/control/PIDController.java @@ -0,0 +1,100 @@ +package simulator.control; + +public class PIDController { + + private static final int DEFAULT_I_ZONE = 200; + + private double kP; + private double kI; + private double kD; + private double iZone; + private double tolerance; + private double waitTime; + + private double errorSum; + private double lastTimestamp; + private double lastError; + private double lastSpeed; + private double error; + private double dt; + private double errorRate; + private boolean onTarget; + + public PIDController(PIDSettings pidSettings) { + this.kP = pidSettings.getP(); + this.kI = pidSettings.getI(); + this.kD = pidSettings.getD(); + this.iZone = DEFAULT_I_ZONE; + this.tolerance = pidSettings.getTolerance(); + this.waitTime = pidSettings.getWaitTime(); + errorSum = 0; + lastTimestamp = System.currentTimeMillis(); + lastError = 0; + errorRate = 0; + } + + public PIDController(double kP, double kI, double kD, double tolerance, double waitTime) { + this(new PIDSettings(kP, kI, kD, tolerance, waitTime)); + } + + public void setP(double kP) { + this.kP = kP; + } + + public void setI(double kI) { + this.kI = kI; + } + + public void setD(double kD) { + this.kD = kD; + } + + public void setPID(double kP, double kI, double kD) { + setP(kP); + setI(kI); + setD(kD); + } + + public void setTolerance(double tolerance) { + this.tolerance = tolerance; + } + + public void setWaitTime(double waitTime) { + this.waitTime = waitTime; + } + + public boolean isOnTarget() { + return onTarget; + } + + public void setPID(PIDSettings pidSettings) { + setPID(pidSettings.getP(), pidSettings.getI(), pidSettings.getD()); + } + + public void setIZone(int iZone) { + this.iZone = iZone; + } + + public int calculate(double source, double setpoint) { + error = setpoint - source; + onTarget = (Math.abs(error) <= tolerance); + dt = System.currentTimeMillis() - lastTimestamp; + errorRate = (error - lastError) / dt; + if (Math.abs(error) < iZone) errorSum += error; + int moveValue = (int) (error * kP + errorSum * kI + errorRate * kD); + lastError = error; + lastTimestamp = System.currentTimeMillis(); + return moveValue; + } + + public void reset() { + errorSum = 0; + lastTimestamp = System.currentTimeMillis(); + lastError = 0; + onTarget = false; + } + + public double getErrorRate() { + return errorRate; + } +} diff --git a/src/simulator/control/PIDSettings.java b/src/simulator/control/PIDSettings.java new file mode 100644 index 0000000..5671d6a --- /dev/null +++ b/src/simulator/control/PIDSettings.java @@ -0,0 +1,64 @@ +package simulator.control; + +public class PIDSettings { + + private double kP; + private double kI; + private double kD; + private double tolerance; + private double waitTime; + + public PIDSettings(double kP, double kI, double kD, double tolerance, double waitTime) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + this.tolerance = tolerance; + this.waitTime = waitTime; + } + + public double getP() { + return kP; + } + + public double getI() { + return kI; + } + + public double getD() { + return kD; + } + + public void setP(double kP) { + this.kP = kP; + } + + public void setI(double kI) { + this.kI = kI; + } + + public void setD(double kD) { + this.kD = kD; + } + + public double getTolerance() { + return this.tolerance; + } + + public double getWaitTime() { + return this.waitTime; + } + + public void setTolerance(double tolerance) { + this.tolerance = tolerance; + } + + public void setWaitTime(double waitTime) { + this.waitTime = waitTime; + } + + public void setPID(double kP, double kI, double kD) { + setP(kP); + setI(kI); + setD(kD); + } +} diff --git a/src/simulator/information/BaseInfoField.java b/src/simulator/information/BaseInfoField.java new file mode 100644 index 0000000..5ac7df9 --- /dev/null +++ b/src/simulator/information/BaseInfoField.java @@ -0,0 +1,22 @@ +package simulator.information; + +import simulator.Character; + +import javax.swing.*; +import javax.swing.border.LineBorder; +import java.awt.*; + +public class BaseInfoField extends JTextField { + + private static final int FONT_SIZE = 20; + + public BaseInfoField(int x, int y, int width, int height, String text) { + this.setText(text); + this.setBounds(x, y, width, height); + this.setEditable(false); + this.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); + this.setLayout(null); + this.setBackground(Color.GRAY); + this.setBorder(new LineBorder(Color.GRAY)); + } +} diff --git a/src/simulator/information/Status.java b/src/simulator/information/Status.java index a58d172..6f1264e 100644 --- a/src/simulator/information/Status.java +++ b/src/simulator/information/Status.java @@ -1,6 +1,7 @@ package simulator.information; import simulator.Character; +import simulator.Setpoint; import javax.swing.*; import javax.swing.border.LineBorder; @@ -13,8 +14,8 @@ public class Status extends JPanel { private static final int X = 800; private static final int Y = 480; private static final int FONT_SIZE = 20; - private JTextField characterError; - private JTextField errorRate; + private BaseInfoField characterError; + private BaseInfoField errorRate; private static Status instance; @@ -26,8 +27,10 @@ public static Status getInstance() { } private Status() { - configureCharacterError(); - configureErrorRate(); + characterError = new BaseInfoField(0, 0, 400, 50, + "Error: " + Character.getInstance().getError()); + errorRate = new BaseInfoField(0, 50, 400, 50, + "Error rate: " + Character.getInstance().getPIDController().getErrorRate()); this.add(characterError); this.add(errorRate); this.setLayout(null); @@ -36,28 +39,8 @@ private Status() { this.setBackground(Color.GRAY); } - public void configureCharacterError() { - characterError = new JTextField("Error: " + Character.getInstance().getError()); - characterError.setSize(200, 50); - characterError.setEditable(false); - characterError.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); - characterError.setLayout(null); - characterError.setBackground(Color.GRAY); - characterError.setBorder(new LineBorder(Color.GRAY)); - } - - public void configureErrorRate() { - errorRate = new JTextField("Error rate: " + Character.getInstance().getRate()); - errorRate.setBounds(0, 50, 400, 50); - errorRate.setEditable(false); - errorRate.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); - errorRate.setLayout(null); - errorRate.setBackground(Color.GRAY); - errorRate.setBorder(new LineBorder(Color.GRAY)); - } - public void update() { - characterError.setText("Error: " + Character.getInstance().getError()); - errorRate.setText("Error rate: " + Character.getInstance().getRate()); + characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().x)); + errorRate.setText("Error rate: " + (Character.getInstance().getPIDController().getErrorRate())); } }