diff --git a/src/simulator/Character.java b/src/simulator/Character.java index 1a38cd5..b1376ba 100644 --- a/src/simulator/Character.java +++ b/src/simulator/Character.java @@ -18,8 +18,9 @@ public class Character extends Rectangle { private static final int VELOCITY_SOURCE_Y = 320; private static final int MAX_SPEED = 120; - private static final int MAX_ACCELERATION = 2; + private static final int MAX_ACCELERATION = 6; private static final int FRICTION = 3; + private static final double POSITION_RANDOM_FACTOR = 3.6; private final PIDSettings pidSettings; private final PIDController pidController; @@ -121,18 +122,20 @@ private void runPosition() { 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); + moveValue += (Math.random() - 0.5) * POSITION_RANDOM_FACTOR * Math.pow(moveValue, 2) / MAX_SPEED; 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); this.translate(moveValue, 0); lastSpeed = moveValue; } private void runVelocity() { - int moveValue = pidController.calculate(lastSpeed, Setpoint.getInstance().x) + - feedForwardController.calculate(lastSpeed, Setpoint.getInstance().x); + 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); @@ -143,10 +146,11 @@ private void runVelocity() { private double normalizeSpeed(double speed) { if (speed != 0) { - if (speed > 0) speed -= FRICTION; - if (speed < 0) speed += FRICTION; + 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)); - } else if (Math.abs(speed) < FRICTION) speed = 0; + } return speed; } } diff --git a/src/simulator/Window.java b/src/simulator/Window.java index b8536fd..beb69bf 100644 --- a/src/simulator/Window.java +++ b/src/simulator/Window.java @@ -106,7 +106,6 @@ private void delay(double seconds) { long targetTime = (long) (System.currentTimeMillis() + seconds * MILLISECONDS_IN_SECOND); while (targetTime > currentTime) { currentTime = System.currentTimeMillis(); - repaint(character); } } diff --git a/src/simulator/control/FeedForwardController.java b/src/simulator/control/FeedForwardController.java index 2a602f3..610b9b6 100644 --- a/src/simulator/control/FeedForwardController.java +++ b/src/simulator/control/FeedForwardController.java @@ -63,7 +63,6 @@ public int calculate(double source, double setpoint) { double error = setpoint - source; double targetDerivative = (setpoint - previousTarget) / Window.PERIODIC_FRAME; previousTarget = setpoint; - double staticVal; if (ControlType.getInstance().getSelectedIndex() == ControlType.Types.VELOCITY.index) { return (int) (kS + kV * setpoint + kA * targetDerivative); } else { diff --git a/src/simulator/information/ErrorGraph.java b/src/simulator/information/ErrorGraph.java index cac6f1c..ad1994b 100644 --- a/src/simulator/information/ErrorGraph.java +++ b/src/simulator/information/ErrorGraph.java @@ -4,6 +4,7 @@ import javax.swing.*; import java.awt.*; +import java.awt.geom.Line2D; import java.util.ArrayList; public class ErrorGraph extends JPanel {