Merge pull request #40 from Team4388/Climber

Created Climber Subsystem

src/main/java/frc4388/robot/Constants.java

@@ -21,6 +21,7 @@ import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.OpenLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.ctre.phoenix6.signals.StaticFeedforwardSignValue;
 import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
@@ -415,7 +416,10 @@ public final class Constants {
         public static final double LEFT_AXIS_DEADBAND = 0.1;
 
     }
-
+    public static final class ClimberConstants {
+        public static final CanDevice CLIMBER_ID = new CanDevice("Climber", 311);
+        public static final double CLIMBER_SPEED = 0.1;
+    }
     public static final class ElevatorConstants {
         public static final CanDevice ENDEFFECTOR_ID = new CanDevice("Endeffector", 15);
         public static final CanDevice ELEVATOR_ID = new CanDevice("Elevator", 16);

src/main/java/frc4388/robot/RobotContainer.java

@@ -61,6 +61,7 @@ import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.Vision;
 import frc4388.robot.subsystems.Elevator.CoordinationState;
 import frc4388.robot.subsystems.Lidar;
+import frc4388.robot.subsystems.Climber;
 import frc4388.robot.subsystems.Elevator;
 // import frc4388.robot.subsystems.Endeffector;
 import frc4388.robot.subsystems.SwerveDrive;
@@ -91,6 +92,7 @@ public class RobotContainer {
     public final Lidar m_lidar = new Lidar();
     public final Elevator m_robotElevator = new Elevator(m_robotMap.elevator, m_robotMap.endeffector, m_robotMap.basinLimitSwitch, m_robotMap.endeffectorLimitSwitch, m_robotLED);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
+    public final Climber m_robotClimber = new Climber(m_robotMap.climber);
     // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
 
 
@@ -542,6 +544,14 @@ public class RobotContainer {
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0 && !operatorManualMode)
             .onTrue(rightAlgaeRemove);
 
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 90)
+            .onTrue(new InstantCommand(() -> m_robotClimber.climberOut()))
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getPOV() == 270)
+            .onTrue(new InstantCommand(() -> m_robotClimber.climberIn()))
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimber()));
+        
         // ? /* Programer Buttons (Controller 3)*/
 
         // * /* Auto Recording */

src/main/java/frc4388/robot/RobotMap.java

@@ -7,25 +7,16 @@
 
 package frc4388.robot;
 
-import com.ctre.phoenix6.hardware.TalonFX;
-
 import org.photonvision.PhotonCamera;
 
 import com.ctre.phoenix6.hardware.CANcoder;
-import com.ctre.phoenix6.hardware.Pigeon2;
+import com.ctre.phoenix6.hardware.TalonFX;
 import com.ctre.phoenix6.swerve.SwerveDrivetrain;
-import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
-import com.ctre.phoenix6.swerve.SwerveModuleConstants;
-import com.ctre.phoenix6.swerve.SwerveModuleConstantsFactory;
 
 import edu.wpi.first.wpilibj.DigitalInput;
+import frc4388.robot.Constants.ClimberConstants;
 import frc4388.robot.Constants.ElevatorConstants;
-// import edu.wpi.first.wpilibj.motorcontrol.Spark;
-// import frc4388.robot.Constants.LEDConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.robot.Constants.VisionConstants;
-// import frc4388.robot.subsystems.SwerveModule;
-import frc4388.utility.RobotGyro;
 
 /**
  * Defines and holds all I/O objects on the Roborio. This is useful for unit
@@ -59,6 +50,9 @@ public class RobotMap {
     public final DigitalInput basinLimitSwitch = new DigitalInput(ElevatorConstants.BASIN_LIMIT_SWITCH);
     public final DigitalInput endeffectorLimitSwitch = new DigitalInput(ElevatorConstants.ENDEFFECTOR_LIMIT_SWITCH);
 
+    /* Climber Subsytem */
+    public final TalonFX climber = new TalonFX(ClimberConstants.CLIMBER_ID.id);
+
     void configureDriveMotorControllers() {}
     
 }

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

@@ -0,0 +1,43 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.subsystems;
+
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc4388.robot.Constants.ClimberConstants;
+
+public class Climber extends SubsystemBase {
+  /** Creates a new Climber. */
+
+  public TalonFX climberMotor;
+
+  public Climber(TalonFX climberTalonFX) {
+    climberMotor = climberTalonFX;
+    climberMotor.setNeutralMode(NeutralModeValue.Brake);
+
+  }
+
+  public void stopClimber(){
+    climberMotor.set(0);
+  }
+
+
+  public void climberOut(){
+    climberMotor.set(ClimberConstants.CLIMBER_SPEED);
+
+  }
+
+  public void climberIn(){
+    climberMotor.set(-ClimberConstants.CLIMBER_SPEED);
+
+  }
+
+  @Override
+  public void periodic() {
+    // This method will be called once per scheduler run
+  }
+}