diff --git a/src/Character.java b/src/Character.java deleted file mode 100644 index 0b51a4d..0000000 --- a/src/Character.java +++ /dev/null @@ -1,102 +0,0 @@ -import java.awt.*; - -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 MAX_SPEED = 120; - 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 static Character instance; - - private double errorSum; - private double lastTimestamp; - private double lastError; - private double lastSpeed; - - public static Character getInstance() { - if (instance == null) { - instance = new Character(WIDTH, HEIGHT, SOURCE_X, SOURCE_Y); - } - return instance; - } - - 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; - } - - public void update() { - double error = Setpoint.getInstance().x - this.x; - double dt = System.currentTimeMillis() - lastTimestamp; - double 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 * Math.signum(lastSpeed)) > MAX_ACCELERATION) { - if (moveValue > 0) moveValue = (int) (lastSpeed + MAX_ACCELERATION); - if (moveValue < 0) moveValue = (int) (lastSpeed - MAX_ACCELERATION); - } - 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; - 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.errorSum = 0; - character.lastTimestamp = System.currentTimeMillis(); - character.lastError = Setpoint.getInstance().x - character.x; - character.lastSpeed = 0; - character.setLocation(SOURCE_X, SOURCE_Y); - } - - 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); - } -} diff --git a/src/Window.java b/src/Window.java deleted file mode 100644 index 3b719d0..0000000 --- a/src/Window.java +++ /dev/null @@ -1,76 +0,0 @@ -import textfields.BaseTextField; - -import javax.swing.*; -import java.awt.*; -import java.util.Set; - -public class Window extends JPanel { - - 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; - private final Setpoint setpoint; - private final RerunButton rerunButton; - private final BaseTextField kPField; - private final BaseTextField kIField; - private final BaseTextField kDField; - - private Window() { - super(IS_DOUBLE_BUFFERED); - this.setPreferredSize(new Dimension(WINDOW_WIDTH, WINDOW_HEIGHT)); - this.setBackground(Color.BLACK); - this.setVisible(true); - this.setLayout(null); - character = Character.getInstance(); - setpoint = Setpoint.getInstance(); - rerunButton = RerunButton.getInstance(); - kPField = new BaseTextField("kP", 300, 30); - kIField = new BaseTextField("kI", 600, 30); - kDField = new BaseTextField("kD", 900, 30); - this.add(rerunButton); - this.add(kPField); - this.add(kIField); - this.add(kDField); - } - - @Override - public void paintComponent(Graphics g) { - super.paintComponent(g); - Graphics2D g2 = (Graphics2D) g; - g2.setColor(Color.WHITE); - g2.fill(character); - g2.setColor(Color.RED); - g2.fill(setpoint); - } - - private void delay(double seconds) { - long currentTime = System.currentTimeMillis(); - long targetTime = (long) (System.currentTimeMillis() + seconds * 1000); - while (targetTime > currentTime) { - currentTime = System.currentTimeMillis(); - repaint(character); - } - } - - private void update() { - delay(0.02); - character.update(); - character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue()); - repaint(); - } - - 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(); - } - } -} diff --git a/src/simulator/Character.java b/src/simulator/Character.java new file mode 100644 index 0000000..73a7d92 --- /dev/null +++ b/src/simulator/Character.java @@ -0,0 +1,155 @@ +package simulator; + +import simulator.control.*; + +import java.awt.*; + +public class Character extends Rectangle { + + public static final int WIDTH = 32; + public static final int HEIGHT = 32; + + private static final int MILLISECONDS_IN_SECOND = 1000; + + private static final int POSITION_SOURCE_X = 0; + private static final int POSITION_SOURCE_Y = Window.WINDOW_HEIGHT / 3; + + private static final int VELOCITY_SOURCE_X = Window.WINDOW_WIDTH / 2; + private static final int VELOCITY_SOURCE_Y = 320; + + private static final int MAX_SPEED = 120; + private static final int MAX_ACCELERATION = 6; + private static final int FRICTION = 3; + + private final PIDSettings pidSettings; + private final PIDController pidController; + private final FeedForwardSettings feedForwardSettings; + private final FeedForwardController feedForwardController; + + private double lastTimeNotOnTarget; + private double lastSpeed; + private boolean commandFinished; + + private static Character instance; + + public static Character getInstance() { + if (instance == null) { + instance = new Character(WIDTH, HEIGHT, POSITION_SOURCE_X, POSITION_SOURCE_Y); + } + return instance; + } + + private Character(int width, int height, int sourceX, int sourceY) { + super(sourceX, sourceY, width, height); + lastSpeed = 0; + pidSettings = new PIDSettings(0, 0, 0, 0, 0); + feedForwardSettings = new FeedForwardSettings(0, 0, 0); + pidController = new PIDController(pidSettings); + feedForwardController = new FeedForwardController(feedForwardSettings); + lastTimeNotOnTarget = System.currentTimeMillis(); + } + + public static void reset() { + Character character = getInstance(); + character.lastSpeed = 0; + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.POSITION.index) { + character.setLocation(POSITION_SOURCE_X, POSITION_SOURCE_Y); + } else { + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + 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() == ControlType.Types.POSITION.index) { + runPosition(); + } else if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + 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); + pidSettings.setWaitTime(waitTime); + } + + public void setIZone(int iZone) { + pidController.setIZone(iZone); + } + + public PIDController getPIDController() { + return pidController; + } + + public double getError() { + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.POSITION.index) { + return Setpoint.getInstance().x - this.x; + } else { + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + return Setpoint.getInstance().x - lastSpeed; + } else { + return 0; + } + } + } + + public void setFF(double kS, double kV, double kA) { + feedForwardController.setGains(kS, kV, kA); + } + + public double getLastSpeed() { + return lastSpeed; + } + + public boolean commandEnded() { + return commandFinished; + } + + 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); + if (Math.abs(moveValue - lastSpeed) > MAX_ACCELERATION) { + if (lastSpeed > moveValue) moveValue = (int) (lastSpeed - MAX_ACCELERATION); + if (lastSpeed < moveValue) moveValue = (int) (lastSpeed + MAX_ACCELERATION); + } + moveValue = (int) normalizeSpeed(moveValue); + moveValue += (Math.random() - 0.5) * Math.pow(moveValue, 2) / MAX_SPEED; + this.translate(moveValue, 0); + lastSpeed = moveValue; + } + + private void runVelocity() { + int moveValue = pidController.calculate(lastSpeed, Setpoint.getInstance().x); + double feedForward = feedForwardController.calculate(lastSpeed, Setpoint.getInstance().x); + moveValue += feedForward * Math.pow(Math.E, -(Math.pow(feedForward / MAX_SPEED, 2) / Math.pow(MAX_SPEED, 0.25))); + 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 > FRICTION) speed -= FRICTION; + else if (speed < -FRICTION) speed += FRICTION; + else return 0; + if (Math.abs(speed) > MAX_SPEED) speed = (int) (MAX_SPEED * Math.signum(speed)); + } + return speed; + } +} diff --git a/src/Main.java b/src/simulator/Main.java similarity index 83% rename from src/Main.java rename to src/simulator/Main.java index ea7f478..6c99465 100644 --- a/src/Main.java +++ b/src/simulator/Main.java @@ -1,3 +1,5 @@ +package simulator; + public class Main { public static void main(String[] args) { diff --git a/src/RerunButton.java b/src/simulator/RerunButton.java similarity index 63% rename from src/RerunButton.java rename to src/simulator/RerunButton.java index 221462f..4ba3510 100644 --- a/src/RerunButton.java +++ b/src/simulator/RerunButton.java @@ -1,15 +1,18 @@ +package simulator; + +import simulator.information.ErrorGraph; +import simulator.textfields.BaseTextField; + import javax.swing.*; import java.awt.*; -import java.awt.event.ActionEvent; -import java.awt.event.ActionListener; public class RerunButton extends JButton { - private static final int WIDTH = 100; - private static final int HEIGHT = 45; + public static final int WIDTH = 100; + public static final int HEIGHT = 45; - private static final int X = 620; - private static final int Y = 700; + private static final int X = Window.WINDOW_WIDTH / 2 - WIDTH / 2; + private static final int Y = 200; private static RerunButton instance; @@ -28,6 +31,7 @@ private RerunButton() { this.setBounds(X, Y, WIDTH, HEIGHT); this.addActionListener(e -> { Character.reset(); + ErrorGraph.reset(); }); } } diff --git a/src/Setpoint.java b/src/simulator/Setpoint.java similarity index 57% rename from src/Setpoint.java rename to src/simulator/Setpoint.java index 1308468..8b61faf 100644 --- a/src/Setpoint.java +++ b/src/simulator/Setpoint.java @@ -1,3 +1,7 @@ +package simulator; + +import simulator.control.ControlType; + import java.awt.*; public class Setpoint extends Rectangle { @@ -6,7 +10,7 @@ public class Setpoint extends Rectangle { private static final int HEIGHT = 32; private static final int SOURCE_X = 1000; - private static final int SOURCE_Y = 320; + private static final int SOURCE_Y = Window.WINDOW_HEIGHT / 3; private static Setpoint instance; @@ -20,4 +24,13 @@ public static Setpoint getInstance() { private Setpoint(int width, int height, int sourceX, int sourceY) { super(sourceX, sourceY, width, height); } + + public void setSetpoint(int position) { + this.x = position; + if (ControlType.getInstance().getSelectedIndex() != ControlType.Types.POSITION.index) { + this.setSize(-1, -1); + } else { + this.setSize(WIDTH, HEIGHT); + } + } } diff --git a/src/simulator/Window.java b/src/simulator/Window.java new file mode 100644 index 0000000..beb69bf --- /dev/null +++ b/src/simulator/Window.java @@ -0,0 +1,141 @@ +package simulator; + +import simulator.control.ControlType; +import simulator.information.Status; +import simulator.textfields.BaseTextField; + +import javax.swing.*; +import javax.tools.Tool; +import java.awt.*; + +public class Window extends JPanel { + + public static final double PERIODIC_FRAME = 0.02; + public static final int WINDOW_WIDTH = Toolkit.getDefaultToolkit().getScreenSize().width; + public static final int WINDOW_HEIGHT = Toolkit.getDefaultToolkit().getScreenSize().height; + private static final int MILLISECONDS_IN_SECOND = 1000; + + public static final boolean IS_DOUBLE_BUFFERED = true; + + private static final Point P_FIELD_LOCATION = + new Point((int) (WINDOW_WIDTH / 2 - BaseTextField.FIELD_WIDTH * 2.5 - 30), 30); + private static final Point I_FIELD_LOCATION = new Point(WINDOW_WIDTH / 2 - BaseTextField.FIELD_WIDTH / 2, 30); + private static final Point D_FIELD_LOCATION = + new Point((int) (WINDOW_WIDTH / 2 + BaseTextField.FIELD_WIDTH * 1.5 + 30), 30); + private static final Point I_ZONE_FIELD_LOCATION = new Point(30, 30); + private static final Point TOLERANCE_FIELD_LOCATION = + new Point((int) (WINDOW_WIDTH / 2 - BaseTextField.FIELD_WIDTH * 1.5 - 15), 65); + private static final Point WAIT_TIME_FIELD_LOCATION = + new Point(WINDOW_WIDTH / 2 + BaseTextField.FIELD_WIDTH / 2 + 15, 65); + private static final Point S_FIELD_LOCATION = + new Point((int) (WINDOW_WIDTH / 2 - BaseTextField.FIELD_WIDTH * 2.5 - 30), 100); + private static final Point V_FIELD_LOCATION = new Point(WINDOW_WIDTH / 2 - BaseTextField.FIELD_WIDTH / 2, 100); + private static final Point A_FIELD_LOCATION = + new Point((int) (WINDOW_WIDTH / 2 + BaseTextField.FIELD_WIDTH * 1.5 + 30), 100); + private static final Point SETPOINT_FIELD_LOCATION = new Point(WINDOW_WIDTH - BaseTextField.FIELD_WIDTH - 30, 30); + + private final Character character; + private final Setpoint setpoint; + private final RerunButton rerunButton; + 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 ControlType controlType; + private final Status status; + + private Window() { + super(IS_DOUBLE_BUFFERED); + this.setPreferredSize(new Dimension(WINDOW_WIDTH, WINDOW_HEIGHT)); + this.setBackground(Color.BLACK); + this.setVisible(true); + this.setLayout(null); + character = Character.getInstance(); + setpoint = Setpoint.getInstance(); + rerunButton = RerunButton.getInstance(); + status = Status.getInstance(); + kPField = new BaseTextField(P_FIELD_LOCATION, "kP"); + kIField = new BaseTextField(I_FIELD_LOCATION, "kI"); + kDField = new BaseTextField(D_FIELD_LOCATION, "kD"); + iZoneField = new BaseTextField(I_ZONE_FIELD_LOCATION, "i zone"); + toleranceField = new BaseTextField(TOLERANCE_FIELD_LOCATION, "tolerance"); + waitTimeField = new BaseTextField(WAIT_TIME_FIELD_LOCATION, "wait time"); + kSField = new BaseTextField(S_FIELD_LOCATION, "kS"); + kVField = new BaseTextField(V_FIELD_LOCATION, "kV"); + kAField = new BaseTextField(A_FIELD_LOCATION, "kA"); + setpointField = new BaseTextField(SETPOINT_FIELD_LOCATION, "setpoint"); + 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 + public void paintComponent(Graphics g) { + super.paintComponent(g); + Graphics2D g2 = (Graphics2D) g; + g2.setColor(Color.WHITE); + g2.fill(character); + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + g2.drawString("Velocity: " + character.getLastSpeed(), character.x, character.y); + } + g2.setColor(Color.RED); + g2.fill(setpoint); + } + + private void delay(double seconds) { + long currentTime = System.currentTimeMillis(); + long targetTime = (long) (System.currentTimeMillis() + seconds * MILLISECONDS_IN_SECOND); + while (targetTime > currentTime) { + currentTime = System.currentTimeMillis(); + } + } + + private void update() { + delay(PERIODIC_FRAME); + 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()); + } + + 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); + } +} diff --git a/src/simulator/control/ControlType.java b/src/simulator/control/ControlType.java new file mode 100644 index 0000000..368c25f --- /dev/null +++ b/src/simulator/control/ControlType.java @@ -0,0 +1,48 @@ +package simulator.control; + +import simulator.Character; +import simulator.Window; +import simulator.textfields.BaseTextField; + +import javax.swing.*; +import java.awt.*; + +public class ControlType extends JComboBox { + + public enum Types { + + POSITION(0), VELOCITY(1); + + public int index; + + Types(int index) { + this.index = index; + } + } + + private static final int WIDTH = 140; + private static final int HEIGHT = 65; + + private static final int X = Window.WINDOW_WIDTH - WIDTH - 30; + private static final int Y = 60 + HEIGHT; + 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()); + } +} diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java new file mode 100644 index 0000000..610b9b6 --- /dev/null +++ b/src/simulator/control/FeedForwardController.java @@ -0,0 +1,72 @@ +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; + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + return (int) (kS + kV * setpoint + kA * targetDerivative); + } else { + 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..1df0277 --- /dev/null +++ b/src/simulator/control/PIDController.java @@ -0,0 +1,94 @@ +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 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(); + 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 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..c158b53 --- /dev/null +++ b/src/simulator/information/BaseInfoField.java @@ -0,0 +1,24 @@ +package simulator.information; + +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)); + } + + public BaseInfoField(Point position, int width, int height, String text) { + this(position.x, position.y, width, height, text); + } +} diff --git a/src/simulator/information/ErrorGraph.java b/src/simulator/information/ErrorGraph.java new file mode 100644 index 0000000..ad1994b --- /dev/null +++ b/src/simulator/information/ErrorGraph.java @@ -0,0 +1,81 @@ +package simulator.information; + +import simulator.Setpoint; + +import javax.swing.*; +import java.awt.*; +import java.awt.geom.Line2D; +import java.util.ArrayList; + +public class ErrorGraph extends JPanel { + + public static final int WIDTH = Status.WIDTH; + public static final int HEIGHT = Status.HEIGHT - Status.INFO_TEXT_HEIGHT * 3; + private static final int X = 0; + private static final int Y = Status.INFO_TEXT_HEIGHT * 3; + + private static final int POINT_WIDTH = 2; + private static final int POINT_HEIGHT = 2; + private static final int DISTANCE_BETWEEN_POINTS = 2; + + private final ArrayList errors; + private int currentErrorLocation; + private double proportion; + + private static ErrorGraph instance; + + public static ErrorGraph getInstance() { + if (instance == null) { + instance = new ErrorGraph(); + } + return instance; + } + + private ErrorGraph() { + this.setLayout(null); + this.setVisible(true); + this.setBounds(X, Y, WIDTH, HEIGHT); + this.setBackground(Color.WHITE); + errors = new ArrayList<>(); + currentErrorLocation = 0; + proportion = 0; + } + + @Override + public void paintComponent(Graphics g) { + super.paintComponent(g); + Graphics2D g2 = (Graphics2D) g; + g2.setColor(Color.RED); + ArrayList rectangles = createRectangles(); + rectangles.forEach(g2::fill); + g2.setColor(Color.BLACK); + g2.fill(new Rectangle(0, HEIGHT / 2, WIDTH, POINT_HEIGHT)); + } + + public static void reset() { + ErrorGraph errorGraph = getInstance(); + errorGraph.errors.clear(); + errorGraph.currentErrorLocation = 0; + errorGraph.setProportions(Setpoint.getInstance().x); + } + + public void addPoint(int error) { + errors.add(error); + } + + public void setProportions(int setpoint) { + if (setpoint == 0) proportion = 0; + else proportion = (double) HEIGHT / setpoint; + } + + private ArrayList createRectangles() { + ArrayList rectangles = new ArrayList<>(); + currentErrorLocation = 0; + errors.forEach(error -> { + rectangles.add(new Rectangle(currentErrorLocation, (int) (error * proportion) / 2 + HEIGHT / 2, + POINT_WIDTH, POINT_HEIGHT)); + currentErrorLocation += DISTANCE_BETWEEN_POINTS; + }); + return rectangles; + } +} diff --git a/src/simulator/information/Status.java b/src/simulator/information/Status.java new file mode 100644 index 0000000..afe6d40 --- /dev/null +++ b/src/simulator/information/Status.java @@ -0,0 +1,72 @@ +package simulator.information; + +import simulator.Character; +import simulator.Setpoint; +import simulator.Window; +import simulator.control.ControlType; + +import javax.swing.*; +import java.awt.*; + +public class Status extends JPanel { + + public static final int WIDTH = Window.WINDOW_WIDTH; + public static final int HEIGHT = Window.WINDOW_HEIGHT / 2; + public static final int INFO_TEXT_WIDTH = WIDTH; + public static final int INFO_TEXT_HEIGHT = HEIGHT / 15; + + private static final int X = 0; + private static final int Y = Window.WINDOW_HEIGHT - HEIGHT; + private static final int FONT_SIZE = 20; + + private static final Point ERROR_LOCATION = new Point(0, 0); + private static final Point ERROR_RATE_LOCATION = new Point(0, INFO_TEXT_HEIGHT); + private static final Point COMMAND_ENDED_LOCATION = new Point(0, INFO_TEXT_HEIGHT * 2); + + private BaseInfoField characterError; + private BaseInfoField errorRate; + private BaseInfoField commandEnded; + private ErrorGraph errorGraph; + + private static Status instance; + + public static Status getInstance() { + if (instance == null) { + instance = new Status(); + } + return instance; + } + + private Status() { + characterError = new BaseInfoField(ERROR_LOCATION, INFO_TEXT_WIDTH, INFO_TEXT_HEIGHT, + "Error: " + Character.getInstance().getError()); + errorRate = new BaseInfoField(ERROR_RATE_LOCATION, INFO_TEXT_WIDTH, INFO_TEXT_HEIGHT, + "Error rate: " + Character.getInstance().getPIDController().getErrorRate()); + commandEnded = new BaseInfoField(COMMAND_ENDED_LOCATION, INFO_TEXT_WIDTH, INFO_TEXT_HEIGHT, "Command ended: " + + Character.getInstance().commandEnded()); + errorGraph = ErrorGraph.getInstance(); + this.add(characterError); + this.add(errorRate); + this.add(commandEnded); + this.add(errorGraph); + this.setLayout(null); + this.setVisible(true); + this.setBounds(X, Y, WIDTH, HEIGHT); + this.setBackground(Color.GRAY); + } + + public void update() { + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.POSITION.index) { + characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().x)); + errorRate.setText("Error rate: " + Character.getInstance().getPIDController().getErrorRate()); + commandEnded.setText("Command ended: " + Character.getInstance().commandEnded()); + } else { + if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { + characterError.setText("Error: " + (Setpoint.getInstance().x - Character.getInstance().getLastSpeed())); + errorRate.setText("Error rate: " + Character.getInstance().getPIDController().getErrorRate()); + commandEnded.setText("Command ended: " + Character.getInstance().commandEnded()); + } + } + errorGraph.addPoint((int) Character.getInstance().getError()); + } +} diff --git a/src/textfields/BaseTextField.java b/src/simulator/textfields/BaseTextField.java similarity index 53% rename from src/textfields/BaseTextField.java rename to src/simulator/textfields/BaseTextField.java index 33fe81f..784c56c 100644 --- a/src/textfields/BaseTextField.java +++ b/src/simulator/textfields/BaseTextField.java @@ -1,26 +1,32 @@ -package textfields; +package simulator.textfields; + +import simulator.Window; import javax.swing.*; import java.awt.*; public class BaseTextField extends JTextField { - private static final int FIELD_WIDTH = 140; - private static final int FIELD_HEIGHT = 65; + public static final int FIELD_WIDTH = Window.WINDOW_WIDTH / 15; + public static final int FIELD_HEIGHT = Window.WINDOW_HEIGHT / 20; private static final int FONT_SIZE = 40; - public BaseTextField(String initialText, int x, int y) { + public BaseTextField(int x, int y, String initialText) { super(initialText); this.setBounds(x, y, FIELD_WIDTH, FIELD_HEIGHT); this.setBackground(Color.GRAY); this.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); } + public BaseTextField(Point position, String initialText) { + this(position.x, position.y, initialText); + } + public double getValue() { try { return Double.parseDouble(this.getText()); - } catch(NumberFormatException e) { + } catch (NumberFormatException e) { return 0; } }