diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 27ce108..baef0d6 100644 --- a/src/simulator/Character.java +++ b/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; + } } diff --git a/src/simulator/Setpoint.java b/src/simulator/Setpoint.java index a058335..ae2acf4 100644 --- a/src/simulator/Setpoint.java +++ b/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); + } } } diff --git a/src/simulator/Window.java b/src/simulator/Window.java index a0a2f9a..e7c9b1d 100644 --- a/src/simulator/Window.java +++ b/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(); - } - } } diff --git a/src/simulator/control/ControlType.java b/src/simulator/control/ControlType.java new file mode 100644 index 0000000..1373a76 --- /dev/null +++ b/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 { + + 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()); + } +} diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java index 920ea35..dc72769 100644 --- a/src/simulator/control/FeedForwardController.java +++ b/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); + } } } diff --git a/src/simulator/information/Status.java b/src/simulator/information/Status.java index 6f1264e..7a104dc 100644 --- a/src/simulator/information/Status.java +++ b/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())); + } + } } }