From d6124f30b44bdf53c47746f982c41bed08b2c06c Mon Sep 17 00:00:00 2001
From: Hayden Heroux <haydenheroux@gmail.com>
Date: Mon, 8 Apr 2024 14:01:09 -0400
Subject: [PATCH] wip

---
 .../lib/controller/PositionControllerIO.java  | 81 +++++++++++++++++++
 .../controller/PositionControllerIOSim.java   | 25 ++++++
 .../lib/controller/VelocityControllerIO.java  |  7 ++
 3 files changed, 113 insertions(+)
 create mode 100644 src/main/java/frc/lib/controller/PositionControllerIO.java
 create mode 100644 src/main/java/frc/lib/controller/PositionControllerIOSim.java

diff --git a/src/main/java/frc/lib/controller/PositionControllerIO.java b/src/main/java/frc/lib/controller/PositionControllerIO.java
new file mode 100644
index 0000000..573f193
--- /dev/null
+++ b/src/main/java/frc/lib/controller/PositionControllerIO.java
@@ -0,0 +1,81 @@
+package frc.lib.controller;
+
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import frc.lib.Telemetry;
+
+/** Position controller interface. */
+public interface PositionControllerIO {
+
+    /** Position controller constants. */
+    public static class PositionControllerIOConstants {
+        /** Interpret counterclockwise rotation on the motor face as having positive velocity, if set to true. */
+        public boolean ccwPositive = true;
+
+        /** Use the motor to brake the controlled mechanism on neutral output, if set to true. */
+        public boolean neutralBrake = false;
+
+        /** Maximum amount of current, in amps, to provide to the motor. */
+        public double currentLimitAmps = 40.0;
+
+        /** Ratio between the velocity sensor and the controlled mechanism. */
+        public double sensorToMechanismRatio = 1.0;
+    }
+
+    /** Position controller values. */
+    public static class PositionControllerIOValues {
+        /** Position in rotations. */
+        public double positionRotations = 0.0;
+
+        /** Velocity in rotations per second. */
+        public double velocityRotationsPerSecond = 0.0;
+
+        /** Acceleration in rotations per second per second. */
+        public double accelerationRotationsPerSecondPerSecond = 0.0;
+
+        /** Motor voltage in volts. */
+        public double motorVolts = 0.0;
+
+        /** Motor current in amps. */
+        public double motorAmps = 0.0;
+    }
+
+    /**
+     * Adds position controller values to Shuffleboard.
+     * 
+     * @param tab
+     * @param name
+     * @param values
+     */
+    public static void addToShuffleboard(ShuffleboardTab tab, String name, PositionControllerIOValues values) {
+        ShuffleboardLayout positionController = Telemetry.addColumn(tab, name);
+
+        positionController.addDouble("Position (rot)", () -> values.positionRotations);
+        positionController.addDouble("Velocity (rps)", () -> values.velocityRotationsPerSecond);
+        positionController.addDouble("Acceleration (rpsps)", () -> values.accelerationRotationsPerSecondPerSecond);
+        positionController.addDouble("Voltage (V)", () -> values.motorVolts);
+        positionController.addDouble("Current (A)", () -> values.motorAmps);
+    }
+    
+    /**
+     * Configures the position controller.
+     * 
+     * @param constants
+     */
+    public void configure(PositionControllerIOConstants constants);
+
+    /**
+     * Updates the position controller's values.
+     * 
+     * @param values
+     */
+    public void update(PositionControllerIOValues values);
+
+    /**
+     * Sets the position setpoint.
+     * 
+     * @param positionRotations
+     * @param velocityRotationsPerSecond
+     */
+    public void setSetpoint(double positionRotations, double velocityRotationsPerSecond);
+}
diff --git a/src/main/java/frc/lib/controller/PositionControllerIOSim.java b/src/main/java/frc/lib/controller/PositionControllerIOSim.java
new file mode 100644
index 0000000..7299e47
--- /dev/null
+++ b/src/main/java/frc/lib/controller/PositionControllerIOSim.java
@@ -0,0 +1,25 @@
+package frc.lib.controller;
+
+/** Simulated position controller. */
+public class PositionControllerIOSim implements PositionControllerIO {
+
+    private double positionRotations = 0.0;
+    
+    private double velocityRotationsPerSecond = 0.0;
+
+    @Override
+    public void configure(PositionControllerIOConstants constants) {}
+
+    @Override
+    public void update(PositionControllerIOValues values) {
+        values.positionRotations = positionRotations;
+        values.velocityRotationsPerSecond = velocityRotationsPerSecond;
+    }
+
+    @Override
+    public void setSetpoint(double positionRotations, double velocityRotationsPerSecond) {
+        this.positionRotations = positionRotations;
+        this.velocityRotationsPerSecond = velocityRotationsPerSecond;
+    }
+    
+}
diff --git a/src/main/java/frc/lib/controller/VelocityControllerIO.java b/src/main/java/frc/lib/controller/VelocityControllerIO.java
index 613796b..5eb1442 100644
--- a/src/main/java/frc/lib/controller/VelocityControllerIO.java
+++ b/src/main/java/frc/lib/controller/VelocityControllerIO.java
@@ -37,6 +37,13 @@ public static class VelocityControllerIOValues {
         public double motorAmps = 0.0;
     }
 
+    /**
+     * Adds velocity controller values to Shuffleboard.
+     * 
+     * @param tab
+     * @param name
+     * @param values
+     */
     public static void addToShuffleboard(ShuffleboardTab tab, String name, VelocityControllerIOValues values) {
         ShuffleboardLayout velocityController = Telemetry.addColumn(tab, name);