mirror of
https://github.com/Team4388/2026KPopRobotHunters.git
synced 2026-06-09 08:48:05 -06:00
@@ -129,7 +129,7 @@ public class Robot extends LoggedRobot {
|
|||||||
|
|
||||||
@Override
|
@Override
|
||||||
public void teleopInit() {
|
public void teleopInit() {
|
||||||
m_robotContainer.stop();
|
// m_robotContainer.stop();
|
||||||
// This makes sure that the autonomous stops running when
|
// This makes sure that the autonomous stops running when
|
||||||
// teleop starts running. If you want the autonomous to
|
// teleop starts running. If you want the autonomous to
|
||||||
// continue until interrupted by another command, remove
|
// continue until interrupted by another command, remove
|
||||||
|
|||||||
@@ -23,11 +23,14 @@ import edu.wpi.first.wpilibj2.command.Command;
|
|||||||
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
import edu.wpi.first.wpilibj2.command.InstantCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.RunCommand;
|
import edu.wpi.first.wpilibj2.command.RunCommand;
|
||||||
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
|
||||||
import edu.wpi.first.wpilibj2.command.WaitCommand;
|
// Autos
|
||||||
import edu.wpi.first.wpilibj2.command.button.JoystickButton;
|
import frc4388.utility.controller.VirtualController;
|
||||||
import edu.wpi.first.wpilibj2.command.button.Trigger;
|
import frc4388.utility.controller.XboxController;
|
||||||
|
import frc4388.utility.structs.LEDPatterns;
|
||||||
import frc4388.robot.commands.MoveForTimeCommand;
|
import frc4388.robot.commands.MoveForTimeCommand;
|
||||||
import frc4388.robot.constants.Constants;
|
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.OIConstants;
|
||||||
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
import frc4388.robot.constants.Constants.SimConstants.Mode;
|
||||||
import frc4388.robot.constants.FieldConstants;
|
import frc4388.robot.constants.FieldConstants;
|
||||||
@@ -61,13 +64,10 @@ public class RobotContainer {
|
|||||||
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
|
||||||
|
|
||||||
/* Subsystems */
|
/* Subsystems */
|
||||||
public final LED m_robotLED = new LED();
|
public final LED m_robotLED = new LED(LEDConstants.LED_SPARK_ID);
|
||||||
//Testing of Colors
|
|
||||||
public final Vision m_vision = new Vision();
|
public final Vision m_vision = new Vision();
|
||||||
public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
// public final Elevator m_robotElevator = new Elevator(m_robotMap.elevatorIO, m_robotLED);
|
||||||
public final Intake m_robotIntake = new Intake(m_robotMap.intakeIO);
|
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
|
||||||
public final Shooter m_robotShooter = new Shooter(m_robotMap.shooterIO, m_robotSwerveDrive, m_robotIntake, m_robotLED);
|
|
||||||
private Boolean operatorManualMode = false;
|
|
||||||
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
// public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain);
|
||||||
|
|
||||||
// public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
|
// public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef");
|
||||||
@@ -82,11 +82,11 @@ public class RobotContainer {
|
|||||||
// public List<Subsystem> subsystems = new ArrayList<>();
|
// public List<Subsystem> subsystems = new ArrayList<>();
|
||||||
|
|
||||||
// ! Teleop Commands
|
// ! Teleop Commands
|
||||||
public void stop() {
|
// public void stop() {
|
||||||
new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
// new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
|
||||||
m_robotSwerveDrive.stopModules();
|
// m_robotSwerveDrive.stopModules();
|
||||||
Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
// Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
|
||||||
}
|
// }
|
||||||
|
|
||||||
// ! /* Autos */
|
// ! /* Autos */
|
||||||
private SendableChooser<String> autoChooser;
|
private SendableChooser<String> autoChooser;
|
||||||
@@ -113,13 +113,10 @@ public class RobotContainer {
|
|||||||
|
|
||||||
configureButtonBindings();
|
configureButtonBindings();
|
||||||
|
|
||||||
// Called on first robot enable
|
// DeferredBlock.addBlock(() -> { // Called on first robot enable
|
||||||
DeferredBlock.addBlock(() -> {
|
// m_robotSwerveDrive.resetGyro();
|
||||||
m_robotSwerveDrive.resetGyro();
|
// }, false);
|
||||||
}, false);
|
DeferredBlock.addBlock(() -> { // Called on every robot enable
|
||||||
|
|
||||||
// Called on every robot enable
|
|
||||||
DeferredBlock.addBlock(() -> {
|
|
||||||
TimesNegativeOne.update();
|
TimesNegativeOne.update();
|
||||||
FieldPositions.update();
|
FieldPositions.update();
|
||||||
}, true);
|
}, true);
|
||||||
@@ -127,19 +124,27 @@ public class RobotContainer {
|
|||||||
|
|
||||||
DriverStation.silenceJoystickConnectionWarning(true);
|
DriverStation.silenceJoystickConnectionWarning(true);
|
||||||
|
|
||||||
// Drive normally
|
// m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
||||||
m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
|
|
||||||
m_robotSwerveDrive.driveWithInput(
|
|
||||||
getDeadbandedDriverController().getLeft(),
|
|
||||||
getDeadbandedDriverController().getRight(),true);
|
|
||||||
|
|
||||||
}, m_robotSwerveDrive)
|
// // IF the driver is holding the aim button, aim the robot towards the hub
|
||||||
.withName("SwerveDrive DefaultCommand"));
|
// if(m_driverXbox.getRightTriggerAxis() > 0.5) {
|
||||||
|
// // Aim
|
||||||
m_robotSwerveDrive.setToSlow();
|
// Translation2d shootTarget = new Translation2d();
|
||||||
|
// // new Rotation2()
|
||||||
makeAutoChooser();
|
// Rotation2d ang = m_robotSwerveDrive.getPose2d().getTranslation().minus(shootTarget).getAngle();
|
||||||
SmartDashboard.putData("Auto Chooser", autoChooser);
|
// 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)
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
|
||||||
|
|
||||||
|
|
||||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
|
new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
|
||||||
// .onTrue(new RotTo45(m_robotSwerveDrive));
|
.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)
|
// new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.8 && !operatorManualMode)
|
||||||
// .onTrue(RobotShoot)
|
// .onTrue(RobotShoot)
|
||||||
@@ -173,86 +187,19 @@ public class RobotContainer {
|
|||||||
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
|
||||||
// .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
|
// .onTrue(new InstantCommand(() -> {m_robotSwerveDrive.softStop();}, m_robotSwerveDrive));
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
|
||||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftUp()));
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
|
||||||
.onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
|
// .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.START_BUTTON)
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.START_BUTTON)
|
||||||
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.activateLuigiMode()));
|
||||||
|
|
||||||
new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
|
||||||
.onTrue(new InstantCommand(() -> {
|
|
||||||
m_robotIntake.io.updateGains();
|
|
||||||
m_robotShooter.io.updateGains();
|
|
||||||
}));
|
|
||||||
|
|
||||||
|
// new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
|
||||||
// IF the driver is holding the left trigger, intake driving
|
// .onTrue(new InstantCommand(() -> m_robotSwerveDrive.deactivateLuigiMode()));
|
||||||
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)));
|
|
||||||
|
|
||||||
/**
|
/**
|
||||||
* Use this to pass the autonomous command to the main {@link Robot} class.
|
* Use this to pass the autonomous command to the main {@link Robot} class.
|
||||||
@@ -302,17 +249,17 @@ public class RobotContainer {
|
|||||||
}
|
}
|
||||||
|
|
||||||
autoChooser.onChange((filename) -> {
|
autoChooser.onChange((filename) -> {
|
||||||
autoChooserUpdated = true;
|
// autoChooserUpdated = true;
|
||||||
if (filename.equals("Taxi")) {
|
// if (filename.equals("Taxi")) {
|
||||||
autoCommand = new SequentialCommandGroup(
|
// autoCommand = new SequentialCommandGroup(
|
||||||
new MoveForTimeCommand(m_robotSwerveDrive,
|
// new MoveForTimeCommand(m_robotSwerveDrive,
|
||||||
new Translation2d(0, -1),
|
// new Translation2d(0, -1),
|
||||||
new Translation2d(), 1000, true
|
// new Translation2d(), 1000, true
|
||||||
), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
// ), new InstantCommand(()-> {m_robotSwerveDrive.softStop();} , m_robotSwerveDrive));
|
||||||
} else {
|
// } else {
|
||||||
autoCommand = new PathPlannerAuto(filename);
|
// autoCommand = new PathPlannerAuto(filename);
|
||||||
}
|
// }
|
||||||
System.out.println("Robot Auto Changed " + filename);
|
// System.out.println("Robot Auto Changed " + filename);
|
||||||
});
|
});
|
||||||
SmartDashboard.putData(autoChooser);
|
SmartDashboard.putData(autoChooser);
|
||||||
|
|
||||||
|
|||||||
@@ -51,7 +51,7 @@ public class RobotMap {
|
|||||||
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
// public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
|
||||||
|
|
||||||
/* Swreve Drive Subsystem */
|
/* Swreve Drive Subsystem */
|
||||||
public final SwerveIO swerveDrivetrain;
|
// public final SwerveIO swerveDrivetrain;
|
||||||
|
|
||||||
// /* Shooter and Intake Subsystem */
|
// /* Shooter and Intake Subsystem */
|
||||||
public final ShooterIO shooterIO;
|
public final ShooterIO shooterIO;
|
||||||
@@ -75,13 +75,13 @@ public class RobotMap {
|
|||||||
// reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
// reverseLidar = new LidarReal(LiDARConstants.REVERSE_LIDAR_DIO_CHANNEL);
|
||||||
|
|
||||||
// Configure swerve drive train
|
// Configure swerve drive train
|
||||||
SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
|
// SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDrivetrainReal = new SwerveDrivetrain<TalonFX, TalonFX, CANcoder> (TalonFX::new, TalonFX::new, CANcoder::new,
|
||||||
SwerveDriveConstants.DrivetrainConstants,
|
// SwerveDriveConstants.DrivetrainConstants,
|
||||||
SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
|
// SwerveDriveConstants.FRONT_LEFT, SwerveDriveConstants.FRONT_RIGHT,
|
||||||
SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
|
// SwerveDriveConstants.BACK_LEFT, SwerveDriveConstants.BACK_RIGHT
|
||||||
);
|
// );
|
||||||
|
|
||||||
swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
|
// swerveDrivetrain = new SwerveReal(swerveDrivetrainReal);
|
||||||
|
|
||||||
// Configure Shooter 22,23,24
|
// Configure Shooter 22,23,24
|
||||||
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS);
|
TalonFX shooter1 = new TalonFX(ShooterConstants.SHOOTER1_ID.id, Constants.CANIVORE_CANBUS);
|
||||||
@@ -103,7 +103,7 @@ public class RobotMap {
|
|||||||
intakeIO = new IntakeReal(arm, roller);
|
intakeIO = new IntakeReal(arm, roller);
|
||||||
|
|
||||||
// Fault
|
// Fault
|
||||||
FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
// FaultPidgeon2.addDevice(swerveDrivetrainReal.getPigeon2(), "Gyro");
|
||||||
|
|
||||||
|
|
||||||
FaultTalonFX.addDevice(shooter1, "Shooter1");
|
FaultTalonFX.addDevice(shooter1, "Shooter1");
|
||||||
@@ -112,18 +112,18 @@ public class RobotMap {
|
|||||||
FaultTalonFX.addDevice(arm, "Arm");
|
FaultTalonFX.addDevice(arm, "Arm");
|
||||||
FaultTalonFX.addDevice(roller, "Roller");
|
FaultTalonFX.addDevice(roller, "Roller");
|
||||||
|
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getDriveMotor(), "Module 0 Drive");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(0).getSteerMotor(), "Module 0 Steer");
|
||||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
|
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(0).getEncoder(), "Module 0 CANCoder");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getDriveMotor(), "Module 1 Drive");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(1).getSteerMotor(), "Module 1 Steer");
|
||||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
|
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(1).getEncoder(), "Module 1 CANCoder");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getDriveMotor(), "Module 2 Drive");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(2).getSteerMotor(), "Module 2 Steer");
|
||||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
|
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(2).getEncoder(), "Module 2 CANCoder");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getDriveMotor(), "Module 3 Drive");
|
||||||
FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
|
// FaultTalonFX.addDevice(swerveDrivetrainReal.getModule(3).getSteerMotor(), "Module 3 Steer");
|
||||||
FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
// FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
|
||||||
|
|
||||||
break;
|
break;
|
||||||
// case SIM:
|
// case SIM:
|
||||||
|
|||||||
@@ -96,7 +96,7 @@ public final class Constants {
|
|||||||
public static final class LEDConstants {
|
public static final class LEDConstants {
|
||||||
public static final int LED_SPARK_ID = 9;
|
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
|
// LED color for when the intake is out
|
||||||
public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
|
public static final LEDPatterns INTAKE_OUT = LEDPatterns.SOLID_RED;
|
||||||
|
|||||||
@@ -10,6 +10,7 @@ package frc4388.robot.subsystems;
|
|||||||
import org.littletonrobotics.junction.AutoLogOutput;
|
import org.littletonrobotics.junction.AutoLogOutput;
|
||||||
|
|
||||||
import edu.wpi.first.wpilibj.DriverStation;
|
import edu.wpi.first.wpilibj.DriverStation;
|
||||||
|
import edu.wpi.first.wpilibj.PWM;
|
||||||
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
import edu.wpi.first.wpilibj.motorcontrol.Spark;
|
||||||
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
import edu.wpi.first.wpilibj2.command.SubsystemBase;
|
||||||
import frc4388.robot.constants.Constants.LEDConstants;
|
import frc4388.robot.constants.Constants.LEDConstants;
|
||||||
@@ -24,11 +25,19 @@ import frc4388.utility.structs.LEDPatterns;
|
|||||||
* Driver
|
* Driver
|
||||||
*/
|
*/
|
||||||
public class LED extends SubsystemBase implements Queryable {
|
public class LED extends SubsystemBase implements Queryable {
|
||||||
public LED() {
|
private PWM m_pwm;
|
||||||
|
|
||||||
|
public LED(int PWMport) {
|
||||||
FaultReporter.register(this);
|
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;
|
private LEDPatterns mode = LEDConstants.DEFAULT_PATTERN;
|
||||||
|
|
||||||
public void setMode(LEDPatterns pattern){
|
public void setMode(LEDPatterns pattern){
|
||||||
@@ -45,14 +54,27 @@ public class LED extends SubsystemBase implements Queryable {
|
|||||||
}
|
}
|
||||||
|
|
||||||
public void update() {
|
public void update() {
|
||||||
if(!LEDController.isAlive() || LEDController.isSafetyEnabled()) return;
|
|
||||||
|
|
||||||
if(DriverStation.isDisabled()){
|
if(DriverStation.isDisabled()){
|
||||||
LEDController.set(LEDConstants.DEFAULT_PATTERN.getValue());
|
m_pwm.setSpeed(LEDConstants.DEFAULT_PATTERN.getValue());
|
||||||
}else
|
}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
|
@AutoLogOutput
|
||||||
public String state() {
|
public String state() {
|
||||||
return mode.getClass().toString();
|
return mode.getClass().toString();
|
||||||
@@ -67,8 +89,8 @@ public class LED extends SubsystemBase implements Queryable {
|
|||||||
public Status diagnosticStatus() {
|
public Status diagnosticStatus() {
|
||||||
Status status = new Status();
|
Status status = new Status();
|
||||||
|
|
||||||
if(!LEDController.isAlive())
|
// if(!LEDController.isAlive())
|
||||||
status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
|
// status.addReport(ReportLevel.ERROR, "LED is DISCONNECTED");
|
||||||
|
|
||||||
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
|
status.addReport(ReportLevel.INFO, "LED Mode: " + mode.name());
|
||||||
|
|
||||||
|
|||||||
Reference in New Issue
Block a user