First couple of Constants files

src/main/java/first/robot/constants/AutoConstants.java

@@ -0,0 +1,60 @@
+package first.robot.constants;
+
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+import org.wpilib.math.trajectory.TrapezoidProfile;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+import com.pathplanner.lib.path.PathConstraints;
+
+public class AutoConstants {
+    public static final double kMaxSpeedMetersPerSecond = 4;
+    public static final double kMaxAccelerationMetersPerSecondSquared = 4;
+    public static final double kMaxAngularSpeedRadiansPerSecond = Math.PI;
+    public static final double kMaxAngularAccelerationRadiansPerSecondSquared = Math.PI;
+
+    public static final double kMaxSpeedMetersPerSecondAutoAlign = 2.5;
+
+    public static final double kPXYController = 3.5;
+    public static final double kPThetaController = 5;
+
+    public static final double kAlignPXYController = 2;
+    public static final double kAlignPThetaController = 5;
+
+    // Constraint for the motion profiled robot angle controller
+    public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    public static final TrapezoidProfile.Constraints kAlignThetaControllerConstraints = new TrapezoidProfile.Constraints(
+        kMaxAngularSpeedRadiansPerSecond, kMaxAngularAccelerationRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+    public static boolean kAutoConfigOk;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXYController, 0, 0),
+        new PIDConstants(kPThetaController, 0, 0)
+    );
+
+    public static final PathConstraints kOnTheFlyConstraints = new PathConstraints(
+        kMaxSpeedMetersPerSecond, 
+        kMaxAccelerationMetersPerSecondSquared, 
+        kMaxAngularSpeedRadiansPerSecond, 
+        kMaxAngularAccelerationRadiansPerSecondSquared
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+            kAutoConfigOk = true;
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+            kAutoConfigOk = false;
+        }
+    }
+}

src/main/java/first/robot/constants/ClimberConstants.java

@@ -0,0 +1,68 @@
+package first.robot.constants;
+
+import java.util.function.Function;
+
+import com.revrobotics.spark.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+public class ClimberConstants {
+    // TODO Real values
+    public enum ClimberPositions {
+        kStow(0),
+        kClimbOffGround(0),
+        kUp(0);
+
+        private double positionMeters;
+
+        private ClimberPositions(double positionMeters) {
+            this.positionMeters = positionMeters;
+        }
+
+        public double getPositionMeters() {
+            return positionMeters;
+        }
+    }
+
+    public static final int kMotorCANID = 0;
+
+    public static final double kConversionFactor = 0;
+
+    public static final double kP = 0;
+    public static final double kI = 0;
+    public static final double kD = 0;
+    public static final double kS = 0;
+    public static final double kV = 0;
+    public static final double kA = 0;
+
+    public static final boolean kMotorInverted = false;
+
+    public static final int kCurrentLimit = 40;
+
+    public static final IdleMode kIdleMode = IdleMode.kBrake;
+
+    // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM
+
+    public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig();
+
+    public static final Function<Double, Double> kPositionConversionFunction = (d) -> {
+        return d * kConversionFactor;
+    };
+
+    public static final Function<Double, Double> kVelocityConversionFunction = (d) -> {
+        return kPositionConversionFunction.apply(d) / 60;
+    };
+
+    static {
+        kMotorConfig
+            .inverted(kMotorInverted)
+            .smartCurrentLimit(kCurrentLimit)
+            .idleMode(kIdleMode);
+        kMotorConfig.closedLoop
+            .feedbackSensor(FeedbackSensor.kPrimaryEncoder)
+            .pid(kP, kI, kD)
+            .outputRange(-1, 1)
+            .feedForward
+                .sva(kS, kV, kA);
+    }
+}