From 1d3657f63a836d6883cb283c69a4c479b0e980d5 Mon Sep 17 00:00:00 2001 From: ColinH0 Date: Tue, 11 Feb 2025 16:18:57 -0500 Subject: [PATCH] reverted previous commit --- .vscode/settings.json | 3 +- src/main/java/frc/robot/RobotContainer.java | 35 ------------- .../coralrollerbar/CoralRoller.java | 50 ------------------- .../coralrollerbar/CoralRollerConstants.java | 6 --- .../coralrollerbar/CoralRollerIO.java | 18 ------- .../coralrollerbar/CoralRollerIOReal.java | 28 ----------- .../coralrollerbar/CoralRollerIOSim.java | 29 ----------- .../frc/robot/subsystems/pivot/Pivot.java | 39 --------------- .../subsystems/pivot/PivotConstants.java | 7 --- .../frc/robot/subsystems/pivot/PivotIO.java | 18 ------- .../robot/subsystems/pivot/PivotIOReal.java | 32 ------------ .../robot/subsystems/pivot/PivotIOSim.java | 33 ------------ 12 files changed, 1 insertion(+), 297 deletions(-) delete mode 100644 src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java delete mode 100644 src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java delete mode 100644 src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/Pivot.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotConstants.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIO.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java delete mode 100644 src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java diff --git a/.vscode/settings.json b/.vscode/settings.json index 71f3b3d..868594c 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,6 +68,5 @@ }, "[java]": { "editor.defaultFormatter": "richardwillis.vscode-spotless-gradle" - }, - "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable" + } } diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index a17fda1..d56c711 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,9 +26,6 @@ import frc.robot.subsystems.climb.Climb; import frc.robot.subsystems.climb.ClimbConstants; import frc.robot.subsystems.climb.ClimbIOSim; -import frc.robot.subsystems.coralrollerbar.CoralRoller; -import frc.robot.subsystems.coralrollerbar.CoralRollerIOReal; -import frc.robot.subsystems.coralrollerbar.CoralRollerIOSim; import frc.robot.subsystems.AlgaeRollerbar.AlgaeRollerbar; import frc.robot.subsystems.AlgaeRollerbar.AlgaeRollerbarIOReal; import frc.robot.subsystems.AlgaeRollerbar.AlgaeRollerbarIOSim; @@ -38,9 +35,6 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOSpark; -import frc.robot.subsystems.pivot.Pivot; -import frc.robot.subsystems.pivot.PivotIOReal; -import frc.robot.subsystems.pivot.PivotIOSim; import org.littletonrobotics.junction.networktables.LoggedDashboardChooser; /** @@ -52,8 +46,6 @@ public class RobotContainer { // Subsystems private final Drive drive; - private final CoralRoller coralRoller; - private final Pivot pivot; private final AlgaeRollerbar algaeRollerbar; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -65,8 +57,6 @@ public class RobotContainer { public RobotContainer() { switch (Constants.currentMode) { case REAL: - coralRoller = CoralRoller.init(new CoralRollerIOReal()); - pivot = Pivot.initialize(new PivotIOReal()); // Real robot, instantiate hardware IO implementations drive = new Drive( @@ -80,8 +70,6 @@ public RobotContainer() { break; case SIM: - coralRoller = CoralRoller.init(new CoralRollerIOSim()); - pivot = Pivot.initialize(new PivotIOSim()); // Sim robot, instantiate physics sim IO implementations drive = new Drive( @@ -94,8 +82,6 @@ public RobotContainer() { break; default: - coralRoller = CoralRoller.init(new CoralRollerIOSim()); - pivot = Pivot.initialize(new PivotIOSim()); // Replayed robot, disable IO implementations drive = new Drive( @@ -139,21 +125,6 @@ public RobotContainer() { * edu.wpi.first.wpilibj2.command.button.JoystickButton}. */ private void configureButtonBindings() { - controller - .leftTrigger() - .whileTrue( - Commands.startEnd( - () -> pivot.setVoltage(controller.getLeftTriggerAxis() * 12), - () -> pivot.setVoltage(0), - pivot)); - controller - .rightTrigger() - .whileTrue( - Commands.startEnd( - () -> pivot.setVoltage(controller.getRightTriggerAxis() * -12), - () -> pivot.setVoltage(0), - pivot)); - // Default command, normal field-relative drive drive.setDefaultCommand( DriveCommands.joystickDrive( @@ -205,12 +176,6 @@ private void configureButtonBindings() { new Pose2d(drive.getPose().getTranslation(), new Rotation2d())), drive) .ignoringDisable(true)); - controller - .leftBumper() - .onTrue(Commands.runOnce(() -> coralRoller.setVoltage(6), coralRoller)); - controller - .rightBumper() - .onTrue(Commands.runOnce(() -> coralRoller.setVoltage(-6), coralRoller)); controller .leftTrigger() diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java deleted file mode 100644 index 9589dd7..0000000 --- a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java +++ /dev/null @@ -1,50 +0,0 @@ -package frc.robot.subsystems.coralrollerbar; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - -public class CoralRoller extends SubsystemBase { - private final CoralRollerIO io; - private final CoralRollerIOInputsAutoLogged inputs = new CoralRollerIOInputsAutoLogged(); - - private double velocity = 0; - private static CoralRoller instance; - - public static CoralRoller getInstance() { - return instance; - } - - public static CoralRoller init(CoralRollerIO io) { - if (instance == null) { - instance = new CoralRoller(io); - } - return instance; - } - - private CoralRoller(CoralRollerIO io) { - this.io = io; - io.updateInputs(inputs); - } - - public void setVoltage(double v) { - io.setCoralRollerVoltage(v); - } - - public double getVelocity() { - return inputs.velocityRPM; - } - - public double getCurrent() { - return inputs.current; - } - - public double getVoltage() { - return inputs.voltage; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("CoralRoller", inputs); - } -} diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java deleted file mode 100644 index 827d7af..0000000 --- a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java +++ /dev/null @@ -1,6 +0,0 @@ -package frc.robot.subsystems.coralrollerbar; - -public final class CoralRollerConstants { - public static final int currentLimit = 30; - public static final double CanID = 0; -} diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java deleted file mode 100644 index 9137e63..0000000 --- a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.subsystems.coralrollerbar; - -import org.littletonrobotics.junction.AutoLog; - -public interface CoralRollerIO { - @AutoLog - public static class CoralRollerIOInputs { - public double current = 0; - public double voltage = 0; - public double velocityRPM = 0; - } - - public default void updateInputs(CoralRollerIOInputs inputs) {} - - public default void setDesiredState(double speed) {} - - public default void setCoralRollerVoltage(double volts) {} -} diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java deleted file mode 100644 index ac4fbdf..0000000 --- a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java +++ /dev/null @@ -1,28 +0,0 @@ -package frc.robot.subsystems.coralrollerbar; - -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; - -import edu.wpi.first.units.measure.Velocity; - -public class CoralRollerIOReal implements CoralRollerIO { - private final SparkMax CoralRollerMotor = new SparkMax(0, MotorType.kBrushless); - - public CoralRollerIOReal() { - SparkMaxConfig coralRollerConfig = new SparkMaxConfig(); - coralRollerConfig.smartCurrentLimit(CoralRollerConstants.currentLimit); - coralRollerConfig.voltageCompensation(12); - } - - @Override - public void setCoralRollerVoltage(double volts) { - CoralRollerMotor.setVoltage(volts); - } - @Override - public void updateInputs(CoralRollerIOInputs inputs){ - inputs.voltage = CoralRollerMotor.getBusVoltage() * CoralRollerMotor.getAppliedOutput(); - inputs.velocityRPM = CoralRollerMotor.getEncoder().getVelocity(); - inputs.current = CoralRollerMotor.getOutputCurrent(); - } -} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java deleted file mode 100644 index 2eb8d04..0000000 --- a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java +++ /dev/null @@ -1,29 +0,0 @@ -package frc.robot.subsystems.coralrollerbar; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.DCMotorSim; - -public class CoralRollerIOSim implements CoralRollerIO { - DCMotorSim motor = - new DCMotorSim( - LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.5, 0.1), - DCMotor.getKrakenX60(1), - 0.0, - 0.0); - private double appliedVolts = 0; - - @Override - public void setCoralRollerVoltage(double volts) { - appliedVolts = volts; - motor.setInputVoltage(appliedVolts); - } - - @Override - public void updateInputs(CoralRollerIOInputs inputs) { - inputs.velocityRPM = motor.getAngularVelocityRPM(); - inputs.voltage = motor.getInputVoltage(); - inputs.current = motor.getCurrentDrawAmps(); - motor.update(0.02); - } -} diff --git a/src/main/java/frc/robot/subsystems/pivot/Pivot.java b/src/main/java/frc/robot/subsystems/pivot/Pivot.java deleted file mode 100644 index 8f450e8..0000000 --- a/src/main/java/frc/robot/subsystems/pivot/Pivot.java +++ /dev/null @@ -1,39 +0,0 @@ -package frc.robot.subsystems.pivot; - -import edu.wpi.first.wpilibj2.command.SubsystemBase; -import org.littletonrobotics.junction.Logger; - -public class Pivot extends SubsystemBase { - private PivotIOInputsAutoLogged inputs = new PivotIOInputsAutoLogged(); - private PivotIO io; - private static Pivot instance; - private double voltage = 0; - - public Pivot(PivotIO somethingElse) { - this.io = somethingElse; - io.updateInputs(inputs); - } - - public static Pivot getInstance() { - return instance; - } - - public static Pivot initialize(PivotIO something) { - if (instance == null) { - instance = new Pivot(something); - } - return instance; - } - - public void setVoltage(double volts) { - io.setVoltage(volts); - voltage = volts; - } - - @Override - public void periodic() { - io.updateInputs(inputs); - Logger.processInputs("Pivot", inputs); - io.setVoltage(voltage); - } -} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java b/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java deleted file mode 100644 index 940097d..0000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotConstants.java +++ /dev/null @@ -1,7 +0,0 @@ -package frc.robot.subsystems.pivot; - -public class PivotConstants { - public static int pivotMotorID = 0; // not right - public static int stallLimit = 30; // not right - public static int freeLimit = 20; // not right -} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java b/src/main/java/frc/robot/subsystems/pivot/PivotIO.java deleted file mode 100644 index 3e09379..0000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIO.java +++ /dev/null @@ -1,18 +0,0 @@ -package frc.robot.subsystems.pivot; - -import org.littletonrobotics.junction.AutoLog; - -public interface PivotIO { - @AutoLog - public static class PivotIOInputs { - public double position = 0; - public double velocity = 0; - public double voltage = 0; - } - - public default void setVoltage(double voltage) {} - ; - - public default void updateInputs(PivotIOInputs inputs) {} - ; -} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java deleted file mode 100644 index 5750d54..0000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOReal.java +++ /dev/null @@ -1,32 +0,0 @@ -package frc.robot.subsystems.pivot; - -import com.revrobotics.AbsoluteEncoder; -import com.revrobotics.spark.SparkBase.PersistMode; -import com.revrobotics.spark.SparkBase.ResetMode; -import com.revrobotics.spark.SparkLowLevel.MotorType; -import com.revrobotics.spark.SparkMax; -import com.revrobotics.spark.config.SparkMaxConfig; - -public class PivotIOReal implements PivotIO { - private SparkMax pivotMotor = new SparkMax(PivotConstants.pivotMotorID, MotorType.kBrushless); - private SparkMaxConfig config = new SparkMaxConfig(); - private AbsoluteEncoder encoder = pivotMotor.getAbsoluteEncoder(); - - public PivotIOReal() { - config.smartCurrentLimit(PivotConstants.stallLimit); - pivotMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters); - pivotMotor.getStickyFaults(); - } - - @Override - public void setVoltage(double voltage) { - pivotMotor.setVoltage(voltage); - } - - @Override - public void updateInputs(PivotIOInputs inputs) { - inputs.voltage = pivotMotor.getBusVoltage(); - inputs.position = encoder.getPosition(); - inputs.velocity = encoder.getVelocity(); - } -} diff --git a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java b/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java deleted file mode 100644 index 8ebef75..0000000 --- a/src/main/java/frc/robot/subsystems/pivot/PivotIOSim.java +++ /dev/null @@ -1,33 +0,0 @@ -package frc.robot.subsystems.pivot; - -import edu.wpi.first.math.system.plant.DCMotor; -import edu.wpi.first.math.system.plant.LinearSystemId; -import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim; - -public class PivotIOSim implements PivotIO { - private double aaaaa = 0; - private SingleJointedArmSim pivotMotorSim = - new SingleJointedArmSim( - LinearSystemId.createSingleJointedArmSystem(DCMotor.getNEO(1), 1, 0.5), - DCMotor.getNEO(1), - 0.5, - 1, - 0, - 10, - false, - 0); - - @Override - public void setVoltage(double voltage) { - pivotMotorSim.setInputVoltage(voltage); - aaaaa = voltage; - } - - @Override - public void updateInputs(PivotIOInputs inputs) { - pivotMotorSim.update(0.02); - inputs.voltage = aaaaa; - inputs.velocity = pivotMotorSim.getVelocityRadPerSec(); - inputs.position = pivotMotorSim.getAngleRads(); - } -}