diff --git a/src/main/java/frc4388/robot/Robot.java b/src/main/java/frc4388/robot/Robot.java index 4523d6e..de52ec1 100644 --- a/src/main/java/frc4388/robot/Robot.java +++ b/src/main/java/frc4388/robot/Robot.java @@ -15,7 +15,6 @@ import org.littletonrobotics.junction.networktables.NT4Publisher; import org.littletonrobotics.junction.wpilog.WPILOGReader; import org.littletonrobotics.junction.wpilog.WPILOGWriter; -import edu.wpi.first.wpilibj.RobotController; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.CommandScheduler; import frc4388.robot.constants.BuildConstants; @@ -70,9 +69,6 @@ public class Robot extends LoggedRobot { @Override public void robotPeriodic() { m_robotTime.updateTimes(); - // SmartDashboard.putNumber("Time", System.currentTimeMillis()); - - m_robotContainer.m_robotLED.update(); // Runs the Scheduler. This is responsible for polling buttons, adding newly-scheduled // commands, running already-scheduled commands, removing finished or interrupted commands, // and running subsystem periodic() methods. This must be called from the robot's periodic diff --git a/src/main/java/frc4388/robot/RobotContainer.java b/src/main/java/frc4388/robot/RobotContainer.java index c06e760..0b378cd 100644 --- a/src/main/java/frc4388/robot/RobotContainer.java +++ b/src/main/java/frc4388/robot/RobotContainer.java @@ -9,7 +9,6 @@ package frc4388.robot; import java.io.File; -import com.ctre.phoenix.CANifier.LEDChannel; import com.pathplanner.lib.commands.PathPlannerAuto; import edu.wpi.first.math.geometry.Rotation2d; @@ -24,20 +23,17 @@ 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; import frc4388.robot.commands.MoveForTimeCommand; import frc4388.robot.constants.Constants; import frc4388.robot.constants.Constants.OIConstants; import frc4388.robot.constants.Constants.SimConstants.Mode; -import frc4388.robot.constants.FieldConstants; // Subsystems import frc4388.robot.subsystems.LED; import frc4388.robot.subsystems.intake.Intake; import frc4388.robot.subsystems.intake.Intake.IntakeMode; import frc4388.robot.subsystems.shooter.Shooter; -import frc4388.robot.subsystems.shooter.Shooter.ShooterMode; import frc4388.robot.subsystems.swerve.SwerveDrive; import frc4388.robot.subsystems.vision.Vision; import frc4388.utility.DeferredBlock; @@ -47,7 +43,6 @@ import frc4388.utility.controller.DeadbandedXboxController; // Autos import frc4388.utility.controller.VirtualController; import frc4388.utility.controller.XboxController; -import frc4388.utility.structs.LEDPatterns; /** * This class is where the bulk of the robot should be declared. Since @@ -68,12 +63,7 @@ public class RobotContainer { 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 SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain); - - // public final LiDAR reefLidar = new LiDAR(m_robotMap.reefLidar, "Reef"); - // public final LiDAR reverseLidar = new LiDAR(m_robotMap.reverseLidar, "Reverse"); - + /* Controllers */ private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID); diff --git a/src/main/java/frc4388/robot/constants/Constants.java b/src/main/java/frc4388/robot/constants/Constants.java index 6ee9e5e..efae75f 100644 --- a/src/main/java/frc4388/robot/constants/Constants.java +++ b/src/main/java/frc4388/robot/constants/Constants.java @@ -9,8 +9,6 @@ package frc4388.robot.constants; import com.ctre.phoenix6.CANBus; -import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.Matrix; import edu.wpi.first.math.VecBuilder; import edu.wpi.first.math.geometry.Rotation3d; diff --git a/src/main/java/frc4388/robot/constants/FieldConstants.java b/src/main/java/frc4388/robot/constants/FieldConstants.java index 9fa9146..68683f6 100644 --- a/src/main/java/frc4388/robot/constants/FieldConstants.java +++ b/src/main/java/frc4388/robot/constants/FieldConstants.java @@ -4,7 +4,6 @@ import java.util.Arrays; import edu.wpi.first.apriltag.AprilTag; import edu.wpi.first.apriltag.AprilTagFieldLayout; -import edu.wpi.first.apriltag.AprilTagFields; import edu.wpi.first.math.geometry.Pose3d; import edu.wpi.first.math.geometry.Rotation3d; import edu.wpi.first.math.geometry.Translation2d; diff --git a/src/main/java/frc4388/robot/subsystems/LED.java b/src/main/java/frc4388/robot/subsystems/LED.java index 71e728c..7fd7859 100644 --- a/src/main/java/frc4388/robot/subsystems/LED.java +++ b/src/main/java/frc4388/robot/subsystems/LED.java @@ -7,11 +7,12 @@ package frc4388.robot.subsystems; +import java.util.concurrent.TimeUnit; + 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; import frc4388.utility.status.Status; @@ -43,20 +44,15 @@ public class LED extends SubsystemBase implements Queryable { public void setMode(LEDPatterns pattern){ // Don't stall the main thread every time the setMode function is called if(this.mode != pattern) { + this.mode = pattern; setTo5V(); } - this.mode = pattern; } public String getMode(){ return mode.name(); } - @Override - public void periodic() { - update(); - } - public void update() { if(DriverStation.isDisabled()){ m_pwm.setSpeed(LEDConstants.DEFAULT_PATTERN.getValue()); @@ -74,7 +70,7 @@ public class LED extends SubsystemBase implements Queryable { public void setTo5V() { try { m_pwm.setPulseTimeMicroseconds(2125); - Thread.sleep(1); + TimeUnit.MICROSECONDS.sleep(2125*2);// Wait long enough for one pulse to go through update(); } catch (InterruptedException e) {} } diff --git a/src/main/java/frc4388/robot/subsystems/TestRobot.java b/src/main/java/frc4388/robot/subsystems/TestRobot.java index c435c43..4bf3dba 100644 --- a/src/main/java/frc4388/robot/subsystems/TestRobot.java +++ b/src/main/java/frc4388/robot/subsystems/TestRobot.java @@ -11,7 +11,6 @@ import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.wpilibj.motorcontrol.Talon; import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.utility.configurable.ConfigurableDouble; diff --git a/src/main/java/frc4388/robot/subsystems/climber/Climber.java b/src/main/java/frc4388/robot/subsystems/climber/Climber.java index 78c06eb..3ddb883 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc4388/robot/subsystems/climber/Climber.java @@ -1,11 +1,7 @@ package frc4388.robot.subsystems.climber; -import java.util.function.Supplier; - import org.littletonrobotics.junction.Logger; -import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase { diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java index c0b5329..131ea38 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberConstants.java @@ -1,20 +1,13 @@ package frc4388.robot.subsystems.climber; -import static edu.wpi.first.units.Units.Degrees; import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Distance; -import frc4388.utility.status.CanDevice; public class ClimberConstants { // Motor conversions diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java index bddb5fc..f401d05 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberIO.java @@ -1,19 +1,12 @@ package frc4388.robot.subsystems.climber; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; import static edu.wpi.first.units.Units.Inches; import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.units.CurrentUnit; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; import edu.wpi.first.units.measure.Distance; -import edu.wpi.first.units.measure.LinearVelocity; public interface ClimberIO { @AutoLog diff --git a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java index 2345a08..9069754 100644 --- a/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java +++ b/src/main/java/frc4388/robot/subsystems/climber/ClimberReal.java @@ -1,20 +1,11 @@ package frc4388.robot.subsystems.climber; import static edu.wpi.first.units.Units.Inches; -import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionDutyCycle; -import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.robot.subsystems.intake.IntakeConstants; -import frc4388.utility.configurable.ConfigurableDouble; public class ClimberReal implements ClimberIO { diff --git a/src/main/java/frc4388/robot/subsystems/intake/Intake.java b/src/main/java/frc4388/robot/subsystems/intake/Intake.java index be507b3..af919f0 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/Intake.java +++ b/src/main/java/frc4388/robot/subsystems/intake/Intake.java @@ -1,18 +1,13 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Rotation; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecondPerSecond; - import java.util.function.Supplier; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.status.FaultReporter; public class Intake extends SubsystemBase { public IntakeIO io; diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java index f2c3490..621f86b 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeConstants.java @@ -1,17 +1,11 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; -import com.ctre.phoenix6.configs.Slot1Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java index b97ad76..5e8f8b9 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeIO.java @@ -1,17 +1,14 @@ package frc4388.robot.subsystems.intake; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.InchesPerSecond; import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.units.CurrentUnit; import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; public interface IntakeIO { @AutoLog diff --git a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java index 547cc90..c94b0d9 100644 --- a/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java +++ b/src/main/java/frc4388/robot/subsystems/intake/IntakeReal.java @@ -1,18 +1,10 @@ package frc4388.robot.subsystems.intake; -import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; -import static edu.wpi.first.units.Units.RotationsPerSecond; - -import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.controls.PositionDutyCycle; import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; import edu.wpi.first.units.measure.*; -import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard; -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import frc4388.utility.configurable.ConfigurableDouble; public class IntakeReal implements IntakeIO { diff --git a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java index 44c9b2c..ed71e49 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/Shooter.java @@ -2,13 +2,10 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.RotationsPerSecond; -import java.text.FieldPosition; - import org.littletonrobotics.junction.AutoLogOutput; import org.littletonrobotics.junction.Logger; import edu.wpi.first.math.geometry.Pose2d; -import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.kinematics.ChassisSpeeds; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc4388.robot.constants.Constants; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java index 108ef6e..31ce6c6 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java @@ -1,16 +1,11 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.Degrees; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.configs.Slot0Configs; import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.signals.NeutralModeValue; -import edu.wpi.first.units.measure.Angle; -import edu.wpi.first.units.measure.AngularVelocity; import frc4388.utility.configurable.ConfigurableDouble; import frc4388.utility.status.CanDevice; diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java index 2e6402b..efd97b0 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java @@ -1,17 +1,12 @@ package frc4388.robot.subsystems.shooter; import static edu.wpi.first.units.Units.Amps; -import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.Rotations; import static edu.wpi.first.units.Units.RotationsPerSecond; import org.littletonrobotics.junction.AutoLog; -import edu.wpi.first.units.CurrentUnit; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; import edu.wpi.first.units.measure.Current; -import edu.wpi.first.units.measure.LinearVelocity; public interface ShooterIO { @AutoLog diff --git a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java index 34dc2df..d07ba5c 100644 --- a/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java +++ b/src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java @@ -1,15 +1,9 @@ package frc4388.robot.subsystems.shooter; -import static edu.wpi.first.units.Units.InchesPerSecond; -import static edu.wpi.first.units.Units.RotationsPerSecond; - import com.ctre.phoenix6.controls.VelocityDutyCycle; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.units.measure.Angle; import edu.wpi.first.units.measure.AngularVelocity; -import frc4388.robot.subsystems.intake.IntakeConstants; -import frc4388.utility.configurable.ConfigurableDouble; public class ShooterReal implements ShooterIO { diff --git a/src/main/java/frc4388/utility/status/Alerts.java b/src/main/java/frc4388/utility/status/Alerts.java index 4065586..351dda9 100644 --- a/src/main/java/frc4388/utility/status/Alerts.java +++ b/src/main/java/frc4388/utility/status/Alerts.java @@ -2,7 +2,6 @@ package frc4388.utility.status; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; -import frc4388.robot.RobotContainer; // Class to update a series of WPILIB Alerts public class Alerts { diff --git a/src/main/java/frc4388/utility/status/FaultReporter.java b/src/main/java/frc4388/utility/status/FaultReporter.java index f4066f8..732bef3 100644 --- a/src/main/java/frc4388/utility/status/FaultReporter.java +++ b/src/main/java/frc4388/utility/status/FaultReporter.java @@ -3,7 +3,6 @@ package frc4388.utility.status; import java.util.ArrayList; import java.util.List; -import com.ctre.phoenix6.CANBus; import com.ctre.phoenix6.CANBus.CANBusStatus; import frc4388.robot.constants.Constants;