From 3c576b688ed411a58e9433870e801dbb61bba41f Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sat, 3 Feb 2024 20:24:31 +0200 Subject: [PATCH 1/9] created several classes related to PID control and FF control to make the code way nicer, implemented FF into the movement --- src/simulator/Character.java | 81 +++++++------------ src/simulator/Window.java | 12 ++- .../control/FeedForwardController.java | 69 ++++++++++++++++ .../control/FeedForwardSettings.java | 44 ++++++++++ src/simulator/control/PIDController.java | 72 +++++++++++++++++ src/simulator/control/PIDSettings.java | 44 ++++++++++ src/simulator/information/Status.java | 6 +- 7 files changed, 269 insertions(+), 59 deletions(-) create mode 100644 src/simulator/control/FeedForwardController.java create mode 100644 src/simulator/control/FeedForwardSettings.java create mode 100644 src/simulator/control/PIDController.java create mode 100644 src/simulator/control/PIDSettings.java diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 829c149..a50e872 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -1,5 +1,10 @@ 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 { @@ -14,21 +19,14 @@ 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 PIDController pidController; + private final FeedForwardController feedForwardController; + private final PIDSettings pidSettings; + private final FeedForwardSettings feedForwardSettings; private static Character instance; - private double errorSum; - private double lastTimestamp; - private double lastError; private double lastSpeed; - private double error; - private double dt; - private double errorRate; public static Character getInstance() { if (instance == null) { @@ -39,37 +37,32 @@ 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); + feedForwardSettings = new FeedForwardSettings(0, 0, 0); + pidController = new PIDController(pidSettings); + feedForwardController = new FeedForwardController(feedForwardSettings); } 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); + 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); - lastError = error; - lastTimestamp = System.currentTimeMillis(); - lastSpeed = normalizeNoFriction(moveValue); + lastSpeed = 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 +73,17 @@ 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); - } - - public void setP(double kP) { - this.kP = kP; - } - - public void setI(double kI) { - this.kI = kI; - } - - public void setD(double kD) { - this.kD = kD; + character.pidController.reset(); + character.feedForwardController.reset(); } public void setPID(double kP, double kI, double kD) { - setP(kP); - setI(kI); - setD(kD); + pidController.setPID(kP, kI, kD); } - public double getError() { - return error; + public void setFF(double kS, double kV, double kA) { + feedForwardController.setGains(kS, kV, kA); } - - public double getRate() { - return errorRate; - } - } diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 15db388..733c663 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -19,6 +19,9 @@ public class Window extends JPanel { private final BaseTextField kPField; private final BaseTextField kIField; private final BaseTextField kDField; + private final BaseTextField kSField; + private final BaseTextField kVField; + private final BaseTextField kAField; private final Status status; private Window() { @@ -34,10 +37,16 @@ private Window() { kPField = new BaseTextField("kP", 300, 30); kIField = new BaseTextField("kI", 600, 30); kDField = new BaseTextField("kD", 900, 30); + kSField = new BaseTextField("kS", 300, 100); + kVField = new BaseTextField("kV", 600, 100); + kAField = new BaseTextField("kA", 900, 100); this.add(rerunButton); this.add(kPField); this.add(kIField); this.add(kDField); + this.add(kSField); + this.add(kVField); + this.add(kAField); this.add(status); } @@ -62,8 +71,9 @@ private void delay(double seconds) { private void update() { delay(0.02); - character.update(); character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue()); + character.setFF(kSField.getValue(), kVField.getValue(), kAField.getValue()); + character.update(); status.update(); repaint(); } diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java new file mode 100644 index 0000000..164095d --- /dev/null +++ b/src/simulator/control/FeedForwardController.java @@ -0,0 +1,69 @@ +package simulator.control; + +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) / 0.02; + 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..203b291 --- /dev/null +++ b/src/simulator/control/PIDController.java @@ -0,0 +1,72 @@ +package simulator.control; + +import simulator.Setpoint; + +public class PIDController { + + private static final int I_ZONE = 200; + + private double kP; + private double kI; + private double kD; + + private double errorSum; + private double lastTimestamp; + private double lastError; + private double lastSpeed; + private double error; + private double dt; + private double errorRate; + + public PIDController(PIDSettings pidSettings) { + this.kP = pidSettings.getP(); + this.kI = pidSettings.getI(); + this.kD = pidSettings.getD(); + errorSum = 0; + lastTimestamp = System.currentTimeMillis(); + lastError = 0; + } + + public PIDController(double kP, double kI, double kD) { + this(new PIDSettings(kP, kI, 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 void setPID(double kP, double kI, double kD) { + setP(kP); + setI(kI); + setD(kD); + } + + public void setPID(PIDSettings pidSettings) { + setPID(pidSettings.getP(), pidSettings.getI(), pidSettings.getD()); + } + + public int calculate(double source, double setpoint) { + error = setpoint - source; + 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); + lastError = error; + lastTimestamp = System.currentTimeMillis(); + return moveValue; + } + + public void reset() { + errorSum = 0; + lastTimestamp = System.currentTimeMillis(); + lastError = 0; + } +} diff --git a/src/simulator/control/PIDSettings.java b/src/simulator/control/PIDSettings.java new file mode 100644 index 0000000..bcc4765 --- /dev/null +++ b/src/simulator/control/PIDSettings.java @@ -0,0 +1,44 @@ +package simulator.control; + +public class PIDSettings { + + private double kP; + private double kI; + private double kD; + + public PIDSettings(double kP, double kI, double kD) { + this.kP = kP; + this.kI = kI; + this.kD = kD; + } + + 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 void setPID(double kP, double kI, double kD) { + setP(kP); + setI(kI); + setD(kD); + } +} diff --git a/src/simulator/information/Status.java b/src/simulator/information/Status.java index a58d172..43d2cd3 100644 --- a/src/simulator/information/Status.java +++ b/src/simulator/information/Status.java @@ -37,7 +37,7 @@ private Status() { } public void configureCharacterError() { - characterError = new JTextField("Error: " + Character.getInstance().getError()); + characterError = new JTextField(); characterError.setSize(200, 50); characterError.setEditable(false); characterError.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); @@ -47,7 +47,7 @@ public void configureCharacterError() { } public void configureErrorRate() { - errorRate = new JTextField("Error rate: " + Character.getInstance().getRate()); + errorRate = new JTextField(); errorRate.setBounds(0, 50, 400, 50); errorRate.setEditable(false); errorRate.setFont(new Font(Font.MONOSPACED, Font.PLAIN, FONT_SIZE)); @@ -57,7 +57,5 @@ public void configureErrorRate() { } public void update() { - characterError.setText("Error: " + Character.getInstance().getError()); - errorRate.setText("Error rate: " + Character.getInstance().getRate()); } } From 46c07036d7995702d89bc4902547608ab9565d04 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:45:21 +0200 Subject: [PATCH 2/9] added wait time and tolerance --- src/simulator/Character.java | 45 ++++++++++++++------ src/simulator/Window.java | 27 ++++++++---- src/simulator/control/PIDController.java | 28 +++++++++++- src/simulator/control/PIDSettings.java | 22 +++++++++- src/simulator/information/BaseInfoField.java | 22 ++++++++++ src/simulator/information/Status.java | 33 ++++---------- 6 files changed, 130 insertions(+), 47 deletions(-) create mode 100644 src/simulator/information/BaseInfoField.java diff --git a/src/simulator/Character.java b/src/simulator/Character.java index a50e872..2858d09 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -26,7 +26,9 @@ public class Character extends Rectangle { private static Character instance; + private double lastTimeNotOnTarget; private double lastSpeed; + private boolean commandFinished; public static Character getInstance() { if (instance == null) { @@ -38,22 +40,30 @@ public static Character getInstance() { private Character(int width, int height, int sourceX, int sourceY) { super(sourceX, sourceY, width, height); lastSpeed = 0; - pidSettings = new PIDSettings(0, 0, 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() { - 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 (!commandFinished) { + commandFinished = (System.currentTimeMillis() - lastTimeNotOnTarget >= pidSettings.getWaitTime() * 1000 && + 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); - lastSpeed = moveValue; } private double normalizeSpeed(double speed) { @@ -61,8 +71,7 @@ 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)); - } - else if (Math.abs(speed) < FRICTION) speed = 0; + } else if (Math.abs(speed) < FRICTION) speed = 0; return speed; } @@ -75,12 +84,24 @@ public static void reset() { Character character = getInstance(); character.lastSpeed = 0; character.setLocation(SOURCE_X, SOURCE_Y); + character.commandFinished = false; + character.lastTimeNotOnTarget = System.currentTimeMillis(); character.pidController.reset(); character.feedForwardController.reset(); } - public void setPID(double kP, double kI, double kD) { + 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 PIDController getPIDController() { + return pidController; + } + + public double getError() { + return Setpoint.getInstance().x - this.x; } public void setFF(double kS, double kV, double kA) { diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 733c663..d2e4318 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -19,6 +19,8 @@ public class Window extends JPanel { private final BaseTextField kPField; private final BaseTextField kIField; private final BaseTextField kDField; + private final BaseTextField toleranceField; + private final BaseTextField waitTimeField; private final BaseTextField kSField; private final BaseTextField kVField; private final BaseTextField kAField; @@ -37,17 +39,13 @@ private Window() { kPField = new BaseTextField("kP", 300, 30); kIField = new BaseTextField("kI", 600, 30); kDField = new BaseTextField("kD", 900, 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); + configureTextFields(); this.add(rerunButton); - this.add(kPField); - this.add(kIField); - this.add(kDField); - this.add(kSField); - this.add(kVField); - this.add(kAField); - this.add(status); } @Override @@ -71,13 +69,26 @@ private void delay(double seconds) { private void update() { delay(0.02); - character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue()); + character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue(), toleranceField.getValue(), + waitTimeField.getValue()); character.setFF(kSField.getValue(), kVField.getValue(), kAField.getValue()); character.update(); status.update(); repaint(); } + private void configureTextFields() { + this.add(kPField); + this.add(kIField); + this.add(kDField); + this.add(toleranceField); + this.add(waitTimeField); + this.add(kSField); + this.add(kVField); + this.add(kAField); + this.add(status); + } + public static void initGame() { JFrame frame = new JFrame(); Window window = new Window(); diff --git a/src/simulator/control/PIDController.java b/src/simulator/control/PIDController.java index 203b291..568d0c8 100644 --- a/src/simulator/control/PIDController.java +++ b/src/simulator/control/PIDController.java @@ -9,6 +9,8 @@ public class PIDController { private double kP; private double kI; private double kD; + private double tolerance; + private double waitTime; private double errorSum; private double lastTimestamp; @@ -17,18 +19,22 @@ public class PIDController { 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.tolerance = pidSettings.getTolerance(); + this.waitTime = pidSettings.getWaitTime(); errorSum = 0; lastTimestamp = System.currentTimeMillis(); lastError = 0; + errorRate = 0; } - public PIDController(double kP, double kI, double kD) { - this(new PIDSettings(kP, kI, kD)); + 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) { @@ -49,12 +55,25 @@ public void setPID(double kP, double kI, double kD) { 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 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) < I_ZONE) errorSum += error; @@ -68,5 +87,10 @@ 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 index bcc4765..5671d6a 100644 --- a/src/simulator/control/PIDSettings.java +++ b/src/simulator/control/PIDSettings.java @@ -5,11 +5,15 @@ 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) { + 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() { @@ -36,6 +40,22 @@ 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); 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 43d2cd3..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,26 +39,8 @@ private Status() { this.setBackground(Color.GRAY); } - public void configureCharacterError() { - characterError = new JTextField(); - 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(); - 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: " + (Setpoint.getInstance().x - Character.getInstance().x)); + errorRate.setText("Error rate: " + (Character.getInstance().getPIDController().getErrorRate())); } } From 175b2e5ab35c5a8137524f5b956bc5821cf5e247 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:50:59 +0200 Subject: [PATCH 3/9] fixed ordering in Character --- src/simulator/Character.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 2858d09..bb67a58 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -19,10 +19,10 @@ public class Character extends Rectangle { private static final int MAX_ACCELERATION = 2; private static final int FRICTION = 3; - private final PIDController pidController; - private final FeedForwardController feedForwardController; private final PIDSettings pidSettings; + private final PIDController pidController; private final FeedForwardSettings feedForwardSettings; + private final FeedForwardController feedForwardController; private static Character instance; From cea0973ed1097b98a040a92ed6ea66f29c782880 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:51:19 +0200 Subject: [PATCH 4/9] fixed ordering in Character --- src/simulator/Character.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index bb67a58..05aaede 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -24,12 +24,13 @@ public class Character extends Rectangle { private final FeedForwardSettings feedForwardSettings; private final FeedForwardController feedForwardController; - private static Character instance; 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, SOURCE_X, SOURCE_Y); From 0dd7a02f701ae394286e80088b48361f14c089b6 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:52:29 +0200 Subject: [PATCH 5/9] removed magic --- src/simulator/Character.java | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 05aaede..0e0a6b9 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -9,6 +9,7 @@ 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; @@ -50,8 +51,8 @@ private Character(int width, int height, int sourceX, int sourceY) { public void update() { if (!commandFinished) { - commandFinished = (System.currentTimeMillis() - lastTimeNotOnTarget >= pidSettings.getWaitTime() * 1000 && - pidController.isOnTarget()); + 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); From f95ec10fa34cbd3e2121e0e6d0730eb86f79cee1 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:53:06 +0200 Subject: [PATCH 6/9] removed magic --- src/simulator/Window.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/simulator/Window.java b/src/simulator/Window.java index d2e4318..223b4bb 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -11,6 +11,8 @@ public class Window extends JPanel { private static final int WINDOW_WIDTH = 1280; private static final int WINDOW_HEIGHT = 792; + private static final double PERIODIC_FRAME = 0.02; + private static final boolean IS_DOUBLE_BUFFERED = true; private final Character character; @@ -68,7 +70,7 @@ private void delay(double seconds) { } private void update() { - delay(0.02); + delay(PERIODIC_FRAME); character.setPID(kPField.getValue(), kIField.getValue(), kDField.getValue(), toleranceField.getValue(), waitTimeField.getValue()); character.setFF(kSField.getValue(), kVField.getValue(), kAField.getValue()); From 1768bf1c0c9edb153106d91858fcadd38dbca78f Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Thu, 8 Feb 2024 13:54:23 +0200 Subject: [PATCH 7/9] removed spaces and fixed magic --- src/simulator/Window.java | 5 ++--- src/simulator/control/FeedForwardController.java | 7 +++---- 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/simulator/Window.java b/src/simulator/Window.java index 223b4bb..b033854 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -8,11 +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 double PERIODIC_FRAME = 0.02; - private static final boolean IS_DOUBLE_BUFFERED = true; private final Character character; diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java index 164095d..920ea35 100644 --- a/src/simulator/control/FeedForwardController.java +++ b/src/simulator/control/FeedForwardController.java @@ -1,13 +1,12 @@ 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) { @@ -62,7 +61,7 @@ public void reset() { public int calculate(double source, double setpoint) { double error = setpoint - source; - double targetDerivative = (setpoint - previousTarget) / 0.02; + double targetDerivative = (setpoint - previousTarget) / Window.PERIODIC_FRAME; previousTarget = setpoint; return (int) (kS * Math.signum(error) + kV * setpoint + kA * targetDerivative); } From be04f063d2136970e5debaf856833f396f2bce05 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sun, 11 Feb 2024 10:07:56 +0200 Subject: [PATCH 8/9] added the option to change i zone --- src/simulator/Character.java | 4 ++++ src/simulator/Window.java | 4 ++++ src/simulator/control/PIDController.java | 12 ++++++++---- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 0e0a6b9..27ce108 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -98,6 +98,10 @@ public void setPID(double kP, double kI, double kD, double tolerance, double wai pidSettings.setWaitTime(waitTime); } + public void setIZone(int iZone) { + pidController.setIZone(iZone); + } + public PIDController getPIDController() { return pidController; } diff --git a/src/simulator/Window.java b/src/simulator/Window.java index b033854..0803db4 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -20,6 +20,7 @@ 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; @@ -40,6 +41,7 @@ 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); @@ -72,6 +74,7 @@ private void update() { delay(PERIODIC_FRAME); 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(); @@ -84,6 +87,7 @@ private void configureTextFields() { this.add(kDField); this.add(toleranceField); this.add(waitTimeField); + this.add(iZoneField); this.add(kSField); this.add(kVField); this.add(kAField); diff --git a/src/simulator/control/PIDController.java b/src/simulator/control/PIDController.java index 568d0c8..903dac0 100644 --- a/src/simulator/control/PIDController.java +++ b/src/simulator/control/PIDController.java @@ -1,14 +1,13 @@ package simulator.control; -import simulator.Setpoint; - public class PIDController { - private static final int I_ZONE = 200; + 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; @@ -25,6 +24,7 @@ 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; @@ -71,12 +71,16 @@ 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) < I_ZONE) errorSum += error; + if (Math.abs(error) < iZone) errorSum += error; int moveValue = (int) (error * kP + errorSum * kI + errorRate * kD); lastError = error; lastTimestamp = System.currentTimeMillis(); From f7eab81c70d01eeeb4adabb951749a7332bced27 Mon Sep 17 00:00:00 2001 From: turtleminecraft Date: Sun, 11 Feb 2024 10:26:52 +0200 Subject: [PATCH 9/9] added the option to change setpoint location --- src/simulator/Setpoint.java | 4 ++++ src/simulator/Window.java | 4 ++++ 2 files changed, 8 insertions(+) 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 0803db4..a0a2f9a 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -26,6 +26,7 @@ public class Window extends JPanel { private final BaseTextField kSField; private final BaseTextField kVField; private final BaseTextField kAField; + private final BaseTextField setpointField; private final Status status; private Window() { @@ -47,6 +48,7 @@ private Window() { 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); } @@ -72,6 +74,7 @@ private void delay(double seconds) { private void update() { 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()); @@ -92,6 +95,7 @@ private void configureTextFields() { this.add(kVField); this.add(kAField); this.add(status); + this.add(setpointField); } public static void initGame() {