Fixing garbage formatting, adding Phoenix and rudimentary climber subsystem

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

@@ -0,0 +1,6 @@
+package frc.robot.constants;
+
+public class ClimberConstants {
+    public static final int kClimberMotor1CANID = 1;
+    public static final int kClimberMotor2CANID = 2;
+}

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

@@ -0,0 +1,39 @@
+package frc.robot.subsystems;
+
+import java.util.function.DoubleSupplier;
+
+import com.ctre.phoenix.motorcontrol.VictorSPXControlMode;
+import com.ctre.phoenix.motorcontrol.can.VictorSPX;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.ClimberConstants;
+
+public class Climber extends SubsystemBase {
+    private VictorSPX motor1;
+    private VictorSPX motor2;
+
+    //TODO What tells the climber to stop climbing when it achieves the target height?
+    public Climber() {
+        motor1 = new VictorSPX(ClimberConstants.kClimberMotor1CANID);
+        motor2 = new VictorSPX(ClimberConstants.kClimberMotor2CANID);
+
+        motor2.follow(motor1);
+    }
+
+    public Command setSpeedWithSupplier(DoubleSupplier speed) {
+        return run(() -> {
+            motor1.set(VictorSPXControlMode.PercentOutput, speed.getAsDouble());
+        });
+    }
+
+    public Command setSpeed(double speed) {
+        return run(() -> {
+            motor1.set(VictorSPXControlMode.PercentOutput, speed);
+        });
+    }
+
+    public Command stop() {
+        return setSpeed(0);
+    }
+}

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

@@ -29,13 +29,21 @@ public class Intake extends SubsystemBase{
         intakePivot.setSmartCurrentLimit(IntakeConstants.kPivotCurrentLimit);
         intakePivot.burnFlash();
 
-        intakeFeedForward = new ArmFeedforward(IntakeConstants.kSFeedForward, IntakeConstants.kGFeedForward, IntakeConstants.kVFeedForward);
+        intakeFeedForward = new ArmFeedforward(
+            IntakeConstants.kSFeedForward, 
+            IntakeConstants.kGFeedForward, 
+            IntakeConstants.kVFeedForward
+        );
 
         intakeEncoder = intakePivot.getEncoder();
         intakeEncoder.setPosition(IntakeConstants.kStartingAngle);
         intakeEncoder.setPositionConversionFactor(IntakeConstants.kIntakePivotConversionFactor);
 
-        intakeAnglePID = new PIDController(IntakeConstants.kPIntake, IntakeConstants.kIIntake, IntakeConstants.kDIntake);
+        intakeAnglePID = new PIDController(
+            IntakeConstants.kPIntake, 
+            IntakeConstants.kIIntake, 
+            IntakeConstants.kDIntake
+        );
 
     }
 

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

@@ -45,25 +45,43 @@ public class Shooter extends SubsystemBase{
 
         pivotEncoder = new Encoder(0, 1);
 
-        topShooterPID = new PIDController(ShooterConstants.kShooterP, ShooterConstants.kShooterI, ShooterConstants.kShooterD);
-        bottomShooterPID = new PIDController(ShooterConstants.kShooterP, ShooterConstants.kShooterI, ShooterConstants.kShooterD);
+        //TODO These can probably devolve into BangBang controllers and let the feed forwards maintain speed
+        topShooterPID = new PIDController(
+            ShooterConstants.kShooterP, 
+            ShooterConstants.kShooterI, 
+            ShooterConstants.kShooterD
+        );
+        bottomShooterPID = new PIDController(
+            ShooterConstants.kShooterP, 
+            ShooterConstants.kShooterI, 
+            ShooterConstants.kShooterD
+        );
 
         topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
         topShooterFF = new SimpleMotorFeedforward(ShooterConstants.kSShooterFF, ShooterConstants.kVShooterFF);
 
         shooterPivotPID = new ProfiledPIDController(
-        ShooterConstants.kShooterPivotP,
-        ShooterConstants.kShooterPivotI,
-        ShooterConstants.kShooterPivotD,
-        new TrapezoidProfile.Constraints(ShooterConstants.kMaxPivotSpeed, ShooterConstants.kMaxPivotAcceleration));
+            ShooterConstants.kShooterPivotP,
+            ShooterConstants.kShooterPivotI,
+            ShooterConstants.kShooterPivotD,
+            new TrapezoidProfile.Constraints(
+                ShooterConstants.kMaxPivotSpeed, 
+                ShooterConstants.kMaxPivotAcceleration
+            )
+        );
 
-        shooterPivotFF = new ArmFeedforward(ShooterConstants.kSShooterPivotFF,
-        ShooterConstants.kGShooterPivotFF, ShooterConstants.kVShooterPivotFF);
+        shooterPivotFF = new ArmFeedforward(
+            ShooterConstants.kSShooterPivotFF,
+            ShooterConstants.kGShooterPivotFF, 
+            ShooterConstants.kVShooterPivotFF
+        );
     }
 
     public Command setShooterAngle(double setpointAngle){
         return run(()-> {
-            shooterPivot.setVoltage(shooterPivotPID.calculate(pivotEncoder.getPosition(), setpointAngle) + );
+            shooterPivot.setVoltage(
+                shooterPivotPID.calculate(pivotEncoder.getDistance(), setpointAngle) + 
+                shooterPivotFF.calculate(setpointAngle, 0));
         });
     }
 }