First subsystems added

src/main/java/frc/robot/constants/ClimberPivotConstants.java

@@ -0,0 +1,8 @@
+package frc.robot.constants;
+
+public class ClimberPivotConstants {
+    public static final int kPivotMotorID = 0;
+    public static final double kPIDControllerP = 0;
+    public static final double kPIDControllerI = 0;
+    public static final double kPIDControllerD = 0;
+}

src/main/java/frc/robot/constants/ClimberRollersConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class ClimberRollersConstants {
+    public static final int kRollerMotorID = 0;
+}

src/main/java/frc/robot/constants/ElevatorConstants.java

@@ -0,0 +1,13 @@
+package frc.robot.constants;
+
+public class ElevatorConstants {
+    public static final int kMotor1ID = 0;
+
+    public static final double kPIDControllerP = 0;
+    public static final double kPIDControllerI = 0;
+    public static final double kPIDControllerD = 0;
+
+    public static final double kFeedForwardS = 0;
+    public static final double kFeedForwardG = 0;
+    public static final double kFeedForwardV = 0;
+}

src/main/java/frc/robot/constants/ManipulatorConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class ManipulatorConstants {
+    
+}

src/main/java/frc/robot/constants/SensorsConstants.java

@@ -0,0 +1,5 @@
+package frc.robot.constants;
+
+public class SensorsConstants {
+    public static final int kClimberLimitSwitchID = 0;
+}

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

@@ -0,0 +1,51 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberPivotConstants;
+
+public class ClimberPivot extends SubsystemBase {
+    private SparkMax pivotMotor;
+
+    private AbsoluteEncoder neoEncoder;
+
+    private PIDController pidController;
+
+    public ClimberPivot() {
+        pivotMotor = new SparkMax(
+            ClimberPivotConstants.kPivotMotorID, 
+            MotorType.kBrushless
+        );
+        pivotMotor.setInverted(true);
+
+        neoEncoder = pivotMotor.getAbsoluteEncoder();
+
+        pidController = new PIDController(
+            ClimberPivotConstants.kPIDControllerP,
+            ClimberPivotConstants.kPIDControllerI, 
+            ClimberPivotConstants.kPIDControllerD
+        );
+    }
+
+    public Command runPivot(double speed) {
+        return run(() -> {
+            pivotMotor.set(speed);
+        });
+    }
+
+    public Command goToAngle(double setpoint) {
+        return run(() -> {
+            pivotMotor.set(
+                pidController.calculate(
+                    neoEncoder.getPosition(), 
+                    setpoint
+                )
+            );
+        });
+    }
+}

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

@@ -0,0 +1,26 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberRollersConstants;
+
+public class ClimberRollers extends SubsystemBase {
+    private SparkMax rollerMotor;
+
+    public ClimberRollers() {
+        rollerMotor = new SparkMax(
+            ClimberRollersConstants.kRollerMotorID, 
+            MotorType.kBrushless
+        );
+        rollerMotor.setInverted(false);
+    }
+
+    public Command runRoller(double speed) {
+        return run(() -> {
+            rollerMotor.set(speed);
+        });
+    }
+}

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

@@ -0,0 +1,60 @@
+package frc.robot.subsystems;
+
+import com.revrobotics.AbsoluteEncoder;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ElevatorConstants;
+
+public class Elevator extends SubsystemBase {
+    private SparkMax motor1;
+
+    private AbsoluteEncoder encoder;
+
+    private PIDController pidController;
+
+    private ElevatorFeedforward feedForward;
+    
+    public Elevator() {
+        motor1 = new SparkMax(
+            ElevatorConstants.kMotor1ID, 
+            MotorType.kBrushless
+        );
+
+        encoder = motor1.getAbsoluteEncoder();
+
+        pidController = new PIDController(
+            ElevatorConstants.kPIDControllerP, 
+            ElevatorConstants.kPIDControllerI, 
+            ElevatorConstants.kPIDControllerD
+        );
+
+        feedForward = new ElevatorFeedforward(
+            ElevatorConstants.kFeedForwardS, 
+            ElevatorConstants.kFeedForwardG, 
+            ElevatorConstants.kFeedForwardV
+        );
+    }
+
+    public Command runElevator(double speed) {
+        return run(() -> {
+            motor1.set(speed);
+        });
+    }
+    
+    public Command goToSetpoint(double setpoint) {
+        return run(() -> {
+            motor1.set(feedForward.calculate(
+                encoder.getVelocity(), 
+                pidController.calculate(
+                    encoder.getPosition(), 
+                    setpoint
+                )
+            ));
+        });
+    }
+}

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

@@ -0,0 +1,17 @@
+package frc.robot.subsystems;
+
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.SensorsConstants;
+
+public class Sensors extends SubsystemBase {
+    private DigitalInput climberLimitSwitch;
+    
+    public Sensors() {
+        climberLimitSwitch = new DigitalInput(SensorsConstants.kClimberLimitSwitchID);
+    }
+
+    public boolean getClimberLimitSwitch() {
+        return climberLimitSwitch.get();
+    }
+}