Feature/climber

src/main/java/frc/robot/Robot.java

@@ -43,7 +43,7 @@ public Robot() {
         partsLogger.logCommandScheduler();
         partsLogger.logPathPlanner();
 
-        CameraServer.startAutomaticCapture();
+        //CameraServer.startAutomaticCapture();
 
         //m_robotContainer.resetStartPose();
         m_robotContainer.setMegaTagMode(MegaTagMode.MEGATAG1);

src/main/java/frc/robot/RobotContainer.java

@@ -39,6 +39,9 @@
 import frc.robot.subsystems.Candle;
 import frc.robot.subsystems.LimelightVision;
 import frc.robot.subsystems.Superstructure;
+import frc.robot.subsystems.Climber.Climber;
+import frc.robot.subsystems.Climber.ClimberPhys;
+import frc.robot.subsystems.Climber.ClimberSim;
 import frc.robot.subsystems.LimelightVision.MegaTagMode;
 import frc.robot.subsystems.Drivetrain.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Drivetrain.PARTsDrivetrain;
@@ -72,7 +75,7 @@
 public class RobotContainer {
     private FieldObject2d hubFieldObject2d;
 
-    private final PARTsCommandController driveController = new PARTsCommandController(0, ControllerType.DS5);
+    private final PARTsCommandController driveController = new PARTsCommandController(0, ControllerType.XBOX);
     private final PARTsCommandController operatorController = new PARTsCommandController(1,
             RobotConstants.ALLOW_AUTO_CONTROLLER_DETECTION);
     private final PARTsButtonBoxController buttonBoxController = new PARTsButtonBoxController(2);
@@ -108,15 +111,19 @@ public class RobotContainer {
 
     private final Intake intake = Robot.isReal() ? new IntakePhys() : new IntakeSim();
 
+    private final Climber climber = Robot.isReal() ? new ClimberPhys() : new ClimberSim();
+
     // private final ShooterSysid shooter = new
     // ShooterSysid(drivetrain.supplierGetPose()); //for sysid
     // private final IntakeSysid intake = new IntakeSysid(); //for sysid
     // private final TurretSysid turret = new
     // TurretSysid(drivetrain.supplierGetPose());
 
-    private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret, candle, drivetrain);
+    private final Superstructure superstructure = new Superstructure(hopper, intake, kicker, shooter, turret, candle,
+            drivetrain);
     private final ArrayList<IPARTsSubsystem> subsystems = new ArrayList<IPARTsSubsystem>(
-            Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake, superstructure));
+            Arrays.asList(candle, drivetrain, vision, shooter, turret, kicker, hopper, intake, superstructure,
+                    climber));
 
     // endregion End Subsystems
 
@@ -128,7 +135,8 @@ public RobotContainer() {
         configureAutonomousCommands();
         configureIntakeBindings();
         configureHopperBindings();
-        configureSuperstructureBindings();
+        configureSuperstructureBindings();                              
+        configureClimberBindings();
         operatorController.povUp().onTrue(Commands.runOnce(() -> SignalLogger.start()));
         operatorController.povDown().onTrue(Commands.runOnce(() -> SignalLogger.stop()));
 
@@ -239,11 +247,14 @@ private void configureTurretBindings() {
     private void configureIntakeBindings() {
         buttonBoxController.positive4Trigger().onTrue(intake.intakeShooting());
         buttonBoxController.negative4Trigger().onTrue(intake.intake());
-        buttonBoxController.positive4Trigger().negate().and(buttonBoxController.negative4Trigger().negate()).onTrue(intake.intakeIdle());
-        /*driveController.x().onTrue(intake.intake());
-        driveController.y().onTrue(intake.intakeIdle());
-        driveController.rightTrigger().onTrue(intake.intakeShooting());
-        //driveController.x().onTrue(intake.home());*/
+        buttonBoxController.positive4Trigger().negate().and(buttonBoxController.negative4Trigger().negate())
+                .onTrue(intake.intakeIdle());
+        /*
+         * driveController.x().onTrue(intake.intake());
+         * driveController.y().onTrue(intake.intakeIdle());
+         * driveController.rightTrigger().onTrue(intake.intakeShooting());
+         * //driveController.x().onTrue(intake.home());
+         */
 
         /*
          * operatorController.a().and(operatorController.rightBumper())
@@ -259,7 +270,13 @@ private void configureIntakeBindings() {
     }
 
     private void configureSuperstructureBindings() {
-        buttonBoxController.handleTrigger().onTrue(superstructure.shoot(buttonBoxController.cruiseTrigger()::getAsBoolean));
+        buttonBoxController.handleTrigger()
+                .onTrue(superstructure.shoot(buttonBoxController.cruiseTrigger()::getAsBoolean));
+    }
+
+    private void configureClimberBindings() {
+        driveController.a().whileTrue(climber.climb()).onFalse(climber.idle());
+        driveController.b().whileTrue(climber.declimb()).onFalse(climber.idle());
     }
 
     public void configureAutonomousCommands() {

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

@@ -0,0 +1,26 @@
+package frc.robot.constants;
+
+public class ClimberConstants {
+    public enum ClimberState {
+        IDLE(0),
+        DISABLED(0),
+        CLIMBING(0.5),
+        DECLIMB(-0.5);
+
+        private final double speed;
+
+        ClimberState(double speed) {
+            this.speed = speed;
+        }
+
+        public double getSpeed() {
+            return speed;
+        }
+    }
+
+    public static final int CLIMBER_MOTOR_ID = 4;
+    public static final String CAN_BUS_NAME = "hi";
+    public static final double CLIMBER_GEAR_RATIO = 4.0/1.0;
+
+
+}

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

@@ -29,7 +29,7 @@ public double getAngle() {
 
     public static final int INTAKE_MOTOR_ID = 36;
     public static final int PIVOT_MOTOR_ID = 38;
-    public static final String CAN_BUS_NAME = "bye";
+    public static final String CAN_BUS_NAME = "hi";
 
     /** The pivot gear ratio. The total is {@code 36/1}. */
     public static final double PIVOT_GEAR_RATIO = (12.0 / 1.0) * (3.0 / 1.0);

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

@@ -0,0 +1,90 @@
+package frc.robot.subsystems.Climber;
+
+import org.parts3492.partslib.command.PARTsCommandUtils;
+import org.parts3492.partslib.command.PARTsSubsystem;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.constants.ClimberConstants;
+import frc.robot.constants.ClimberConstants.ClimberState;
+
+public abstract class Climber extends PARTsSubsystem {
+    private ClimberState climberState = ClimberState.IDLE;
+
+    public Climber() {
+        super("Climber");
+    }
+
+    // region Generic Subsystem Functions
+    @Override
+    public void outputTelemetry() {
+        partsNT.putString("Climber State", climberState.toString());
+    }
+
+    @Override
+    public void stop() {
+        climberState = ClimberState.DISABLED;
+    }
+
+    @Override
+    public void reset() {
+        climberState = ClimberState.IDLE;
+    }
+
+    @Override
+    public void log() {
+        partsLogger.logString("Climber State", climberState.toString());
+    }
+    // endregion
+
+    @Override
+    public void periodic() {
+        switch (climberState) {
+            case CLIMBING:
+                setSpeed(climberState.getSpeed());
+                break;
+            case DECLIMB:
+                setSpeed(climberState.getSpeed());
+                break;
+            case DISABLED:
+                setSpeed(climberState.getSpeed());
+                break;
+            case IDLE:
+                setSpeed(climberState.getSpeed());
+                break;
+            default:
+                setSpeed(0);
+                break;
+        }
+    }
+
+    // region Custom Public Functions
+    /**
+     * Sets the speed of the Shooter.
+     * 
+     * @param speed The speed between <code>-1.0</code> and <code>1.0</code>.
+     */
+    protected abstract void setSpeed(double speed);
+
+    public ClimberState getState() {
+        return climberState;
+    }
+
+    public Command idle() {
+        return PARTsCommandUtils.setCommandName("Command Idle", this.runOnce(() -> {
+            climberState = ClimberState.IDLE;
+        }));
+    }
+
+    public Command climb() {
+        return PARTsCommandUtils.setCommandName("Command Climb", this.runOnce(() -> {
+            climberState = ClimberConstants.ClimberState.CLIMBING;
+        }));
+    }
+
+    public Command declimb() {
+        return PARTsCommandUtils.setCommandName("Command Declimb", this.runOnce(() -> {
+            climberState = ClimberConstants.ClimberState.DECLIMB;
+        }));
+    }
+    // endregion
+}

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

@@ -0,0 +1,53 @@
+package frc.robot.subsystems.Climber;
+
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import com.revrobotics.PersistMode;
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.revrobotics.spark.config.SparkMaxConfig;
+
+import frc.robot.constants.ClimberConstants;
+
+public class ClimberPhys extends Climber {
+    protected final SparkMax climberMotor;
+    protected final RelativeEncoder climberEncoder;
+
+    public ClimberPhys() {
+        super();
+
+        SparkMaxConfig climberConfig = new SparkMaxConfig();
+        climberConfig.idleMode(IdleMode.kBrake);
+
+        climberMotor = new SparkMax(ClimberConstants.CLIMBER_MOTOR_ID, MotorType.kBrushless);
+        climberEncoder = climberMotor.getEncoder();
+        climberMotor.configure(climberConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    protected void setSpeed(double speed) {
+        climberMotor.set(speed);
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+    }
+
+    @Override
+    public void outputTelemetry() {
+        partsNT.putDouble("Output", climberMotor.getOutputCurrent());
+        partsNT.putDouble("Current", climberMotor.getAppliedOutput());
+    }
+
+    @Override
+    public void log() {
+        partsLogger.logDouble("Output", climberMotor.getOutputCurrent());
+        partsLogger.logDouble("Current", climberMotor.getAppliedOutput());
+    }
+}

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

@@ -0,0 +1,8 @@
+package frc.robot.subsystems.Climber;
+
+public class ClimberSim extends Climber {
+    @Override
+    protected void setSpeed(double speed) {
+        partsNT.putDouble("Our Climber Speed", speed);
+    }
+}