diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 5853bfb..f98c824 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -112,7 +112,7 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(); /* Subsystems */ - // public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); + public final Climber m_robotClimber = new Climber(m_robotMap.shoulder, m_robotMap.elbow, m_robotMap.gyro, false); public final Claws m_robotClaws = new Claws(m_robotMap.leftClaw, m_robotMap.rightClaw); public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.leftFront, m_robotMap.leftBack, m_robotMap.rightFront, m_robotMap.rightBack, m_robotMap.gyro); public final Serializer m_robotSerializer = new Serializer(m_robotMap.serializerBelt, /*m_robotMap.serializerShooterBelt,*/ m_robotMap.serializerBeam); @@ -128,7 +128,7 @@ public class RobotContainer { private final WPI_TalonFX testShoulderMotor = new WPI_TalonFX(30); private final WPI_TalonFX testElbowMotor = new WPI_TalonFX(31); - //public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); + // public final Climber m_robotClimber = new Climber(testShoulderMotor, testElbowMotor); // Controllers private final XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); @@ -200,7 +200,7 @@ public class RobotContainer { getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); - m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.35), m_robotBoomBoom)); + m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); // m_robotStorage.setDefaultCommand( // new ManageStorage(m_robotStorage, @@ -235,15 +235,12 @@ public class RobotContainer { if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> { - // if (this.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0, 0); } - // if (this.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - // getOperatorController().getRightY() * 10000); }} - // , m_robotClimber)); + m_robotClimber.setDefaultCommand( + new RunCommand(() -> m_robotClimber.setMotors(0.0, getOperatorController().getRightY() * 0.5) + , m_robotClimber)); - // m_robotTurret.setDefaultCommand( - // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); + // m_robotTurret.setDefaultCommand( + // new AimToCenter(m_robotTurret, m_robotSwerveDrive, m_robotVisionOdometry)); // continually sends updates to the Blinkin LED controller to keep the lights on // m_robotLED.setDefaultCommand(new RunCommand(m_robotLED::updateLED, m_robotLED).withName("LED update defaultCommand")); @@ -355,7 +352,7 @@ public class RobotContainer { .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - // .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) + .whenPressed(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(false), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) @@ -364,13 +361,13 @@ public class RobotContainer { .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - // .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setClimberSoftLimits(true), m_robotClimber)) .whenReleased(new InstantCommand(() -> m_robotTurret.m_boomBoomRotateEncoder.setPosition(0), m_robotTurret)) .whenReleased(new InstantCommand(() -> m_robotHood.m_angleEncoder.setPosition(0), m_robotHood)) .whenReleased(new InstantCommand(() -> m_robotExtender.setEncoder(0), m_robotExtender)) - .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)); - // .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); + .whenReleased(new InstantCommand(() -> ExtenderIntakeGroup.setDirectionToOut(), m_robotIntake, m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotClimber.setEncoders(0), m_robotClimber)); new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 3a92ee8..1db64ed 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -196,8 +196,8 @@ public class RobotMap { } /* Climb Subsystem */ -// public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO -// public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO + public final WPI_TalonFX shoulder = new WPI_TalonFX(ClimberConstants.SHOULDER_ID); // TODO + public final WPI_TalonFX elbow = new WPI_TalonFX(ClimberConstants.ELBOW_ID); // TODO /* Hooks Subsystem */ // public final CANSparkMax leftClaw = new CANSparkMax(ClawConstants.LEFT_CLAW_ID, MotorType.kBrushless); diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index f0ababd..666f9be 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -164,8 +164,8 @@ public class Climber extends SubsystemBase { // } public void setMotors(double shoulderOutput, double elbowOutput) { - m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER * this.shoulderSpeedLimiter); - m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER * this.elbowSpeedLimiter); + m_shoulder.set(shoulderOutput * ClimberConstants.INPUT_MULTIPLIER );//* this.shoulderSpeedLimiter); + m_elbow.set(elbowOutput * ClimberConstants.INPUT_MULTIPLIER);// * this.elbowSpeedLimiter); } public double[] getJointAngles() { @@ -206,7 +206,11 @@ public class Climber extends SubsystemBase { public void setJointSpeeds(double[] speeds) { setJointSpeeds(speeds[0], speeds[1]); } - + /** + * Velocity PID set for joints + * @param shoulderSpeed + * @param elbowSpeed + */ public void setJointSpeeds(double shoulderSpeed, double elbowSpeed) { m_shoulder.set(TalonFXControlMode.Velocity, shoulderSpeed); m_elbow.set(TalonFXControlMode.Velocity, elbowSpeed); @@ -220,6 +224,7 @@ public class Climber extends SubsystemBase { * @param yInput Rate of change of Y position of target point * @deprecated use controlJointsWithInput() instead */ + @Deprecated public void controlWithInput(double xInput, double yInput) { moving = xInput != 0 || yInput != 0; @@ -310,7 +315,7 @@ public class Climber extends SubsystemBase { * @return [shoulderAngle, elbowAngle] in radians * @deprecated * */ - + @Deprecated public static double[] getTargetJointAngles(Point targetPoint, double tiltAngle) { double [] angles = new double[2];