diff --git a/.vscode/settings.json b/.vscode/settings.json index 868594c..71f3b3d 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -68,5 +68,6 @@ }, "[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 5c16d9e..09d2a57 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -23,6 +23,9 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.commands.DriveCommands; +import frc.robot.subsystems.coralrollerbar.CoralRoller; +import frc.robot.subsystems.coralrollerbar.CoralRollerIOReal; +import frc.robot.subsystems.coralrollerbar.CoralRollerIOSim; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.GyroIO; import frc.robot.subsystems.drive.GyroIOPigeon2; @@ -43,6 +46,7 @@ public class RobotContainer { // Subsystems private final Drive drive; + private final CoralRoller coralRoller; private final Pivot pivot; // Controller private final CommandXboxController controller = new CommandXboxController(0); @@ -54,6 +58,7 @@ 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 = @@ -66,6 +71,7 @@ public RobotContainer() { break; case SIM: + coralRoller = CoralRoller.init(new CoralRollerIOSim()); pivot = Pivot.initialize(new PivotIOSim()); // Sim robot, instantiate physics sim IO implementations drive = @@ -78,6 +84,7 @@ public RobotContainer() { break; default: + coralRoller = CoralRoller.init(new CoralRollerIOSim()); pivot = Pivot.initialize(new PivotIOSim()); // Replayed robot, disable IO implementations drive = @@ -166,6 +173,12 @@ 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)); } /** diff --git a/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java new file mode 100644 index 0000000..9589dd7 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRoller.java @@ -0,0 +1,50 @@ +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 new file mode 100644 index 0000000..827d7af --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerConstants.java @@ -0,0 +1,6 @@ +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 new file mode 100644 index 0000000..9137e63 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIO.java @@ -0,0 +1,18 @@ +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 new file mode 100644 index 0000000..ac4fbdf --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOReal.java @@ -0,0 +1,28 @@ +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 new file mode 100644 index 0000000..2eb8d04 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/coralrollerbar/CoralRollerIOSim.java @@ -0,0 +1,29 @@ +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); + } +}