diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 75a2675..72b7c4b 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -181,7 +181,7 @@ public class Robot extends TimedRobot { LOGGER.log(Level.SEVERE, "Unable to record path to {0}", outputFile.getPath()); } - m_robotContainer.m_robotVisionOdometry.setLEDs(false); + m_robotContainer.m_robotVisionOdometry.setLEDs(true); } @Override diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 826ff61..45e523c 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -302,15 +302,22 @@ public class RobotContainer { new JoystickButton(getButtonBox(), ButtonBox.Button.kMiddleSwitch.value) .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whenPressed(new InstantCommand(() -> this.currentControlMode = ControlMode.CLIMBER)) + .whenReleased(new InstantCommand(() -> this.currentControlMode = ControlMode.SHOOTER)); + + new JoystickButton(getButtonBox(), ButtonBox.Button.kRightSwitch.value) + .whileHeld(new InstantCommand(() -> m_robotExtender.invertExtender(-1.0))) + .whenReleased(new InstantCommand(() -> m_robotExtender.invertExtender(1.0))); // Left Button > Extender In new JoystickButton(getButtonBox(), ButtonBox.Button.kLeftButton.value) - .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.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)) + .whileHeld(new RunCommand(() -> m_robotExtender.runExtender(-1.0), m_robotExtender)) .whenReleased(new RunCommand(() -> m_robotExtender.runExtender(0.0), m_robotExtender)); } @@ -398,7 +405,7 @@ public class RobotContainer { )); // * weird way of shooting, i think we should make a new TrackTarget with built-in Storage control instead. // ! THREE BALL AUTO (HOPEFULLY) - return new SequentialCommandGroup( new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), // * aim with turret to target + return new SequentialCommandGroup( new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2((219.25 / 2.00) - turretDistanceFromFront, (82.83 / 2.00) - 15.56)), m_robotTurret), 1.0, true), // * aim with turret to target weirdAutoShootingGroup, // * shoot new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage @@ -409,15 +416,17 @@ public class RobotContainer { new SequentialCommandGroup( new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive), // * reset gyro before moving new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 1.0, 0.0, 0.0}, (40.44 - offset) / distancePerSecond), // * drive to first ball - new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363) - - new InstantCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), // * aim with turret to target + //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, -1.0, 0.0, 0.0, true)), // * brake (see line 363) + new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), + new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-Math.atan2(firstBallPosition.y, firstBallPosition.x)), m_robotTurret), 1.0, true), // * aim with turret to target weirdAutoShootingGroup, // * shoot new InstantCommand(() -> m_robotStorage.runStorage(0.0), m_robotStorage), // * stop running storage - new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball + //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y, true), m_robotSwerveDrive), // * rotate so intake points towards second ball + new DriveWithInputForTime(m_robotSwerveDrive, new double[] {0.0, 0.0, -firstToSecond.unit().x, -firstToSecond.unit().y}, 0.5d), new DriveWithInputForTime(m_robotSwerveDrive, new double[] {-firstToSecond.unit().x, -firstToSecond.unit().y, 0.0, 0.0}, (firstToSecond.magnitude() - offset) / distancePerSecond), // * drive to second ball - new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363) + //new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(firstToSecond.unit().x, firstToSecond.unit().y, 0.0, 0.0, true)), // * brake (see line 363) + new InstantCommand(() -> m_robotSwerveDrive.stopModules(), m_robotSwerveDrive), new Shoot(m_robotSwerveDrive, m_robotBoomBoom, m_robotTurret, m_robotHood, m_robotVisionOdometry, secondBallPosition.toDoubleArray()), // * aim to target weirdAutoShootingGroup, // * shoot diff --git a/src/main/java/frc4388/robot/subsystems/Climber.java b/src/main/java/frc4388/robot/subsystems/Climber.java index 0b589ac..cf6c633 100644 --- a/src/main/java/frc4388/robot/subsystems/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/Climber.java @@ -18,7 +18,7 @@ public class Climber extends SubsystemBase { /** Creates a new Climber */ public Climber(WPI_TalonFX elbow) { this.elbow = elbow; - elbow.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + elbow.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); elbow.overrideLimitSwitchesEnable(true); //Check elbow.configReverseSoftLimitThreshold(ClimberConstants.ELBOW_FORWARD_SOFT_LIMIT); // Tune @@ -39,6 +39,7 @@ public class Climber extends SubsystemBase { @Override public void periodic() { - SmartDashboard.putBoolean("Climber Folded", (elbow.isFwdLimitSwitchClosed() == 1)); + SmartDashboard.putNumber("Elbow Angle", elbow.getSelectedSensorPosition()); + SmartDashboard.putBoolean("Climber Folded", (elbow.isRevLimitSwitchClosed() == 1)); } } \ No newline at end of file diff --git a/src/main/java/frc4388/robot/subsystems/Extender.java b/src/main/java/frc4388/robot/subsystems/Extender.java index 001208f..da30563 100644 --- a/src/main/java/frc4388/robot/subsystems/Extender.java +++ b/src/main/java/frc4388/robot/subsystems/Extender.java @@ -16,6 +16,7 @@ import frc4388.robot.Constants.ExtenderConstants; public class Extender extends SubsystemBase { private CANSparkMax m_extenderMotor; + private double m_invert = 1; // private SparkMaxLimitSwitch m_inLimit; // private SparkMaxLimitSwitch m_outLimit; @@ -44,6 +45,10 @@ public class Extender extends SubsystemBase { m_extenderMotor.enableSoftLimit(SoftLimitDirection.kReverse, set); } + public void invertExtender(double invert){ + m_invert = invert; + } + @Override public void periodic() { // This method will be called once per scheduler run @@ -52,7 +57,7 @@ public class Extender extends SubsystemBase { public void runExtender(double input) { // if (!m_serializer.getBeam() && input < 0.) return; - m_extenderMotor.set(input); + m_extenderMotor.set(input * m_invert); } public double getPosition() {