diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java index a11dc8e..ab95d51 100644 --- a/src/main/java/frc/robot/Robot.java +++ b/src/main/java/frc/robot/Robot.java @@ -73,7 +73,9 @@ public void robotInit() { */ @Override public void robotPeriodic() { - if (this.isTest()) CommandScheduler.getInstance().run(); + if (this.isTest()) + return; + CommandScheduler.getInstance().run(); } /** @@ -98,9 +100,8 @@ public void autonomousInit() { autonomousCommand = robotContainer.getAutonomousCommand(); // schedule the autonomous command (example) - if (autonomousCommand != null) { + if (autonomousCommand != null) autonomousCommand.schedule(); - } } /** diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 9e5b8e5..141cf80 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -15,7 +15,6 @@ import frc.robot.commands.DriveCommands; import frc.robot.subsystems.drive.*; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; -import org.littletonrobotics.junction.networktables.LoggedDashboardNumber; /** * This class is where the bulk of the robot should be declared. Since Command-based is a @@ -32,8 +31,6 @@ public class RobotContainer { // Dashboard inputs private final LoggedDashboardChooser autoChooser; - private final LoggedDashboardNumber flywheelSpeedInput = - new LoggedDashboardNumber("Flywheel Speed", 1500.0); /** * The container for the robot. Contains subsystems, OI devices, and commands. diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java index da069fc..d2fe898 100644 --- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java +++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java @@ -10,6 +10,7 @@ import com.ctre.phoenix6.hardware.Pigeon2; import edu.wpi.first.math.geometry.Rotation2d; import edu.wpi.first.math.util.Units; +import frc.robot.Constants; import java.util.OptionalDouble; import java.util.Queue; @@ -18,7 +19,7 @@ * IO implementation for Pigeon2 */ public class GyroIOPigeon2 implements GyroIO { - private final Pigeon2 pigeon = new Pigeon2(0, "ChassisCanivore"); + private final Pigeon2 pigeon = new Pigeon2(0, Constants.ChassisConfigs.DEFAULT_CHASSIS_CANIVORE); private final StatusSignal yaw = pigeon.getYaw(); private final Queue yawPositionQueue; private final Queue yawTimestampQueue; @@ -45,7 +46,7 @@ public GyroIOPigeon2(boolean phoenixDrive) { @Override public void updateInputs(GyroIOInputs inputs) { - inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).equals(StatusCode.OK); + inputs.connected = BaseStatusSignal.refreshAll(yaw, yawVelocity).isOK(); inputs.yawPosition = Rotation2d.fromDegrees(yaw.getValueAsDouble()); inputs.yawVelocityRadPerSec = Units.degreesToRadians(yawVelocity.getValueAsDouble()); diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java index 3c47f93..0144b54 100644 --- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java +++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java @@ -106,16 +106,14 @@ public ModuleIOTalonFX(int index) { timestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue(); drivePosition = driveTalon.getPosition(); - drivePositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); + drivePositionQueue = PhoenixOdometryThread.getInstance().registerSignal(driveTalon, driveTalon.getPosition()); driveVelocity = driveTalon.getVelocity(); driveAppliedVolts = driveTalon.getMotorVoltage(); driveCurrent = driveTalon.getSupplyCurrent(); turnAbsolutePosition = cancoder.getAbsolutePosition(); turnPosition = turnTalon.getPosition(); - turnPositionQueue = - PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); + turnPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(turnTalon, turnTalon.getPosition()); turnVelocity = turnTalon.getVelocity(); turnAppliedVolts = turnTalon.getMotorVoltage(); turnCurrent = turnTalon.getSupplyCurrent(); @@ -148,33 +146,25 @@ public void updateInputs(ModuleIOInputs inputs) { turnAppliedVolts, turnCurrent); - inputs.drivePositionRad = - Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; - inputs.driveVelocityRadPerSec = - Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble()) / DRIVE_GEAR_RATIO; + inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / DRIVE_GEAR_RATIO; inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble(); inputs.driveCurrentAmps = new double[]{driveCurrent.getValueAsDouble()}; - inputs.turnAbsolutePosition = - Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) - .minus(absoluteEncoderOffset); - inputs.turnPosition = - Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); - inputs.turnVelocityRadPerSec = - Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; + inputs.turnAbsolutePosition = Rotation2d.fromRotations(turnAbsolutePosition.getValueAsDouble()) + .minus(absoluteEncoderOffset); + inputs.turnPosition = Rotation2d.fromRotations(turnPosition.getValueAsDouble() / TURN_GEAR_RATIO); + inputs.turnVelocityRadPerSec = Units.rotationsToRadians(turnVelocity.getValueAsDouble()) / TURN_GEAR_RATIO; inputs.turnAppliedVolts = turnAppliedVolts.getValueAsDouble(); inputs.turnCurrentAmps = new double[]{turnCurrent.getValueAsDouble()}; - inputs.odometryTimestamps = - timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); - inputs.odometryDrivePositionsRad = - drivePositionQueue.stream() - .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) - .toArray(); - inputs.odometryTurnPositions = - turnPositionQueue.stream() - .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) - .toArray(Rotation2d[]::new); + inputs.odometryTimestamps = timestampQueue.stream().mapToDouble((Double value) -> value).toArray(); + inputs.odometryDrivePositionsRad = drivePositionQueue.stream() + .mapToDouble((Double value) -> Units.rotationsToRadians(value) / DRIVE_GEAR_RATIO) + .toArray(); + inputs.odometryTurnPositions = turnPositionQueue.stream() + .map((Double value) -> Rotation2d.fromRotations(value / TURN_GEAR_RATIO)) + .toArray(Rotation2d[]::new); timestampQueue.clear(); drivePositionQueue.clear(); turnPositionQueue.clear(); @@ -201,10 +191,7 @@ public void setDriveBrakeMode(boolean enable) { @Override public void setTurnBrakeMode(boolean enable) { var config = new MotorOutputConfigs(); - config.Inverted = - isTurnMotorInverted - ? InvertedValue.Clockwise_Positive - : InvertedValue.CounterClockwise_Positive; + config.Inverted = isTurnMotorInverted ? InvertedValue.Clockwise_Positive : InvertedValue.CounterClockwise_Positive; config.NeutralMode = enable ? NeutralModeValue.Brake : NeutralModeValue.Coast; turnTalon.getConfigurator().apply(config); }