diff --git a/src/main/java/frc/robot/commands/ClimberCommand.java b/src/main/java/frc/robot/commands/ClimberCommand.java new file mode 100644 index 0000000..4e44963 --- /dev/null +++ b/src/main/java/frc/robot/commands/ClimberCommand.java @@ -0,0 +1,27 @@ +package frc.robot.commands; + +import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climber.ClimberSubsystem; + +public class ClimberCommand extends Command{ + private final ClimberSubsystem climberSubsystem; + private final double climberTarget; + + public ClimberCommand(ClimberSubsystem climberSubsystem, double climberPosition) { + this.climberSubsystem = climberSubsystem; + this.climberTarget = climberPosition; + } + public void initialize() { + + climberSubsystem.setClimberTargetPosition(climberTarget); + } + public void execute() { + climberSubsystem.runClimber(); + SmartDashboard.putNumber("climberMotor", climberSubsystem.getClimberPosition()); + SmartDashboard.putNumber("climberTarget", climberTarget); + } + public void end(boolean interrupted) { + climberSubsystem.resetClimberPosition(); + } +} diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java new file mode 100644 index 0000000..13dbfb3 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -0,0 +1,13 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.CANBus; + +public class ClimberConstants { + public static final CANBus climberBus = new CANBus("GTX7130"); + public static final int leftMotorID = 20; + public static final int rightMotorID = 21; + + public static final double climberKP = 0.1; + public static final double climberKI = 0.0; + public static final double climberKD = 0.0; +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java new file mode 100644 index 0000000..f38ff9e --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climber/ClimberSubsystem.java @@ -0,0 +1,51 @@ +package frc.robot.subsystems.climber; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.configs.FeedbackConfigs; +import com.ctre.phoenix6.configs.MotorOutputConfigs; +import com.ctre.phoenix6.configs.TalonFXConfiguration; +import com.ctre.phoenix6.controls.Follower; +import com.ctre.phoenix6.hardware.TalonFX; +import com.ctre.phoenix6.signals.InvertedValue; +import com.ctre.phoenix6.signals.MotorAlignmentValue; + +import edu.wpi.first.math.controller.PIDController; +import frc.robot.subsystems.climber.ClimberConstants; + +public class ClimberSubsystem { + private TalonFX leftMotor = new TalonFX(ClimberConstants.leftMotorID,ClimberConstants.climberBus); + private TalonFX rightMotor = new TalonFX(ClimberConstants.rightMotorID,ClimberConstants.climberBus); + + private PIDController climberPID = new PIDController(ClimberConstants.climberKP,ClimberConstants.climberKI, ClimberConstants.climberKD); + + public ClimberSubsystem() { + TalonFXConfiguration leftConfiguration = new TalonFXConfiguration(); + TalonFXConfiguration rightConfiguration = new TalonFXConfiguration(); + leftConfiguration.Feedback.withSensorToMechanismRatio(12); + rightConfiguration.Feedback.withSensorToMechanismRatio(12); + + MotorOutputConfigs leftConfigs = new MotorOutputConfigs(); + MotorOutputConfigs rightConfigs = new MotorOutputConfigs(); + leftConfigs.Inverted = InvertedValue.CounterClockwise_Positive; + rightConfigs.Inverted = InvertedValue.Clockwise_Positive; + leftMotor.setControl(new Follower(ClimberConstants.rightMotorID , MotorAlignmentValue.Aligned)); + rightMotor.setPosition(0); + } + + public double getClimberPosition() { + return (leftMotor.getPosition().getValueAsDouble()+rightMotor.getPosition().getValueAsDouble())/2; + } + + public void resetClimberPosition() { + rightMotor.setPosition(0); + } + + public void setClimberTargetPosition(double position) { + climberPID.setSetpoint(position); + } + + public void runClimber() { + leftMotor.setVoltage(climberPID.calculate((leftMotor.getPosition().getValueAsDouble() + + rightMotor.getPosition().getValueAsDouble())/2)); + } +}