diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index c147692..4523d6e 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -129,7 +129,7 @@ public class Robot extends LoggedRobot { @Override public void teleopInit() { - m_robotContainer.stop(); + // m_robotContainer.stop(); // This makes sure that the autonomous stops running when // teleop starts running. If you want the autonomous to // continue until interrupted by another command, remove diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index 9a6b70e..d551676 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -23,11 +23,14 @@ import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.RunCommand; import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import edu.wpi.first.wpilibj2.command.WaitCommand; -import edu.wpi.first.wpilibj2.command.button.JoystickButton; -import edu.wpi.first.wpilibj2.command.button.Trigger; +// Autos +import frc4388.utility.controller.VirtualController; +import frc4388.utility.controller.XboxController; +import frc4388.utility.structs.LEDPatterns; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.constants.Constants; +import frc4388.robot.constants.Constants.AutoConstants; +import frc4388.robot.constants.Constants.LEDConstants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; import frc4388.robot.constants.FieldConstants; @@ -61,13 +64,10 @@ public class RobotContainer { public final RobotMap m_robotMap = new RobotMap(Mode.REAL); /* Subsystems */ - public final LED m_robotLED = new LED(); - //Testing of Colors + public final LED m_robotLED = new LED(LEDConstants.LED_SPARK_ID); public final Vision m_vision = new Vision(); - public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); - public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO); - public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED); - private Boolean operatorManualMode = false; + // public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED); + // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision); // public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); @@ -82,11 +82,11 @@ public class RobotContainer { // public List subsystems = new ArrayList<>(); // ! Teleop Commands - public void stop() { - new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); - m_robotSwerveDrive.stopModules(); - Constants.AutoConstants.Y_OFFSET_TRIM.set(0); - } + // public void stop() { + // new InstantCommand(()->{}, m_robotSwerveDrive).schedule(); + // m_robotSwerveDrive.stopModules(); + // Constants.AutoConstants.Y_OFFSET_TRIM.set(0); + // } // ! /* Autos */ private SendableChooser autoChooser; @@ -113,13 +113,10 @@ public class RobotContainer { configureButtonBindings(); - // Called on first robot enable - DeferredBlock.addBlock(() -> { - m_robotSwerveDrive.resetGyro(); - }, false); - - // Called on every robot enable - DeferredBlock.addBlock(() -> { + // DeferredBlock.addBlock(() -> { // Called on first robot enable + // m_robotSwerveDrive.resetGyro(); + // }, false); + DeferredBlock.addBlock(() -> { // Called on every robot enable TimesNegativeOne.update(); FieldPositions.update(); }, true); @@ -127,19 +124,27 @@ public class RobotContainer { DriverStation.silenceJoystickConnectionWarning(true); - // Drive normally - m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - m_robotSwerveDrive.driveWithInput( - getDeadbandedDriverController().getLeft(), - getDeadbandedDriverController().getRight(),true); + // m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> { - }, m_robotSwerveDrive) - .withName("SwerveDrive DefaultCommand")); - - m_robotSwerveDrive.setToSlow(); - - makeAutoChooser(); - SmartDashboard.putData("Auto Chooser", autoChooser); + // // IF the driver is holding the aim button, aim the robot towards the hub + // if(m_driverXbox.getRightTriggerAxis() > 0.5) { + // // Aim + // Translation2d shootTarget = new Translation2d(); + // // new Rotation2() + // Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle(); + // m_robotSwerveDrive.driveFieldAngle( + // getDeadbandedDriverController().getLeft(), + // ang); + // } else { + // // Drive normally + // m_robotSwerveDrive.driveWithInput( + // getDeadbandedDriverController().getLeft(), + // getDeadbandedDriverController().getRight(),true); + // } + + // }, m_robotSwerveDrive) + // .withName("SwerveDrive DefaultCommand")); + // m_robotSwerveDrive.setToSlow(); } @@ -158,9 +163,18 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro())); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new RotTo45(m_robotSwerveDrive)); + + new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.C1C2_GRADIENT))); + + new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.OCEAN_RAINBOW))); + + new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.RAINBOW_RAINBOW))); + + new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + .onTrue(new InstantCommand(() -> m_robotLED.setTo5V())); // new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode) // .onTrue(RobotShoot) @@ -173,86 +187,19 @@ public class RobotContainer { // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) // .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive)); - new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); + // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp())); - new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) - .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); - - - - // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Active);}, m_robotShooter)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotShooter.setMode(ShooterMode.Inactive);}, m_robotShooter)); - - // new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON) - // .onTrue(new InstantCommand(() -> {;}, m_robotIntake)); - // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON) - // .onTrue(new InstantCommand(() -> {m_robotIntake.setMode(IntakeMode.Retracted);}, m_robotIntake)); - - + // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown())); // new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON) // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode())); - new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) - .onTrue(new InstantCommand(() -> { - m_robotIntake.io.updateGains(); - m_robotShooter.io.updateGains(); - })); - - // IF the driver is holding the left trigger, intake driving - new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveIntake( - getDeadbandedDriverController().getLeft(), - false - ); - }, m_robotSwerveDrive)) - .onTrue(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Extended); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracted); - m_robotSwerveDrive.softStop(); - }, m_robotSwerveDrive)); - - // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready - new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5) - .whileTrue(new RunCommand( - () -> { - m_robotSwerveDrive.driveFacingPosition( - getDeadbandedDriverController().getLeft(), - FieldPositions.HUB_POSITION); - }, m_robotSwerveDrive) - ) - .onTrue(new InstantCommand(() -> { - // When Right trigger is pressed, - m_robotIntake.setMode(IntakeMode.Extended); - })) - .onFalse(new InstantCommand(() -> { - m_robotIntake.setMode(IntakeMode.Retracted); - m_robotSwerveDrive.softStop(); - }, m_robotSwerveDrive)); - - - // D-PAD fine alignment - new Trigger(() -> getDeadbandedDriverController().getPOV() != -1) - .whileTrue(new RunCommand( - () -> m_robotSwerveDrive.driveFine( - new Translation2d( - 1, - Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV()) - ), - getDeadbandedDriverController().getRight(), 0.15 - ), m_robotSwerveDrive)) - .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive)); - } - -//.onTrue(new InstantCommand(() -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT))); + // new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON) + // .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode())); + } /** * Use this to pass the autonomous command to the main {@link Robot} class. @@ -302,17 +249,17 @@ public class RobotContainer { } autoChooser.onChange((filename) -> { - autoChooserUpdated = true; - if (filename.equals("Taxi")) { - autoCommand = new SequentialCommandGroup( - new MoveForTimeCommand(m_robotSwerveDrive, - new Translation2d(0, -1), - new Translation2d(), 1000, true - ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); - } else { - autoCommand = new PathPlannerAuto(filename); - } - System.out.println("Robot Auto Changed " + filename); + // autoChooserUpdated = true; + // if (filename.equals("Taxi")) { + // autoCommand = new SequentialCommandGroup( + // new MoveForTimeCommand(m_robotSwerveDrive, + // new Translation2d(0, -1), + // new Translation2d(), 1000, true + // ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive)); + // } else { + // autoCommand = new PathPlannerAuto(filename); + // } + // System.out.println("Robot Auto Changed " + filename); }); SmartDashboard.putData(autoChooser); diff --git a/src/main/java/frc4388/robot/RobotMap.java b/src/main/java/frc4388/robot/RobotMap.java index 5067e69..e462071 100644 --- a/src/main/java/frc4388/robot/RobotMap.java +++ b/src/main/java/frc4388/robot/RobotMap.java @@ -51,7 +51,7 @@ public class RobotMap { // public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); /* Swreve Drive Subsystem */ - public final SwerveIO swerveDrivetrain; + // public final SwerveIO swerveDrivetrain; // /* Shooter and Intake Subsystem */ public final ShooterIO shooterIO; @@ -75,13 +75,13 @@ public class RobotMap { // reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL); // Configure swerve drive train - SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, - SwerveDriveConstants.DrivetrainConstants, - SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, - SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT - ); + // SwerveDrivetrain swerveDrivetrainReal = new SwerveDrivetrain (TalonFX::new, TalonFX::new, CANcoder::new, + // SwerveDriveConstants.DrivetrainConstants, + // SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT, + // SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT + // ); - swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); + // swerveDrivetrain = new SwerveReal(swerveDrivetrainReal); // Configure Shooter 22,23,24 TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS); @@ -103,7 +103,7 @@ public class RobotMap { intakeIO = new IntakeReal(arm, roller); // Fault - FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); + // FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro"); FaultTalonFX.addDevice(shooter1, "Shooter1"); @@ -112,18 +112,18 @@ public class RobotMap { FaultTalonFX.addDevice(arm, "Arm"); FaultTalonFX.addDevice(roller, "Roller"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); - FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); - FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); - FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); - FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); - FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer"); + // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer"); + // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer"); + // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive"); + // FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer"); + // FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder"); break; // case SIM: diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 9ff5234..6ee9e5e 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -96,7 +96,7 @@ public final class Constants { public static final class LEDConstants { public static final int LED_SPARK_ID = 9; - public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.FOREST_WAVES; + public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED_ORANGE; // LED color for when the intake is out public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 9edfb9d..6bb0ee9 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -10,6 +10,7 @@ package frc4388.robot.subsystems; import org.littletonrobotics.junction.AutoLogOutput; import edu.wpi.first.wpilibj.DriverStation; +import edu.wpi.first.wpilibj.PWM; import edu.wpi.first.wpilibj.motorcontrol.Spark; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants.LEDConstants; @@ -24,11 +25,19 @@ import frc4388.utility.structs.LEDPatterns; * Driver */ public class LED extends SubsystemBase implements Queryable { - public LED() { + private PWM m_pwm; + + public LED(int PWMport) { FaultReporter.register(this); + m_pwm = new PWM(PWMport); + + m_pwm.setBoundsMicroseconds(2003, 1550, 1500, 1460, 999); + m_pwm.setPeriodMultiplier(PWM.PeriodMultiplier.k1X); + m_pwm.setSpeed(0.0); + m_pwm.setZeroLatch(); + } - private static Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID); private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN; public void setMode(LEDPatterns pattern){ @@ -45,14 +54,27 @@ public class LED extends SubsystemBase implements Queryable { } public void update() { - if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return; - if(DriverStation.isDisabled()){ - LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue()); + m_pwm.setSpeed(LEDConstants.DEFAULT_PATTERN.getValue()); }else - LEDController.set(mode.getValue()); + m_pwm.setSpeed(mode.getValue()); } + // I freaking hate unmaintained code so much + // https://github.com/REVrobotics/Blinkin-Firmware/pull/12 + // Turns out the REV Blinkin firmware has an undocumented 'feature' + // that means that whenever a specific pulse length is randomly generated, + // the pulse has a chance of changing the selected strip of the Blinkin + // 2125 μs for 5v, 2145 μs for 12v + // Also check out: https://www.chiefdelphi.com/t/rev-blinkin-resetting-strip-mode-randomly/432510/12 + public void setTo5V() { + try { + m_pwm.setPulseTimeMicroseconds(2125); + Thread.sleep(10); + update(); + } catch (InterruptedException e) {} + } + @AutoLogOutput public String state() { return mode.getClass().toString(); @@ -67,8 +89,8 @@ public class LED extends SubsystemBase implements Queryable { public Status diagnosticStatus() { Status status = new Status(); - if(!LEDController.isAlive()) - status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); + // if(!LEDController.isAlive()) + // status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED"); status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());