diff --git a/src/main/java/frc4388/robot/Constants.java b/src/main/java/frc4388/robot/Constants.java index e1dcc07..b46a442 100644 --- a/src/main/java/frc4388/robot/Constants.java +++ b/src/main/java/frc4388/robot/Constants.java @@ -175,7 +175,7 @@ public final class Constants { public static final int ELBOW_ID = 31; public static final int GYRO_ID = 14; - public static final double INPUT_MULTIPLIER = 0.7; + public static final double INPUT_MULTIPLIER = 0.4; public static final double ELBOW_SOFT_LIMIT_TOLERANCE = 20000.0; public static final double SHOULDER_SOFT_LIMIT_TOLERANCE = 12000.0; @@ -269,7 +269,7 @@ public final class Constants { true, 27, 0, 0); public static final int SHOOTER_FALCON_LEFT_CAN_ID = 21; public static final int SHOOTER_FALCON_RIGHT_CAN_ID = 22; - public static final double TURRET_SPEED_MULTIPLIER = 0.8; + public static final double TURRET_SPEED_MULTIPLIER = 0.4; public static final double TURRET_CALIBRATION_MULTIPLIER = 0.5; public static final double TURRET_DEGREES_PER_ROT = 180.0/105.45445251464844; public static final int TURRET_MOTOR_POS_AT_ZERO_ROT = 0; diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 57d4a4f..cf3b893 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -127,10 +127,6 @@ public class RobotContainer { public final Turret m_robotTurret = new Turret(m_robotMap.shooterTurret); public final VisionOdometry m_robotVisionOdometry = new VisionOdometry(m_robotSwerveDrive, m_robotTurret); - 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); - // Controllers private final static XboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); private final static XboxController m_operatorXbox = new DeadbandedXboxController(OIConstants.XBOX_OPERATOR_ID); @@ -171,79 +167,57 @@ public class RobotContainer { public RobotContainer() { configureButtonBindings(); /* Default Commands */ - - // IK command - // m_robotClimber.setDefaultCommand( - // new RunCommand(() -> m_robotClimber.setJointSpeeds(getOperatorController().getLeftX() * 10000, - // getOperatorController().getRightY() * 10000), m_robotClimber).withName("Climber controlWithInput defaultCommand")); - - // Swerve Drive with Input + // Swerve Drive with Input m_robotSwerveDrive.setDefaultCommand( new RunCommand(() -> { - // if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotSwerveDrive.driveWithInput( getDriverController().getLeftX(), getDriverController().getLeftY(), getDriverController().getRightX(), getDriverController().getRightY(), - true); //} - // if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { - // m_robotSwerveDrive.driveWithInput( 0, - // 0, - // 0, - // 0, - // true); - }//} + true); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { + m_robotSwerveDrive.driveWithInput( 0, + 0, + 0, + 0, + true); + }} , m_robotSwerveDrive).withName("Swerve driveWithInput defaultCommand")); - // Intake with Triggers + // Intake with Triggers m_robotIntake.setDefaultCommand( new RunCommand(() -> m_robotIntake.runWithTriggers( getOperatorController().getLeftTriggerAxis(), getOperatorController().getRightTriggerAxis()), m_robotIntake).withName("Intake runWithTriggers defaultCommand")); m_robotBoomBoom.setDefaultCommand(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - - // m_robotStorage.setDefaultCommand( - // new ManageStorage(m_robotStorage, - // m_robotBoomBoom, - // m_robotTurret).withName("Storage ManageStorage defaultCommand")); - // Storage Management - // m_robotStorage.setDefaultCommand( - // new RunCommand(() -> m_robotStorage.manageStorage(), - // m_robotStorage).withName("Storage manageStorage defaultCommand")); - - // Serializer Management + // Serializer Manual m_robotSerializer.setDefaultCommand( new RunCommand(() -> m_robotSerializer.setSerializer(getOperatorController().getLeftTriggerAxis() * 0.8),//m_robotSerializer.setSerializerStateWithBeam(), m_robotSerializer).withName("Serializer setSerializerStateWithBeam defaultCommand")); - // Turret Manual - // m_robotTurret.setDefaultCommand( - // new RunCommand(() -> m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()), - // m_robotTurret).withName("Turret runTurretWithInput defaultCommand")); + // Turret Manual m_robotTurret.setDefaultCommand( new RunCommand(() -> { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotTurret.runTurretWithInput(getOperatorController().getLeftX()); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotTurret.runTurretWithInput(0); } }, m_robotTurret)); + // Hood Manual m_robotHood.setDefaultCommand( new RunCommand(() -> { if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotHood.runHood(getOperatorController().getRightY()); } if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotHood.runHood(0); } }, m_robotHood)); + //Climber Manual m_robotClimber.setDefaultCommand( - new RunCommand(() -> m_robotClimber.setMotors(getOperatorController().getRightY() * 0.5) - , m_robotClimber)); - - // 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")); - + new RunCommand(() -> { + if (RobotContainer.currentControlMode.equals(ControlMode.SHOOTER)) { m_robotClimber.setMotors(0.0); } + if (RobotContainer.currentControlMode.equals(ControlMode.CLIMBER)) { m_robotClimber.setMotors(getOperatorController().getRightY()); } + }, m_robotClimber)); // autoInit(); // recordInit(); } @@ -256,138 +230,115 @@ public class RobotContainer { */ private void configureButtonBindings() { - /* Driver Buttons */ + //! Driver Buttons // Start > Calibrate Odometry new JoystickButton(getDriverController(), XboxController.Button.kBack.value) .whenPressed(() -> resetOdometry(new Pose2d(0, 0, new Rotation2d(0)))); - // Start > Calibrate Odometry + + // Start > Calibrate Odometry new JoystickButton(getDriverController(), XboxController.Button.kStart.value) - .whenPressed(m_robotSwerveDrive::resetGyro); + .whenPressed(m_robotSwerveDrive::resetGyro); + // Left Bumper > Shift Down new JoystickButton(getDriverController(), XboxController.Button.kLeftBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(false)); + // Right Bumper > Shift Up new JoystickButton(getDriverController(), XboxController.Button.kRightBumper.value) .whenPressed(() -> m_robotSwerveDrive.highSpeed(true)); - // new JoystickButton(getDriverController(), XboxController.Button.kA.value) - // .whileHeld(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, 0.2, true)) - // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - - // new JoystickButton(getDriverController(), XboxController.Button.kB.value) - // .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)) - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)); - - // new JoystickButton(getDriverController(), XboxController.Button.kY.value) - // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, false)); - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - // .whenReleased(() -> m_robotSwerveDrive.stopModules()); - - // .whenReleased(new InstantCommand(() -> m_robotTurret.runTurretWithInput(0.0), m_robotTurret)) - // .whenReleased(new InstantCommand(() -> m_robotHood.runHood(0.0), m_robotHood)) - // .whenReleased(new InstantCommand(() -> m_robotBoomBoom.runDrumShooter(0.0), m_robotBoomBoom)); - - /* Operator Buttons */ - + + + //! Operator Buttons + + // Right Bumper > Storage Out + new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // Left Bumper > Storage In + new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) + .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) + .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); + + // B > Toggle claws + new JoystickButton(getOperatorController(), XboxController.Button.kB.value) + .whenPressed(new RunCommand(() -> m_robotClaws.toggleClaws(), m_robotClaws)); + + // X > Toggles extender in and out + new JoystickButton(getOperatorController(), XboxController.Button.kX.value) + .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); + + // A > Spit Out Ball + new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + .whenPressed(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)) + .whileHeld(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))); + + // Y > Full aim command new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); + .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); + + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, false, false)); // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); + // .whileHeld(new TrackTarget(m_robotSwerveDrive, m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)); - new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - .whileHeld(new AimToCenter(m_robotTurret, m_robotVisionOdometry, m_robotSwerveDrive::getOdometry)); - - new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - .whileHeld(new Seek(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry)); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); + // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(true))); // new JoystickButton(getOperatorController(), XboxController.Button.kB.value) - // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); - - // new JoystickButton(getOperatorController(), XboxController.Button.kY.value) - // .whenPressed(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false), m_robotVisionOdometry)); + // .whileHeld(new RunCommand(() -> m_robotClaws.setOpen(false))); - new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - .whenPressed(new RunCommand(() -> m_robotStorage.runStorage(0.9), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood)); - new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-0.9), m_robotStorage)) - .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage)); + // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) + // .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) + // .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - //Toggles extender in and out - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whenPressed(new ExtenderIntakeGroup(m_robotIntake, m_robotExtender)); - // new JoystickButton(getOperatorController(), XboxController.Button.kX.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.gotoZero(), m_robotTurret)) - // .whenReleased(new RunCommand(() -> m_robotTurret.m_boomBoomRotateMotor.set(0), m_robotTurret)); - - // Right Bumper > Storage In - // new JoystickButton(getOperatorController(), XboxController.Button.kRightBumper.value) - // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED))) - // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // Left Bumper > Storage Out (note: necessary?) - // new JoystickButton(getOperatorController(), XboxController.Button.kLeftBumper.value) - // .whileHeld(new RunCommand(() -> m_robotStorage.runStorage(-StorageConstants.STORAGE_SPEED))) - // .whenReleased(new RunCommand(() -> m_robotStorage.runStorage(0.0))); - - // A > Shoot with Odo - /*new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - .whenPressed(new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood));*/ - - // new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new RunCommand(() -> m_robotTurret.gotoMidpoint(), m_robotTurret)); - - //B > Shoot with Lime - new JoystickButton(getOperatorController(), XboxController.Button.kA.value) - // .whileHeld(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry)) - // .whenReleased(new InstantCommand(() -> m_robotVisionOdometry.setLEDs(false))) - .whenPressed(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.25))) - .whenReleased(new RunCommand(() -> m_robotBoomBoom.runDrumShooter(0.0))); - // .whileHeld% - - /* Button Box Buttons */ + //! Button Box Buttons + // Left Switch > Disables soft limits on press, release resets encoders (all for turret, hood, climber, and extender) new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftSwitch.value) - .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) - .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(false), m_robotTurret)) + .whenPressed(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 0.3, m_robotTurret)) - .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) - .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(false), m_robotHood)) + .whenPressed(new InstantCommand(() -> m_robotHood.calibrationSpeed = 0.3, m_robotHood)) - .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) + .whenPressed(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(false), m_robotExtender)) - .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.setTurretSoftLimits(true), m_robotTurret)) + .whenReleased(new InstantCommand(() -> m_robotTurret.calibrationSpeed = 1.0, m_robotTurret)) - .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.setHoodSoftLimits(true), m_robotHood)) + .whenReleased(new InstantCommand(() -> m_robotHood.calibrationSpeed = 1.0, m_robotHood)) - .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) + .whenReleased(new InstantCommand(() -> m_robotExtender.setExtenderSoftLimits(true), m_robotExtender)) - .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(() -> 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)); + // Middle Switch > Climber and Shooter mode switching new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) - .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) - .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + // Left Button > Extender In new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + // Left Button > Extender Out new JoystickButton(getButtonBox(), ButtonBox.Button.kRightButton.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) - .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(1.0), m_robotExtender)) + .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } /** diff --git a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java index 806446d..5fe6a4e 100644 --- a/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java +++ b/src/main/java/frc4388/robot/commands/ShooterCommands/AimToCenter.java @@ -74,6 +74,7 @@ public class AimToCenter extends CommandBase { public static double angleToCenter(double x, double y, double gyro) { double angle = ((Math.atan2(y, x) * (180./Math.PI) - gyro) + 180. + (360. * 4)) % 360.; // Finds the angle between the gyro of the robot and the target (positive x is gyro 0) + // double angle = Math.toDegrees(Math.atan2(y, -x) - gyro); return (angle - 360); } diff --git a/src/main/java/frc4388/robot/subsystems/Claws.java b/src/main/java/frc4388/robot/subsystems/Claws.java index 2b811b0..f5fcc1b 100644 --- a/src/main/java/frc4388/robot/subsystems/Claws.java +++ b/src/main/java/frc4388/robot/subsystems/Claws.java @@ -32,140 +32,35 @@ public class Claws extends SubsystemBase { private boolean m_open; public static enum ClawType {LEFT, RIGHT} - public Claws(/*CANSparkMax*/Servo leftClaw, /*CANSparkMax*/Servo rightClaw) { + public Claws(Servo leftClaw, Servo rightClaw) { m_leftClaw = leftClaw; m_rightClaw = rightClaw; - - // m_leftLimitSwitchF = m_leftClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_rightLimitSwitchF = m_rightClaw.getForwardLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - // m_leftLimitSwitchR = m_leftClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyClosed); //Wired wrong lol - // m_rightLimitSwitchR = m_rightClaw.getReverseLimitSwitch(SparkMaxLimitSwitch.Type.kNormallyOpen); - - // m_leftLimitSwitchF.enableLimitSwitch(true); - // m_rightLimitSwitchF.enableLimitSwitch(true); - // m_leftLimitSwitchR.enableLimitSwitch(true); - // m_rightLimitSwitchR.enableLimitSwitch(true); - - // m_leftClaw.setInverted(true); - // m_rightClaw.setInverted(true); - m_open = false; - - // m_leftClaw.set(ClawConstants.CALIBRATION_SPEED); - // m_rightClaw.set(ClawConstants.CALIBRATION_SPEED); } - /** - * Run a specific claw to open or close. - * @param which Which claw to run. - * @param open Whether to open or close the claw. - */ - // public void runClaw(ClawType which, boolean open) { - - // int direction = open ? 1 : -1; - - // if (which == Claws.ClawType.LEFT) { - - // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_leftOffset : ClawsConstants.CLOSE_POSITION + m_leftOffset; - // // m_leftClaw.getEncoder().setPosition(setPos); - // m_leftClaw.setSpeed(direction * 0.1); - - // } else if (which == Claws.ClawType.RIGHT) { - - // // double setPos = open ? ClawsConstants.OPEN_POSITION + m_rightOffset : ClawsConstants.CLOSE_POSITION + m_rightOffset; - // // m_rightClaw.getEncoder().setPosition(setPos); - // m_rightClaw.setSpeed(direction * 0.1); - // } - - // m_open = open; - // } - - // public void runClaw(ClawType which, double input) { - // if (which == Claws.ClawType.LEFT) { - // m_leftClaw.setSpeed(input); - - // } else if (which == Claws.ClawType.RIGHT) { - // m_rightClaw.setSpeed(input); - // } - // } - - // public void runClaws(double input) - // { - // m_leftClaw.setSpeed(input); - // m_rightClaw.setSpeed(input); - // } - /** * Sets the state of both hooks * @param open The state of the hooks */ public void setOpen(boolean open) { - SmartDashboard.putBoolean("open", open); if(open) { - // m_leftClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_leftOffset); - // m_rightClaw.getEncoder().setPosition(ClawsConstants.OPEN_POSITION + m_rightOffset); - // m_leftClaw.setPosition(.7); - // m_rightClaw.setPosition(.7); - m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900);//ClawConstants.OPEN_POSITION); - m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100);//ClawConstants.OPEN_POSITION); + m_leftClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 900); + m_rightClaw.setRaw(ClawConstants.TOP_LIMIT + 100); + SmartDashboard.putBoolean("Claws Closed", false); } else { - // m_leftClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_leftOffset); - // m_rightClaw.getEncoder().setPosition(ClawsConstants.CLOSE_POSITION + m_rightOffset); - // m_leftClaw.setPosition(.3); - // m_rightClaw.setPosition(.3); - m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 100); - m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 700); + m_leftClaw.setRaw(ClawConstants.TOP_LIMIT - 400); + m_rightClaw.setRaw(ClawConstants.BOTTOM_LIMIT - 400); + SmartDashboard.putBoolean("Claws Closed", true); } } - // public double[] getOffsets() { - // return new double[] {m_leftOffset, m_rightOffset}; - // } + public void toggleClaws() { + m_open = !m_open; + setOpen(m_open); + } - // public boolean fullyOpen() { - // return Math.abs(m_leftClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD && - // Math.abs(m_rightClaw.getPosition() - ClawConstants.OPEN_POSITION) < ClawConstants.THRESHOLD; } - // public boolean fullyClosed() { - // return Math.abs(m_leftClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD && - // Math.abs(m_rightClaw.getPosition() - ClawConstants.CLOSE_POSITION) < ClawConstants.THRESHOLD; - // } - - /** - * Check if a limit switch is pressed or current limit exceeded for a claw. - * @param which Which claw to check. - * @param limit The current limit. - * @return Whether to interrupt the RunClaw command or not. - */ - // public boolean checkSwitchAndCurrent(ClawType which) { - // if (which == ClawType.LEFT) { - // if (m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed() || m_leftClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - // return true; - // } - // } - // else if (which == ClawType.RIGHT) { - // if (m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed() || m_rightClaw.getOutputCurrent() >= ClawConstants.CURRENT_LIMIT) { - // return true; - // } - // } - // return false; - // } - - // @Override + @Override public void periodic() { - // SmartDashboard.putNumber("Servo Left Pos", m_leftClaw.getRaw()); - // SmartDashboard.putNumber("Servo Right Pos", m_rightClaw.getRaw()); - // if (fullyOpen() || fullyClosed()) { - // m_leftClaw.setSpeed(0.0); - // m_rightClaw.setSpeed(0.0); - // } - - // if(m_leftLimitSwitchF.isPressed() || m_leftLimitSwitchR.isPressed()) - // // m_leftOffset = m_leftClaw.getEncoder().getPosition(); - // m_leftOffset = m_leftClaw.getPosition(); - - // if(m_rightLimitSwitchF.isPressed() || m_rightLimitSwitchR.isPressed()) - // // m_rightOffset = m_rightClaw.getEncoder().getPosition(); - // m_rightOffset = m_rightClaw.getPosition(); } } \ No newline at end of file