From bea25eefa65b1cd4c3b7e9c384cf0c6327032499 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 15 Jun 2024 21:58:00 +0300 Subject: [PATCH 1/6] added velocity control --- src/simulator/Character.java | 59 +++++++++++++------ src/simulator/Setpoint.java | 9 ++- src/simulator/Window.java | 9 ++- src/simulator/control/ControlType.java | 37 ++++++++++++ .../control/FeedForwardController.java | 7 ++- src/simulator/information/Status.java | 12 +++- 6 files changed, 109 insertions(+), 24 deletions(-) create mode 100644 src/simulator/control/ControlType.java diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 27ce108..d090935 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; } @@ -51,17 +50,29 @@ private Character(int width, int height, int sourceX, int sourceY) { 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); + if (ControlType.getInstance().getSelectedIndex() == 0) { + 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; + } else if (ControlType.getInstance().getSelectedIndex() == 1) { + 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; + System.out.println(lastSpeed); } - this.translate(moveValue, 0); - lastSpeed = moveValue; } if (!pidController.isOnTarget()) { lastTimeNotOnTarget = System.currentTimeMillis(); @@ -85,7 +96,13 @@ private double normalizeNoFriction(double 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(); @@ -113,4 +130,8 @@ public double getError() { public void setFF(double kS, double kV, double kA) { feedForwardController.setGains(kS, kV, kA); } + + public double getLastSpeed() { + return lastSpeed; + } } 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..78ce489 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,10 @@ 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); } @Override @@ -59,6 +63,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,7 +81,7 @@ private void delay(double seconds) { private void update() { delay(PERIODIC_FRAME); - setpoint.setPosition((int) setpointField.getValue()); + setpoint.setSetpoint((int) setpointField.getValue()); character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue(), toleranceField.getValue(), waitTimeField.getValue()); character.setIZone((int) iZoneField.getValue()); 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())); + } + } } } From 9a9452edff8416da107c38523efd6b2e35abbde1 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 15 Jun 2024 22:04:17 +0300 Subject: [PATCH 2/6] removed debug sout --- src/simulator/Character.java | 1 - 1 file changed, 1 deletion(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index d090935..52476f0 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -71,7 +71,6 @@ public void update() { if (lastSpeed < moveValue) moveValue = (int) (lastSpeed + MAX_ACCELERATION); } lastSpeed = moveValue; - System.out.println(lastSpeed); } } if (!pidController.isOnTarget()) { From fdee782695b5037eccb48de1efa1b290c856b781 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 15 Jun 2024 22:06:17 +0300 Subject: [PATCH 3/6] organized update() in Character to different functions --- src/simulator/Character.java | 46 +++++++++++++++++++++--------------- 1 file changed, 27 insertions(+), 19 deletions(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 52476f0..3c71cc7 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -51,26 +51,9 @@ private Character(int width, int height, int sourceX, int sourceY) { public void update() { if (!commandFinished) { if (ControlType.getInstance().getSelectedIndex() == 0) { - 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; + runPosition(); } else if (ControlType.getInstance().getSelectedIndex() == 1) { - 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; + runVelocity(); } } if (!pidController.isOnTarget()) { @@ -78,6 +61,31 @@ public void update() { } } + 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; From e0d683b52339409cb18691cac9cf9bb789583f80 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 15 Jun 2024 22:09:02 +0300 Subject: [PATCH 4/6] fixed ordering --- src/simulator/Character.java | 84 ++++++++++++++++++------------------ 1 file changed, 42 insertions(+), 42 deletions(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 3c71cc7..baef0d6 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -48,6 +48,22 @@ private Character(int width, int height, int sourceX, int sourceY) { lastTimeNotOnTarget = System.currentTimeMillis(); } + public static void reset() { + Character character = getInstance(); + character.lastSpeed = 0; + 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) { @@ -61,6 +77,32 @@ public void update() { } } + 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() { + return Setpoint.getInstance().x - this.x; + } + + 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()); @@ -99,46 +141,4 @@ 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; - 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 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() { - return Setpoint.getInstance().x - this.x; - } - - public void setFF(double kS, double kV, double kA) { - feedForwardController.setGains(kS, kV, kA); - } - - public double getLastSpeed() { - return lastSpeed; - } } From 49839e9ac5de41ea868013d889e296448f00782f Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 15 Jun 2024 22:11:16 +0300 Subject: [PATCH 5/6] fixed ordering --- src/simulator/Window.java | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 78ce489..1c0a97d 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -57,6 +57,18 @@ private Window() { 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); @@ -104,16 +116,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(); - } - } } From 00bdff9992c69a4b9b3066f28fc3703c015ab621 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sun, 16 Jun 2024 16:39:04 +0300 Subject: [PATCH 6/6] function --- src/simulator/Window.java | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 1c0a97d..e7c9b1d 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -93,14 +93,18 @@ private void delay(double seconds) { 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()); - character.update(); - status.update(); - repaint(); } private void configureTextFields() {