Added beta subsystems

src/main/java/frc/robot/maps/subsystems/CoralManipMap.java

@@ -0,0 +1,30 @@
+package frc.robot.maps.subsystems;
+
+import com.chopshop166.chopshoplib.logging.DataWrapper;
+import com.chopshop166.chopshoplib.logging.LoggableMap;
+import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
+import com.chopshop166.chopshoplib.motors.SmartMotorController;
+
+public class CoralManipMap implements LoggableMap<CoralManipMap.Data> {
+    public SmartMotorController motor;
+
+    public CoralManipMap() {
+        this(new SmartMotorController());
+    }
+
+    public CoralManipMap(SmartMotorController motor) {
+        this.motor = motor;
+    }
+
+    // Will add data/logging stuff when we need it
+
+    @Override
+    public void updateData(Data data) {
+        data.motor.updateData(motor);
+    }
+
+    public static class Data extends DataWrapper {
+        public MotorControllerData motor = new MotorControllerData();
+    }
+
+}

src/main/java/frc/robot/maps/subsystems/DeepClimbMap.java

@@ -0,0 +1,30 @@
+package frc.robot.maps.subsystems;
+
+import com.chopshop166.chopshoplib.logging.DataWrapper;
+import com.chopshop166.chopshoplib.logging.LoggableMap;
+import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
+import com.chopshop166.chopshoplib.motors.SmartMotorController;
+
+public class DeepClimbMap implements LoggableMap<DeepClimbMap.Data> {
+
+    public SmartMotorController motor;
+
+    public DeepClimbMap() {
+        this(new SmartMotorController());
+    }
+
+    public DeepClimbMap(SmartMotorController motor) {
+        this.motor = motor;
+    }
+
+    // Will add data/logging stuff when we need it
+
+    @Override
+    public void updateData(Data data) {
+        data.motor.updateData(motor);
+    }
+
+    public static class Data extends DataWrapper {
+        public MotorControllerData motor = new MotorControllerData();
+    }
+}

src/main/java/frc/robot/maps/subsystems/ElevatorMap.java

@@ -0,0 +1,30 @@
+package frc.robot.maps.subsystems;
+
+import com.chopshop166.chopshoplib.logging.DataWrapper;
+import com.chopshop166.chopshoplib.logging.LoggableMap;
+import com.chopshop166.chopshoplib.logging.data.MotorControllerData;
+import com.chopshop166.chopshoplib.motors.SmartMotorController;
+
+public class ElevatorMap implements LoggableMap<ElevatorMap.Data> {
+
+    public SmartMotorController motor;
+
+    public ElevatorMap() {
+        this(new SmartMotorController());
+    }
+
+    public ElevatorMap(SmartMotorController motor) {
+        this.motor = motor;
+    }
+
+    // Will add data/logging stuff when we need it
+
+    @Override
+    public void updateData(Data data) {
+        data.motor.updateData(motor);
+    }
+
+    public static class Data extends DataWrapper {
+        public MotorControllerData motor = new MotorControllerData();
+    }
+}

src/main/java/frc/robot/subsystems/CoralManip.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems;
+
+import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
+
+import frc.robot.maps.subsystems.CoralManipMap;
+import frc.robot.maps.subsystems.CoralManipMap.Data;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
+
+    public CoralManip(CoralManipMap coralManipMap) {
+        super(new Data(), coralManipMap);
+    }
+
+    @Override
+    public void safeState() {
+        // TODO Auto-generated method stub
+        throw new UnsupportedOperationException("Unimplemented method 'safeState'");
+    }
+
+}

src/main/java/frc/robot/subsystems/DeepClimb.java

@@ -0,0 +1,21 @@
+package frc.robot.subsystems;
+
+import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
+
+import frc.robot.maps.subsystems.DeepClimbMap;
+import frc.robot.maps.subsystems.DeepClimbMap.Data;
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class DeepClimb extends LoggedSubsystem<Data, DeepClimbMap> {
+
+    public DeepClimb(DeepClimbMap deepClimbMap) {
+        super(new Data(), deepClimbMap);
+    }
+
+    @Override
+    public void safeState() {
+        // TODO Auto-generated method stub
+        throw new UnsupportedOperationException("Unimplemented method 'safeState'");
+    }
+
+}

src/main/java/frc/robot/subsystems/Elevator.java

@@ -0,0 +1,22 @@
+package frc.robot.subsystems;
+
+import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
+
+import frc.robot.maps.subsystems.ElevatorMap;
+import frc.robot.maps.subsystems.ElevatorMap.Data;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class Elevator extends LoggedSubsystem<Data, ElevatorMap> {
+
+    public Elevator(ElevatorMap elevatorMap) {
+        super(new Data(), elevatorMap);
+    }
+
+    @Override
+    public void safeState() {
+        // TODO Auto-generated method stub
+        throw new UnsupportedOperationException("Unimplemented method 'safeState'");
+    }
+
+}

src/main/java/frc/robot/util/DrivePID.java

@@ -9,7 +9,7 @@
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 public class DrivePID {
-    
+
     private final ProfiledPIDController xPid;
     private final ProfiledPIDController yPid;
     private final RotationPIDController anglePid;
@@ -55,7 +55,7 @@ public void reset(Translation2d measurement) {
     public Transform2d calculate(Pose2d currentPose, Pose2d targetPose) {
         double x = xPid.calculate(currentPose.getX(), targetPose.getX());
         if (Math.abs(x) > 0.01) {
-        x += Math.signum(x) * 0.15;
+            x += Math.signum(x) * 0.15;
         }
         double y = yPid.calculate(currentPose.getY(), targetPose.getY());
         if (Math.abs(y) > 0.01) {