diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 1b40185d..94fa7f9b 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -65,10 +65,24 @@ public static class RollerandPivotConstants { public static final double pivotclockwise = 1; public static final double pivotcounterclockwise = -1; public static final double pastsensortime = 0.5; + } + + public static class ClimbConstants { + public static final int LEFT_WINCH_MOTOR_ID = 21; + public static final int LEFT_ZERO_LIMIT_ID = 11; + public static final int RIGHT_WINCH_MOTOR_ID = 22; + public static final int RIGHT_ZERO_LIMIT_ID = 12; + + public static final double WINCH_REDUCTION = 9.49; + public static final double AXLE_PERIMETER_METERS = 6 * Units.inchesToMeters(.289) ; + + public static final double EXTENSION_LIMIT_METERS = Units.inchesToMeters(39); + public static final double EXTENSION_TOLERANCE_METERS = 0.01; + public static final double MAX_WINCH_POWER = 0.6; } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6c30d4e3..481cdc2b 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -13,6 +13,9 @@ import frc.robot.controllers.BaseDriveController; import frc.robot.controllers.DualJoystickDriveController; import frc.robot.controllers.XboxDriveController; +import frc.robot.commands.climb.ClimbLowerCommand; +import frc.robot.commands.climb.ClimbRaiseCommand; +import frc.robot.subsystems.climb.ClimbSubsystem; import frc.robot.subsystems.swerve.BaseSwerveSubsystem; import frc.robot.subsystems.swerve.SingleModuleSwerveSubsystem; import frc.robot.subsystems.swerve.SwerveModule; @@ -29,6 +32,8 @@ import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.button.JoystickButton; +import edu.wpi.first.wpilibj2.command.button.Trigger; +import static frc.robot.Constants.SwerveConstants.*; public class RobotContainer { // The robot's subsystems and commands are defined here... @@ -41,6 +46,8 @@ public class RobotContainer { private final ShooterFeederSubsystem feederSubsystem; private final ShooterPivotSubsystem shooterPivotSubsystem; + private final ClimbSubsystem climbSubsystem; + private final XboxController mechController = new XboxController(2); private final JoystickButton aButton = new JoystickButton(mechController, XboxController.Button.kA.value); @@ -65,6 +72,8 @@ public RobotContainer() { shooterPivotSubsystem = new ShooterPivotSubsystem(false); shooterSubsystem = new ShooterFlywheelSubsystem(); + + climbSubsystem = new ClimbSubsystem(); traj = Choreo.getTrajectory("Curve"); @@ -173,7 +182,7 @@ private void configureBindings() { } - } + } public Command getAutonomousCommand() { if(baseSwerveSubsystem instanceof SwerveSubsystem){ diff --git a/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java b/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java new file mode 100644 index 00000000..57845eae --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/ClimbLowerCommand.java @@ -0,0 +1,33 @@ +package frc.robot.commands.climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climb.ClimbSubsystem; + +public class ClimbLowerCommand extends Command { + private ClimbSubsystem climbSubsystem; + + public ClimbLowerCommand(ClimbSubsystem climbSubsystem){ + this.climbSubsystem = climbSubsystem; + this.addRequirements(climbSubsystem); + } + + @Override + public void initialize() { + System.out.println("LOWERING CLIMB..."); + climbSubsystem.goToExtension(0); + } + + @Override + public boolean isFinished() { + return climbSubsystem.isAtExtension(); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + System.out.println(interrupted ? "CLIMB LOWERING INTERRUPTED!" : "CLIMB LOWERED!"); + } + +} diff --git a/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java b/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java new file mode 100644 index 00000000..eea65a93 --- /dev/null +++ b/src/main/java/frc/robot/commands/climb/ClimbRaiseCommand.java @@ -0,0 +1,34 @@ +package frc.robot.commands.climb; + +import edu.wpi.first.wpilibj2.command.Command; +import frc.robot.subsystems.climb.ClimbSubsystem; +import static frc.robot.Constants.ClimbConstants.*; + +public class ClimbRaiseCommand extends Command { + private ClimbSubsystem climbSubsystem; + + public ClimbRaiseCommand(ClimbSubsystem climbSubsystem){ + this.climbSubsystem = climbSubsystem; + this.addRequirements(climbSubsystem); + } + + @Override + public void initialize() { + System.out.println("RAISING CLIMB..."); + climbSubsystem.goToExtension(EXTENSION_LIMIT_METERS); + } + + @Override + public boolean isFinished() { + return climbSubsystem.isAtExtension(); + } + + @Override + public void execute() {} + + @Override + public void end(boolean interrupted) { + System.out.println(interrupted ? "CLIMB RAISING INTERRUPTED!" : "CLIMB RAISED!"); + } + +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbArm.java b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java new file mode 100644 index 00000000..ba3bb542 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbArm.java @@ -0,0 +1,66 @@ +package frc.robot.subsystems.climb; + +import static frc.robot.Constants.ClimbConstants.*; + +import com.revrobotics.CANSparkBase.IdleMode; +import com.revrobotics.CANSparkBase.SoftLimitDirection; +import com.revrobotics.CANSparkMax; +import com.revrobotics.RelativeEncoder; + +import edu.wpi.first.math.MathUtil; +import edu.wpi.first.wpilibj.DigitalInput; +import frc.robot.util.MotorUtil; + +public class ClimbArm { + private final CANSparkMax winchMotor; + private RelativeEncoder extensionEncoder; + + private final DigitalInput zeroLimitSwitch; + + private double desiredExtension = 0; + + public ClimbArm(int WINCH_MOTOR_ID, int ZERO_LIMIT_ID) { + winchMotor = MotorUtil.createSparkMax(WINCH_MOTOR_ID, (sparkMax) -> { + sparkMax.setIdleMode(IdleMode.kBrake); + sparkMax.setInverted(false); + + extensionEncoder = sparkMax.getEncoder(); + extensionEncoder.setPositionConversionFactor(AXLE_PERIMETER_METERS / WINCH_REDUCTION); + extensionEncoder.setPosition(0); + + sparkMax.setSoftLimit(SoftLimitDirection.kForward, (float) EXTENSION_LIMIT_METERS); + sparkMax.enableSoftLimit(SoftLimitDirection.kForward, true); + sparkMax.setSoftLimit(SoftLimitDirection.kReverse, 0); + sparkMax.enableSoftLimit(SoftLimitDirection.kReverse, true); + }); + + zeroLimitSwitch = new DigitalInput(LEFT_ZERO_LIMIT_ID); + } + + public void update() { + if (zeroLimitSwitch != null && zeroLimitSwitch.get()) + resetEncoder(); + + if (extensionEncoder.getPosition() == 0 && !zeroLimitSwitch.get()) + System.out.println(this.toString() + "encoder uncalibrated!"); + + if (isAtExtension()) + winchMotor.set(0); + else + winchMotor.set(desiredExtension > extensionEncoder.getPosition() + ? MAX_WINCH_POWER : -MAX_WINCH_POWER); + + } + + public void goToExtension(double desiredHeight) { + desiredExtension = MathUtil.clamp(desiredHeight, 0, EXTENSION_LIMIT_METERS); + } + + public boolean isAtExtension() { + return Math.abs(extensionEncoder.getPosition() - desiredExtension) < EXTENSION_TOLERANCE_METERS; + } + + private void resetEncoder() { + extensionEncoder.setPosition(0); + } +} diff --git a/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java new file mode 100644 index 00000000..6e88cddd --- /dev/null +++ b/src/main/java/frc/robot/subsystems/climb/ClimbSubsystem.java @@ -0,0 +1,30 @@ +package frc.robot.subsystems.climb; + +import static frc.robot.Constants.ClimbConstants.*; + +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class ClimbSubsystem extends SubsystemBase{ + private final ClimbArm leftClimbArm; + private final ClimbArm rightClimbArm; + + public ClimbSubsystem() { + leftClimbArm = new ClimbArm(LEFT_WINCH_MOTOR_ID, LEFT_ZERO_LIMIT_ID); + rightClimbArm = new ClimbArm(RIGHT_WINCH_MOTOR_ID, RIGHT_ZERO_LIMIT_ID); + } + + @Override + public void periodic() { + leftClimbArm.update(); + rightClimbArm.update(); + } + + public void goToExtension(double height) { + leftClimbArm.goToExtension(height); + rightClimbArm.goToExtension(height); + } + + public boolean isAtExtension() { + return leftClimbArm.isAtExtension() && rightClimbArm.isAtExtension(); + } +} diff --git a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java index ccf2af24..6a814ca3 100644 --- a/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java +++ b/src/main/java/frc/robot/subsystems/swerve/SwerveModule.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.swerve; import com.revrobotics.CANSparkMax; -import com.revrobotics.RelativeEncoder; +// import com.revrobotics.RelativeEncoder; import com.revrobotics.SparkAnalogSensor; import com.revrobotics.SparkPIDController; import com.revrobotics.CANSparkBase.ControlType; @@ -23,7 +23,7 @@ public class SwerveModule { private final SwerveDriveMotor driveMotor; private final CANSparkMax steerMotor; - private RelativeEncoder steerRelativeEncoder; + // private RelativeEncoder steerRelativeEncoder; private SparkAnalogSensor steerAbsoluteEncoder; private SparkPIDController steerPidController;