diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index 28f2924..4807698 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/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); diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index fdfa49f..c31278f 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/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 */ diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 0313a70..cd58c9c 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/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() {} - + } diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java new file mode 100644 index 0000000..34d8e65 --- /dev/null +++ b/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 + } +}