diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java index 8802c0bec..13da0f95c 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpet.java @@ -3,9 +3,10 @@ import org.wpilib.command3.Command; import frc.robot.RobotState; import org.littletonrobotics.junction.Logger; -import org.wpilib.command3.Mechanism; -public class MagicCarpet implements Mechanism { +import frc.lib.SubsystemBase; + +public class MagicCarpet extends SubsystemBase { private final MagicCarpetIO io; private final MagicCarpetIOInputsAutoLogged inputs = new MagicCarpetIOInputsAutoLogged(); public boolean manualRun; @@ -26,19 +27,22 @@ public MagicCarpet(MagicCarpetIO io) { * @return A command to run the magic carpet at a constant speed */ public Command run() { - return setManualControl(true) - .andThen(Commands.run(() -> io.setSpeed(MagicCarpetConstants.BELT_SPEED), this)) - .finallyDo(() -> setManualControl(false).schedule()) - .withName("start"); + return this.run( + coro -> { + io.setSpeed(MagicCarpetConstants.BELT_SPEED); + coro.fork(setManualControl(false)); + } + ).named("MagicCarpet Start"); } + /** * Control whether to run magic carpet through commands or periodic * * @param manual Call run manually instead of through periodic */ public Command setManualControl(boolean manual) { - return Commands.runOnce(() -> this.manualRun = manual, this); + return this.run(_ -> this.manualRun = manual).named("MagicCarpet SetManualControl"); } @Override diff --git a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java index 87ad37118..358295b05 100644 --- a/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java +++ b/src/main/java/frc/robot/subsystems/magicCarpet/MagicCarpetSparkMax.java @@ -10,6 +10,7 @@ import com.revrobotics.spark.SparkMax; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; import com.revrobotics.spark.config.SparkMaxConfig; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.util.MathUtil; import frc.robot.Schematic; @@ -20,7 +21,7 @@ public class MagicCarpetSparkMax implements MagicCarpetIO { public MagicCarpetSparkMax() { - motor = new SparkMax(Schematic.magicCarpetCanId, MotorType.kBrushless); + motor = new SparkMax(CANPort.CAN_D0, Schematic.magicCarpetCanId, MotorType.kBrushless); SparkMaxConfig config = new SparkMaxConfig(); config @@ -37,15 +38,14 @@ public MagicCarpetSparkMax() { @Override public void updateInputs(MagicCarpetIOInputs inputs) { // Called every 20 ms by subsystem periodic - inputs.appliedVolts = motor.getAppliedOutput() * motor.getBusVoltage(); - inputs.currentAmps = motor.getOutputCurrent(); - inputs.motorVelocity = encoder.getVelocity(); + inputs.appliedVolts = motor.getAppliedOutput().get() * motor.getBusVoltage().get(); + inputs.currentAmps = motor.getOutputCurrent().get(); + inputs.motorVelocity = encoder.getVelocity().get(); } @Override public void setSpeed(double speed) { - - motor.set(MathUtil.clamp(speed, 0.0, 1.0)); // so the is between 0 and 1, 1==100% + motor.setVoltage(MagicCarpetConstants.CURRENT_LIMIT * Math.clamp(speed, 0.0, 1.0)); } /** @@ -53,6 +53,6 @@ public void setSpeed(double speed) { */ @Override public void stop() { - motor.set(0.0); + motor.setVoltage(0); } }