diff --git a/src/main/java/com/bthsrobotics/motorsim/robot/Robot.java b/src/main/java/com/bthsrobotics/motorsim/robot/Robot.java index c21443a..34897d2 100644 --- a/src/main/java/com/bthsrobotics/motorsim/robot/Robot.java +++ b/src/main/java/com/bthsrobotics/motorsim/robot/Robot.java @@ -26,9 +26,12 @@ public Robot() { // This method will be called once every 20ms public void periodic() { motor.getFrame().setMarkerPosition(reference); - + + // CODE HERE + // create error and deltaError variables double error = reference - motor.getPosition(); - double deltaError = (error - prevError) / period; + double deltaError = error - prevError; + // CODE HERE prevError = error; @@ -38,8 +41,10 @@ public void periodic() { hasMeasurement = true; } - double pid = kP * error + kD * deltaError; - - motor.setVolts(pid); + // CODE HERE + // calculate input variable, setVolts + double input = kP * error + kD * (deltaError / period); + motor.setVolts(input); + // CODE HERE } } diff --git a/target/classes/com/bthsrobotics/motorsim/robot/Robot.class b/target/classes/com/bthsrobotics/motorsim/robot/Robot.class index ed2d067..68905a6 100644 Binary files a/target/classes/com/bthsrobotics/motorsim/robot/Robot.class and b/target/classes/com/bthsrobotics/motorsim/robot/Robot.class differ