From 5c4b4e184d0ac5bfa236f98d311d924e2672da5c Mon Sep 17 00:00:00 2001 From: Gavin P Date: Fri, 17 Jan 2025 22:01:48 -0800 Subject: [PATCH] Code cleanup --- src/main/java/frc/robot/RobotContainer.java | 11 +++----- .../java/frc/robot/commands/DriveForward.java | 5 +--- .../frc/robot/constants/CORALOUTTAKE.java | 4 +-- src/main/java/frc/robot/constants/SWERVE.java | 2 ++ src/main/java/frc/robot/constants/USB.java | 2 +- .../java/frc/robot/subsystems/Controls.java | 1 - .../frc/robot/subsystems/CoralOuttake.java | 11 ++++++-- .../java/frc/robot/subsystems/Elevator.java | 25 ------------------- src/main/java/frc/robot/utils/CtreUtils.java | 4 +-- 9 files changed, 19 insertions(+), 46 deletions(-) diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 4d49545..ae5d321 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -75,7 +75,6 @@ public RobotContainer() { // Configure the trigger bindings configureBindings(); - initSmartDashboard(); } private void initializeSubSystems() { @@ -168,13 +167,9 @@ private void initSysidChooser() { * joysticks}. */ private void configureBindings() { - // // Schedule `ExampleCommand` when `exampleCondition` changes to `true` - // new Trigger(m_exampleSubsystem::exampleCondition) - // .onTrue(new ExampleCommand(m_exampleSubsystem)); - // - // // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, - // // cancelling on release. - // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); + // Schedule `exampleMethodCommand` when the Xbox controller's B button is pressed, + // cancelling on release. + // m_driverController.b().whileTrue(m_exampleSubsystem.exampleMethodCommand()); m_driverController.leftBumper().whileTrue(new RunCoralOuttake(m_coralOuttake, 0.15)); // outtake m_driverController .rightBumper() diff --git a/src/main/java/frc/robot/commands/DriveForward.java b/src/main/java/frc/robot/commands/DriveForward.java index 103ca7d..73cf85b 100644 --- a/src/main/java/frc/robot/commands/DriveForward.java +++ b/src/main/java/frc/robot/commands/DriveForward.java @@ -14,12 +14,9 @@ import edu.wpi.first.wpilibj2.command.WaitCommand; import frc.robot.subsystems.CommandSwerveDrivetrain; -// NOTE: Consider using this command inline, rather than writing a subclass. For more -// information, see: -// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html public class DriveForward extends SequentialCommandGroup { /** Creates a new DriveForward. */ - public DriveForward(CommandSwerveDrivetrain swerveDrive) { + public DriveForward(CommandSwerveDrivetrain swerveDrive /* TODO: Add field sim */) { try { PathPlannerPath path = PathPlannerPath.fromPathFile("DriveForward"); diff --git a/src/main/java/frc/robot/constants/CORALOUTTAKE.java b/src/main/java/frc/robot/constants/CORALOUTTAKE.java index 0718b69..c00624d 100644 --- a/src/main/java/frc/robot/constants/CORALOUTTAKE.java +++ b/src/main/java/frc/robot/constants/CORALOUTTAKE.java @@ -7,6 +7,6 @@ public class CORALOUTTAKE { public static final double kI = 0.0; public static final double kD = 0.0; public static final double gearRatio = 1.0; - public static final double Inertia = 0.001; - public static final DCMotor Gearbox = DCMotor.getKrakenX60(1); + public static final double inertia = 0.001; + public static final DCMotor gearbox = DCMotor.getKrakenX60(1); } diff --git a/src/main/java/frc/robot/constants/SWERVE.java b/src/main/java/frc/robot/constants/SWERVE.java index ceb63f9..c15f340 100644 --- a/src/main/java/frc/robot/constants/SWERVE.java +++ b/src/main/java/frc/robot/constants/SWERVE.java @@ -3,6 +3,7 @@ import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.util.Units; +// TODO: most of these values have been moved to delete or update public class SWERVE { public static double kTrackWidth = Units.inchesToMeters(21); public static final double kWheelBase = Units.inchesToMeters(19); @@ -24,6 +25,7 @@ public class SWERVE { // public static final SwerveDriveKinematics kSwerveKinematics = // new SwerveDriveKinematics( // ModuleMap.orderedValues(kModuleTranslations, new Translation2d[0])); + public static final Translation2d kFrontLeftPosition = new Translation2d(kWheelBase / 2.0, kTrackWidth / 2.0); public static final Translation2d kFrontRightPosition = diff --git a/src/main/java/frc/robot/constants/USB.java b/src/main/java/frc/robot/constants/USB.java index 1d8d7c7..3b49457 100644 --- a/src/main/java/frc/robot/constants/USB.java +++ b/src/main/java/frc/robot/constants/USB.java @@ -6,4 +6,4 @@ public final class USB { public static final int xBoxController = 2; public static final int testController = 3; -} +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/Controls.java b/src/main/java/frc/robot/subsystems/Controls.java index 28b97f9..d60604f 100644 --- a/src/main/java/frc/robot/subsystems/Controls.java +++ b/src/main/java/frc/robot/subsystems/Controls.java @@ -8,7 +8,6 @@ import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Controls extends SubsystemBase { - private CommandSwerveDrivetrain m_swervedrive; private static DriverStation.Alliance m_allianceColor = DriverStation.Alliance.Red; /** Creates a new Controls. */ diff --git a/src/main/java/frc/robot/subsystems/CoralOuttake.java b/src/main/java/frc/robot/subsystems/CoralOuttake.java index 8ef4080..4b85cf2 100644 --- a/src/main/java/frc/robot/subsystems/CoralOuttake.java +++ b/src/main/java/frc/robot/subsystems/CoralOuttake.java @@ -14,6 +14,8 @@ import com.ctre.phoenix6.signals.NeutralModeValue; import com.ctre.phoenix6.sim.TalonFXSimState; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.numbers.N1; +import edu.wpi.first.math.numbers.N2; import edu.wpi.first.math.system.LinearSystem; import edu.wpi.first.math.system.plant.LinearSystemId; import edu.wpi.first.networktables.DoubleSubscriber; @@ -29,10 +31,10 @@ public class CoralOuttake extends SubsystemBase { private boolean m_isOuttaking = false; - private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1); + private LinearSystem outtakePlant = LinearSystemId.createDCMotorSystem(1, 1); private final TalonFX outtakeMotor = new TalonFX(CAN.coralOuttakeMotor); private final DCMotorSim outtakeMotorSim = - new DCMotorSim(outtakePlant, CORALOUTTAKE.Gearbox); // TODO implement sim code + new DCMotorSim(outtakePlant, CORALOUTTAKE.gearbox); // TODO implement sim code private double m_desiredPercentOutput; private final DutyCycleOut m_dutyCycleRequest = new DutyCycleOut(0); private double m_rpmSetpoint; @@ -144,4 +146,9 @@ private void updateSmartDashboard() { SmartDashboard.putNumber("CoralOuttake/DesiredPercentOutput", m_desiredPercentOutput); SmartDashboard.putNumber("CoralOuttake/rpmSetpoint", m_rpmSetpoint); } + + @Override + public void periodic() { + updateSmartDashboard(); + } } diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java index 79aa58d..0fa1942 100644 --- a/src/main/java/frc/robot/subsystems/Elevator.java +++ b/src/main/java/frc/robot/subsystems/Elevator.java @@ -6,7 +6,6 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.TalonFX; -import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.utils.CtreUtils; @@ -28,30 +27,6 @@ public Elevator() { CtreUtils.configureTalonFx(elevatormotors[1], configElevator); } - /** - * Example command factory method. - * - * @return a command - */ - public Command exampleMethodCommand() { - // Inline construction of command goes here. - // Subsystem::RunOnce implicitly requires `this` subsystem. - return runOnce( - () -> { - /* one-time action goes here */ - }); - } - - /** - * An example method querying a boolean state of the subsystem (for example, a digital sensor). - * - * @return value of some boolean subsystem state, such as a digital sensor. - */ - public boolean exampleCondition() { - // Query some boolean state, such as a digital sensor. - return false; - } - @Override public void periodic() { // This method will be called once per scheduler run diff --git a/src/main/java/frc/robot/utils/CtreUtils.java b/src/main/java/frc/robot/utils/CtreUtils.java index f90491c..6b7316b 100644 --- a/src/main/java/frc/robot/utils/CtreUtils.java +++ b/src/main/java/frc/robot/utils/CtreUtils.java @@ -5,15 +5,13 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration; import com.ctre.phoenix6.hardware.CANcoder; import com.ctre.phoenix6.hardware.TalonFX; -import com.ctre.phoenix6.signals.*; import edu.wpi.first.wpilibj.Alert; import edu.wpi.first.wpilibj.Alert.AlertType; import edu.wpi.first.wpilibj.RobotBase; import edu.wpi.first.wpilibj.Timer; import frc.robot.constants.CAN; -// import frc.robot.constants.SWERVE; - +// TODO: This class is also a mess public final class CtreUtils { /** * Initialize Phoenix Server by creating a dummy device. We do this so that the CANCoders don't