From 5461251189ef45bf79fd7711c456377b3fcc1727 Mon Sep 17 00:00:00 2001 From: ghorrocks217 Date: Tue, 21 Jan 2025 20:55:24 -0500 Subject: [PATCH 1/6] State entrance and elevator bar --- .../frc/robot/statemachine/StateMachine.java | 26 +++++++++---------- .../robot/statemachine/reusable/State.java | 12 ++++----- .../elevator/ElevatorConstants.java | 2 ++ .../robot/subsystems/elevator/ElevatorIO.java | 1 + .../subsystems/elevator/ElevatorIOSpark.java | 12 +++++++++ 5 files changed, 34 insertions(+), 19 deletions(-) diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java index c95f35f..3d9ab53 100644 --- a/src/main/java/frc/robot/statemachine/StateMachine.java +++ b/src/main/java/frc/robot/statemachine/StateMachine.java @@ -65,25 +65,25 @@ public StateMachine(CommandXboxController driverController, CommandXboxControlle .withChild(goToIntake) .withChild(intakeGamePiece); - // Children inside children - goToScoringPosition.withChild(goToScoreCoral) - .withChild(goToScoreAlgae); - scoreGamePiece.withChild(scoreCoral) - .withChild(scoreAlgae); + goToScoringPosition.withChild(goToScoreCoral, () -> false, 0, "Has coral") + .withChild(goToScoreAlgae, () -> false, 1, "Has algae"); - goToIntake.withChild(goToReefIntake) - .withChild(goToStationIntake); + scoreGamePiece.withChild(scoreCoral, () -> false, 0, "Has coral") + .withChild(scoreAlgae, () -> false, 1, "Has algae"); - intakeGamePiece.withChild(intakeCoral) - .withChild(intakeAlgae); + goToIntake.withChild(goToReefIntake, () -> false, 0, "Reef selected") + .withChild(goToStationIntake, () -> false, 1, "Station selected"); + + intakeGamePiece.withChild(intakeCoral, () -> false, 0, "Reef selected") + .withChild(intakeAlgae, () -> false, 1, "Station selected"); // Specific Algae intake and score - goToScoreAlgae.withChild(goToScoreNet) - .withChild(goToScoreProcessor); + goToScoreAlgae.withChild(goToScoreNet, () -> false, 0, "Net selected") + .withChild(goToScoreProcessor, () -> false, 1, "Processor selected"); - scoreAlgae.withChild(scoreAlgaeNet) - .withChild(scoreAlgaeProcessor); + scoreAlgae.withChild(scoreAlgaeNet, () -> false, 0, "Net selected") + .withChild(scoreAlgaeProcessor, () -> false, 1, "Processor selected"); // Example, will be button board later manual.withTransition(goToScoringPosition, () -> false, "Driver presses score") diff --git a/src/main/java/frc/robot/statemachine/reusable/State.java b/src/main/java/frc/robot/statemachine/reusable/State.java index 15da786..f404a0b 100644 --- a/src/main/java/frc/robot/statemachine/reusable/State.java +++ b/src/main/java/frc/robot/statemachine/reusable/State.java @@ -147,8 +147,8 @@ public State withModeTransitions(State disabled, State teleop, State auto, State * * @return This state */ - public State withChild(State child, BooleanSupplier condition, int priority) { - addChild(child, condition, priority, false); + public State withChild(State child, BooleanSupplier condition, int priority, String entranceConditionName) { + addChild(child, condition, priority, false, entranceConditionName); return this; } @@ -156,7 +156,7 @@ public State withChild(State child, BooleanSupplier condition, int priority) { * Add a child state to this state (will never be entered by default) */ public State withChild(State child) { - addChild(child, () -> false, Integer.MAX_VALUE, false); + addChild(child, () -> false, Integer.MAX_VALUE, false, "impossible"); return this; } @@ -167,7 +167,7 @@ public State withChild(State child) { * @return This state */ public State withDefaultChild(State child) { - addChild(child, () -> true, Integer.MAX_VALUE, true); + addChild(child, () -> true, Integer.MAX_VALUE, true, "default"); return this; } @@ -326,7 +326,7 @@ void setParentState(State parentState) { this.parentState = parentState; } - private void addChild(State child, BooleanSupplier condition, int priority, boolean isDefault) { + private void addChild(State child, BooleanSupplier condition, int priority, boolean isDefault, String entranceConditionName) { if (isDefault) { if (hasDefaultChild) throw new RuntimeException("A state can only have one default child"); @@ -334,6 +334,6 @@ private void addChild(State child, BooleanSupplier condition, int priority, bool } child.setParentState(this); children.add(child); - entranceConditions.add(new TransitionInfo(child, condition, priority, null)); + entranceConditions.add(new TransitionInfo(child, condition, priority, entranceConditionName)); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index 5a4428f..c2d0bcb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -6,4 +6,6 @@ public class ElevatorConstants { public static final int currentLimit = 40; public static final double bottomHallEffect = 0; public static final double topHallEffect = 1; + public static final double bottomBarMeters = 0.2; + public static final double topBarMeters = 2; } \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java index 9e9e57d..ef06697 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java @@ -13,6 +13,7 @@ public static class ElevatorIOInputs { public boolean topHallEffectClosed = false; public boolean bottomEffectClosed = false; public double velocityRadPerSec; + } public default void updateInputs(ElevatorIOInputs inputs) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index 2a55e9b..2963aeb 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -23,6 +23,8 @@ public class ElevatorIOSpark implements ElevatorIO { private final DigitalInput topHallEffect = new DigitalInput(0); private final DigitalInput bottomHallEffect = new DigitalInput(0); + public double currentBarMeters = 0; + public ElevatorIOSpark(int currentLimit) { SparkMaxConfig configRight = new SparkMaxConfig(); @@ -67,5 +69,15 @@ public void updateInputs(ElevatorIOInputs inputs) { inputs.bottomEffectClosed = bottomHallEffect.get(); inputs.topHallEffectClosed = topHallEffect.get(); + + if(bottomHallEffect.get()) { + currentBarMeters = 0; + elevatorEncoder.setPosition(0); + } + + if(topHallEffect.get()) { + //Unknown height + currentBarMeters = 2; + } } } From d1f68585783d07a43d0cf4792523915b120525d9 Mon Sep 17 00:00:00 2001 From: Alex-idk Date: Tue, 21 Jan 2025 20:55:31 -0500 Subject: [PATCH 2/6] ADd note fot swerve following --- src/main/java/frc/robot/subsystems/drive/DriveCommands.java | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java index 23786d9..607fbf2 100644 --- a/src/main/java/frc/robot/subsystems/drive/DriveCommands.java +++ b/src/main/java/frc/robot/subsystems/drive/DriveCommands.java @@ -164,6 +164,7 @@ private static Command followTrajectory(Drive drive, Trajectory traj, Supplier desiredRotationSupplier = desiredRot; return Commands.run(() -> { double curTime = timer.get(); @@ -185,6 +186,7 @@ private static Command followTrajectory(Drive drive, Trajectory traj, Supplier timer.hasElapsed(traj.getTotalTimeSeconds())) .finallyDo(timer::stop).withName("Follow Trajectory"); }; + //change stop private static Command goToPoint(Drive drive, Pose2d pose, Supplier desiredRotation, DoubleSupplier joystickRot, boolean useJoystick) { From 76b3808445c51c520358a140c78bb4ccf6d9f19f Mon Sep 17 00:00:00 2001 From: Alex-idk Date: Wed, 22 Jan 2025 14:57:28 -0500 Subject: [PATCH 3/6] Fixed subsystem code and test mode --- src/main/java/frc/robot/RobotContainer.java | 20 ++++- .../robot/statemachine/GoToStationIntkae.java | 5 -- .../frc/robot/statemachine/StateMachine.java | 7 +- .../robot/statemachine/states/TestState.java | 10 ++- .../subsystems/EndEffector/EndEffector.java | 47 +---------- .../EndEffector/EndEffectorConstants.java | 9 +- .../subsystems/EndEffector/EndEffectorIO.java | 21 ++--- .../EndEffector/EndEffectorIOSpark.java | 82 ++++++------------- .../frc/robot/subsystems/climber/Climber.java | 3 +- .../robot/subsystems/elevator/Elevator.java | 8 +- .../elevator/ElevatorConstants.java | 4 +- .../subsystems/elevator/ElevatorIOSpark.java | 47 +++++------ .../robot/subsystems/shoulder/Shoulder.java | 71 ++++++++++++++++ .../shoulder/ShoulderConstants.java | 7 ++ .../robot/subsystems/shoulder/ShoulderIO.java | 22 +++++ .../subsystems/shoulder/ShoulderIOSpark.java | 57 +++++++++++++ 16 files changed, 260 insertions(+), 160 deletions(-) delete mode 100644 src/main/java/frc/robot/statemachine/GoToStationIntkae.java create mode 100644 src/main/java/frc/robot/subsystems/shoulder/Shoulder.java create mode 100644 src/main/java/frc/robot/subsystems/shoulder/ShoulderConstants.java create mode 100644 src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java create mode 100644 src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSpark.java diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 0535404..6b3d3cb 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -26,6 +26,8 @@ import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine; import frc.robot.statemachine.StateMachine; +import frc.robot.subsystems.EndEffector.EndEffector; +import frc.robot.subsystems.EndEffector.EndEffectorIOSpark; import frc.robot.subsystems.drive.Drive; import frc.robot.subsystems.drive.DriveCommands; import frc.robot.subsystems.drive.DriveConstants; @@ -35,6 +37,10 @@ import frc.robot.subsystems.drive.ModuleIO; import frc.robot.subsystems.drive.ModuleIOSim; import frc.robot.subsystems.drive.ModuleIOSpark; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.elevator.ElevatorIOSpark; +import frc.robot.subsystems.shoulder.Shoulder; +import frc.robot.subsystems.shoulder.ShoulderIOSpark; import frc.robot.subsystems.vision.Vision; import frc.robot.subsystems.vision.VisionIO; import frc.robot.subsystems.vision.VisionIOPhotonVisionSim; @@ -58,6 +64,9 @@ public class RobotContainer { // Subsystems private final Drive drive; private final Vision vision; + private final Elevator elevator; + private final Shoulder shoulder; + private final EndEffector endEffector; private SwerveDriveSimulation driveSimulation = null; // Controller @@ -86,6 +95,9 @@ public RobotContainer() { new ModuleIOSpark(3)); this.vision = new Vision(drive); + elevator = new Elevator(new ElevatorIOSpark()); + shoulder = new Shoulder(new ShoulderIOSpark()); + endEffector = new EndEffector(new EndEffectorIOSpark()); break; case SIM: @@ -110,6 +122,9 @@ public RobotContainer() { camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose)); vision.setPoseSupplier(driveSimulation::getSimulatedDriveTrainPose); + elevator = new Elevator(new ElevatorIOSpark()); + shoulder = new Shoulder(new ShoulderIOSpark()); + endEffector = new EndEffector(new EndEffectorIOSpark()); break; default: @@ -128,12 +143,15 @@ public RobotContainer() { vision = new Vision(drive, new VisionIO() { }, new VisionIO() { }); + elevator = new Elevator(new ElevatorIOSpark()); + shoulder = new Shoulder(new ShoulderIOSpark()); + endEffector = new EndEffector(new EndEffectorIOSpark()); break; } AllianceUtil.setRobot(drive::getPose); - stateMachine = new StateMachine(driverController, operatorController, drive); + stateMachine = new StateMachine(driverController, operatorController, drive, elevator, shoulder, endEffector); // Set up auto routines autoChooser = new LoggedDashboardChooser<>("Auto Choices"); diff --git a/src/main/java/frc/robot/statemachine/GoToStationIntkae.java b/src/main/java/frc/robot/statemachine/GoToStationIntkae.java deleted file mode 100644 index 185076f..0000000 --- a/src/main/java/frc/robot/statemachine/GoToStationIntkae.java +++ /dev/null @@ -1,5 +0,0 @@ -package frc.robot.statemachine; - -public class GoToStationIntkae { - -} diff --git a/src/main/java/frc/robot/statemachine/StateMachine.java b/src/main/java/frc/robot/statemachine/StateMachine.java index 3d9ab53..7f83c1e 100644 --- a/src/main/java/frc/robot/statemachine/StateMachine.java +++ b/src/main/java/frc/robot/statemachine/StateMachine.java @@ -24,18 +24,21 @@ import frc.robot.statemachine.states.tele.ScoreAlgaeProcessor; import frc.robot.statemachine.states.tele.ScoreCoral; import frc.robot.statemachine.states.tele.ScoreGamePiece; +import frc.robot.subsystems.EndEffector.EndEffector; import frc.robot.subsystems.drive.Drive; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.shoulder.Shoulder; public class StateMachine extends StateMachineBase { public StateMachine(CommandXboxController driverController, CommandXboxController operatorController, - Drive drive) { + Drive drive, Elevator elevator, Shoulder shoulder, EndEffector endEffector) { super(); State disabled = new DisabledState(this); currentState = disabled; State teleop = new TeleState(this); State auto = new AutoState(this); - State test = new TestState(this, driverController); + State test = new TestState(this, driverController, elevator, shoulder, endEffector); this.registerToRootState(test, auto, teleop, disabled); diff --git a/src/main/java/frc/robot/statemachine/states/TestState.java b/src/main/java/frc/robot/statemachine/states/TestState.java index 59e4569..0bce41f 100644 --- a/src/main/java/frc/robot/statemachine/states/TestState.java +++ b/src/main/java/frc/robot/statemachine/states/TestState.java @@ -4,12 +4,20 @@ import frc.robot.statemachine.reusable.SmartXboxController; import frc.robot.statemachine.reusable.State; import frc.robot.statemachine.reusable.StateMachineBase; +import frc.robot.subsystems.EndEffector.EndEffector; +import frc.robot.subsystems.elevator.Elevator; +import frc.robot.subsystems.shoulder.Shoulder; public class TestState extends State { - public TestState(StateMachineBase stateMachine, CommandXboxController controller) { + public TestState(StateMachineBase stateMachine, CommandXboxController controller, Elevator elevator, Shoulder shoulder, EndEffector endEffector) { super(stateMachine); @SuppressWarnings("unused") SmartXboxController testController = new SmartXboxController(controller, loop); + + startWhenActive(elevator.elevatorMove(controller::getRightY)); + startWhenActive(shoulder.manualPivot(controller::getLeftY)); + controller.a().onTrue(endEffector.runIntake(() -> 1.0)); + controller.b().onTrue(endEffector.runIntake(() -> -1.0)); } } diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java index 9f8ce80..60c8493 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java @@ -12,24 +12,6 @@ public class EndEffector extends SubsystemBase { private final EndEffectorIO io; private final EndEffectorIOInputsAutoLogged inputs = new EndEffectorIOInputsAutoLogged(); - enum LevelAngle { - L1(0, 0, 0, 0), - L23(0, 0, 0, 0), - L4(0, 0, 0, 0); - - private double closeDist; - private double closeAngle; - private double farDist; - private double farAngle; - - private LevelAngle(double closeDist, double closeAngle, double farDist, double farAngle) { - this.closeAngle = closeAngle; - this.closeDist = closeDist; - this.farDist = farDist; - this.farAngle = farAngle; - } - }; - public EndEffector(EndEffectorIO io) { this.io = io; } @@ -40,32 +22,11 @@ public void periodic() { Logger.processInputs("EndEffector", inputs); } - public Command manualpivot(double desiredspeed) { - return null; - } - - public Command scoreL1(DoubleSupplier robotDistance) { - return Commands - .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L1)), this) - .withName("Set Shoulder to L1 Scoring position"); + public Command runIntake(DoubleSupplier desiredSpeed) { + return Commands.none(); } - public Command scoreL2AndL3(DoubleSupplier robotDistance) { - return Commands - .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L23)), this) - .withName("Set Shoulder to L1 Scoring position"); + public Command runUntilSpike(DoubleSupplier desiredSpeed) { + return Commands.none(); } - - public Command scoreL4(DoubleSupplier robotDistance) { - return Commands - .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L4)), this) - .withName("Set Shoulder to L1 Scoring position"); - } - - private double calculateAngleForDist(double robotDist, LevelAngle desiredLevel) { - return ((desiredLevel.farAngle - desiredLevel.closeAngle) - / (desiredLevel.farDist - desiredLevel.closeDist)) * (robotDist - desiredLevel.closeDist) - + desiredLevel.closeAngle; - } - } diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java index 17c99ff..3b8022e 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorConstants.java @@ -1,11 +1,10 @@ package frc.robot.subsystems.EndEffector; public class EndEffectorConstants { - public static final int algeaMotorCanId = 0; - public static final int coralMotorCanId = 0; - public static final int shoulderCanId = 0; + public static final int coralCanId = 9; + public static final int algaeCanId = 10; public static final int proximitySensorChannel = 0; - public static final double intakeMotorReduction = 1.0; - public static final double wristMotorReduction = 1.0; + public static final double coralMotorReduction = 1.0; + public static final double algaeMotorReduction = 1.0; public static final int currentLimit = 40; } diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java index 062f4f6..e198d9b 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIO.java @@ -3,25 +3,18 @@ import org.littletonrobotics.junction.AutoLog; public interface EndEffectorIO { - @AutoLog public static class EndEffectorIOInputs { - public double wristAngleRad = 0.0; - public double wristSpeedRadsPerSec = 0.0; - public double wristAppliedVoltage = 0.0; - public double wristCurrantAmps = 0.0; - public double intakeWheelsSpeedRadPerSec = 0.0; - public double intakeWheelsAppliedVoltage = 0.0; - public double intakeWheelsCurrantAmps = 0.0; - + public double coralAppliedVoltage = 0.0; + public double coralCurrentAmps = 0.0; + public double algaeAppliedVoltage = 0.0; + public double algaeCurrentAmps = 0.0; public double proximitySensorDistance = 0.0; } public void updateInputs(EndEffectorIOInputs inputs); - public void setShoulderAngle(double angle); - - public void setShoulderSpeed(double speed); + public void setCoralSpeed(double speed); - public void setIntakeSpeed(double speed); -} \ No newline at end of file + public void setAlgaeSpeed(double speed); +} diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java index 04125ad..7d118c4 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java @@ -1,15 +1,12 @@ package frc.robot.subsystems.EndEffector; -import static frc.robot.subsystems.EndEffector.EndEffectorConstants.currentLimit; -import static frc.robot.subsystems.EndEffector.EndEffectorConstants.intakeMotorReduction; import static frc.robot.subsystems.EndEffector.EndEffectorConstants.*; -import static frc.robot.subsystems.EndEffector.EndEffectorConstants.wristMotorReduction; -import static frc.robot.util.SparkUtil.ifOk; import static frc.robot.util.SparkUtil.tryUntilOk; import java.util.function.DoubleSupplier; -import com.revrobotics.AbsoluteEncoder; +import static frc.robot.util.SparkUtil.ifOk; + import com.revrobotics.spark.SparkBase.PersistMode; import com.revrobotics.spark.SparkBase.ResetMode; import com.revrobotics.spark.SparkLowLevel.MotorType; @@ -18,83 +15,54 @@ import com.revrobotics.spark.config.SparkMaxConfig; public class EndEffectorIOSpark implements EndEffectorIO { - private final SparkMax algeaMotor = new SparkMax(algeaMotorCanId, MotorType.kBrushless); - private final SparkMax shoulderMotor = new SparkMax(shoulderCanId, MotorType.kBrushless); - private final SparkMax coralMotor = new SparkMax(coralMotorCanId, MotorType.kBrushless); - private final AbsoluteEncoder shoulderEncoder = algeaMotor.getAbsoluteEncoder(); - + private final SparkMax coralMotor = new SparkMax(coralCanId, MotorType.kBrushless); + private final SparkMax algaeMotor = new SparkMax(algaeCanId, MotorType.kBrushless); + public EndEffectorIOSpark() { - var algeaMotorConfig = new SparkMaxConfig(); - algeaMotorConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(12.0); - algeaMotorConfig + var algaeMotorConfig = new SparkMaxConfig(); + algaeMotorConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(12.0); + algaeMotorConfig .encoder - .positionConversionFactor(2.0 * Math.PI / intakeMotorReduction) - .velocityConversionFactor((2.0 * Math.PI) / 60.0 / intakeMotorReduction) + .positionConversionFactor(2.0 * Math.PI / algaeMotorReduction) + .velocityConversionFactor((2.0 * Math.PI) / 60.0 / algaeMotorReduction) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); - tryUntilOk(algeaMotor, 5, () -> algeaMotor.configure(algeaMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - + tryUntilOk(algaeMotor, 5, () -> algaeMotor.configure(algaeMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + var coralMotorConfig = new SparkMaxConfig(); coralMotorConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(12.0); coralMotorConfig .encoder - .positionConversionFactor(2.0 * Math.PI / intakeMotorReduction) - .velocityConversionFactor((2.0 * Math.PI) / 60.0 / intakeMotorReduction) + .positionConversionFactor(2.0 * Math.PI / coralMotorReduction) + .velocityConversionFactor((2.0 * Math.PI) / 60.0 / coralMotorReduction) .uvwMeasurementPeriod(10) .uvwAverageDepth(2); tryUntilOk(coralMotor, 5, () -> coralMotor.configure(coralMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - - - var wristConfig = new SparkMaxConfig(); - wristConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(12.0); - wristConfig - .encoder - .positionConversionFactor(2.0 * Math.PI / wristMotorReduction) - .velocityConversionFactor((2.0 * Math.PI) / 60.0 / wristMotorReduction) - .uvwMeasurementPeriod(10) - .uvwAverageDepth(2); - - tryUntilOk(shoulderMotor, 5, () -> shoulderMotor.configure(wristConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - } @Override public void updateInputs(EndEffectorIOInputs inputs) { - ifOk(algeaMotor, shoulderEncoder::getVelocity, (value) -> inputs.intakeWheelsSpeedRadPerSec = value); + ifOk(coralMotor, coralMotor::getOutputCurrent, (value) -> inputs.coralCurrentAmps = value); + ifOk(algaeMotor, algaeMotor::getOutputCurrent, (value) -> inputs.algaeCurrentAmps = value); ifOk( - algeaMotor, - new DoubleSupplier[] {algeaMotor::getAppliedOutput, algeaMotor::getBusVoltage}, - (values) -> inputs.intakeWheelsAppliedVoltage = values[0] * values[1]); - ifOk(algeaMotor, algeaMotor::getOutputCurrent, (value) -> inputs.intakeWheelsCurrantAmps = value); - - - ifOk(shoulderMotor, shoulderEncoder::getPosition, (value) -> inputs.wristAngleRad = value); - ifOk(shoulderMotor, shoulderEncoder::getVelocity, (value) -> inputs.wristSpeedRadsPerSec = value); - ifOk(shoulderMotor, shoulderMotor::getOutputCurrent, (value) -> inputs.wristCurrantAmps = value); + coralMotor, + new DoubleSupplier[] { coralMotor::getAppliedOutput, coralMotor::getBusVoltage }, + (values) -> inputs.coralAppliedVoltage = values[0] * values[1]); ifOk( - shoulderMotor, - new DoubleSupplier[] {algeaMotor::getAppliedOutput, algeaMotor::getBusVoltage}, - (values) -> inputs.wristAppliedVoltage = values[0] * values[1]); - + algaeMotor, + new DoubleSupplier[] { algaeMotor::getAppliedOutput, algaeMotor::getBusVoltage }, + (values) -> inputs.algaeAppliedVoltage = values[0] * values[1]); } @Override - public void setShoulderAngle(double desiredangle) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setAngle'"); - } + public void setCoralSpeed(double speed) { - @Override - public void setShoulderSpeed(double desiredspeed) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setSpeed'"); } @Override - public void setIntakeSpeed(double desiredspeed) { - // TODO Auto-generated method stub - throw new UnsupportedOperationException("Unimplemented method 'setSpeed'"); + public void setAlgaeSpeed(double speed) { + } } diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index 79a75a4..cb2a885 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -3,6 +3,7 @@ import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; public class Climber extends SubsystemBase{ @@ -20,6 +21,6 @@ public void periodic() { } public Command setRadians(double radians) { - return null; + return Commands.none(); } } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index c4a8250..7664e37 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -41,12 +41,12 @@ public void periodic() { Logger.processInputs("Elevator", inputs); } - public Command elevatorMove(double speed) { - return null; + public Command elevatorMove(DoubleSupplier speed) { + return Commands.run(() -> io.setSpeed(speed.getAsDouble())); } public Command runOnJoystick() { - return null; + return Commands.none(); } public Command toL1(DoubleSupplier robotDistance) { @@ -70,7 +70,7 @@ public Command toL4(DoubleSupplier robotDistance) { } public Command intake() { - return null; + return Commands.none(); } private double calculateAngleForDist(double robotDist, LevelHeight desiredLevel) { diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java index c2d0bcb..9c296f1 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java @@ -1,7 +1,7 @@ package frc.robot.subsystems.elevator; public class ElevatorConstants { - public static final int leftCanId = 5; - public static final int rightCanId = 5; + public static final int leftCanId = 6; + public static final int rightCanId = 7; public static final double motorReduction = 1.0; public static final int currentLimit = 40; public static final double bottomHallEffect = 0; diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java index 2963aeb..e52e6d7 100644 --- a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSpark.java @@ -9,6 +9,7 @@ import com.revrobotics.RelativeEncoder; 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.SparkBaseConfig.IdleMode; @@ -17,37 +18,33 @@ import com.revrobotics.spark.config.SparkMaxConfig; public class ElevatorIOSpark implements ElevatorIO { - private final SparkMax leftElevatorMoter = new SparkMax(leftCanId, null); - private final SparkMax rightElevatorMoter = new SparkMax(rightCanId, null); + private final SparkMax leftElevatorMotor = new SparkMax(leftCanId, MotorType.kBrushless); + private final SparkMax rightElevatorMotor = new SparkMax(rightCanId, MotorType.kBrushless); - private final DigitalInput topHallEffect = new DigitalInput(0); - private final DigitalInput bottomHallEffect = new DigitalInput(0); + private final RelativeEncoder elevatorEncoder = leftElevatorMotor.getEncoder(); - public double currentBarMeters = 0; + private final DigitalInput topHallEffect = new DigitalInput(0); + private final DigitalInput bottomHallEffect = new DigitalInput(1); - public ElevatorIOSpark(int currentLimit) { + public ElevatorIOSpark() { SparkMaxConfig configRight = new SparkMaxConfig(); - configRight.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(0); - configRight.encoder + configRight.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).encoder .positionConversionFactor(0); - tryUntilOk(rightElevatorMoter, 5, () -> rightElevatorMoter.configure(configRight, + tryUntilOk(rightElevatorMotor, 5, () -> rightElevatorMotor.configure(configRight, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); SparkMaxConfig configLeft = new SparkMaxConfig(); - configLeft.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(0).inverted(true); - configLeft.encoder - .positionConversionFactor(0); + configLeft.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).inverted(true); - tryUntilOk(leftElevatorMoter, 5, () -> leftElevatorMoter.configure(configLeft, ResetMode.kResetSafeParameters, + + tryUntilOk(leftElevatorMotor, 5, () -> leftElevatorMotor.configure(configLeft, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); - configRight.follow(leftElevatorMoter); + configRight.follow(leftElevatorMotor); } - private final RelativeEncoder elevatorEncoder = leftElevatorMoter.getEncoder(); - @Override public void setPosition(double desiredPosition) { throw new UnsupportedOperationException("Unimplemented method 'setPosition'"); @@ -55,29 +52,29 @@ public void setPosition(double desiredPosition) { @Override public void setSpeed(double desiredSpeed) { - throw new UnsupportedOperationException("Unimplemented method 'setSpeed'"); + leftElevatorMotor.set(desiredSpeed); } public void updateInputs(ElevatorIOInputs inputs) { - ifOk(leftElevatorMoter, elevatorEncoder::getPosition, (value) -> inputs.positionRad = value); - ifOk(leftElevatorMoter, elevatorEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); + ifOk(leftElevatorMotor, elevatorEncoder::getPosition, (value) -> inputs.positionRad = value); + ifOk(leftElevatorMotor, elevatorEncoder::getVelocity, (value) -> inputs.velocityRadPerSec = value); ifOk( - leftElevatorMoter, - new DoubleSupplier[] { leftElevatorMoter::getAppliedOutput, leftElevatorMoter::getBusVoltage }, + leftElevatorMotor, + new DoubleSupplier[] { leftElevatorMotor::getAppliedOutput, leftElevatorMotor::getBusVoltage }, (values) -> inputs.appliedVolts = values[0] * values[1]); - ifOk(leftElevatorMoter, leftElevatorMoter::getOutputCurrent, (value) -> inputs.currentAmps = value); + ifOk(leftElevatorMotor, leftElevatorMotor::getOutputCurrent, (value) -> inputs.currentAmps = value); inputs.bottomEffectClosed = bottomHallEffect.get(); inputs.topHallEffectClosed = topHallEffect.get(); if(bottomHallEffect.get()) { - currentBarMeters = 0; - elevatorEncoder.setPosition(0); + elevatorEncoder.setPosition(bottomBarMeters); + } if(topHallEffect.get()) { //Unknown height - currentBarMeters = 2; + elevatorEncoder.setPosition(topBarMeters); } } } diff --git a/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java b/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java new file mode 100644 index 0000000..c23e230 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shoulder/Shoulder.java @@ -0,0 +1,71 @@ +package frc.robot.subsystems.shoulder; + +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.Logger; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.SubsystemBase; + +public class Shoulder extends SubsystemBase { + private final ShoulderIO io; + private final ShoulderIOInputsAutoLogged inputs = new ShoulderIOInputsAutoLogged(); + + enum LevelAngle { + L1(0, 0, 0, 0), + L23(0, 0, 0, 0), + L4(0, 0, 0, 0); + + private double closeDist; + private double closeAngle; + private double farDist; + private double farAngle; + + private LevelAngle(double closeDist, double closeAngle, double farDist, double farAngle) { + this.closeAngle = closeAngle; + this.closeDist = closeDist; + this.farDist = farDist; + this.farAngle = farAngle; + } + }; + + public Shoulder(ShoulderIO io) { + this.io = io; + } + + @Override + public void periodic() { + io.updateInputs(inputs); + Logger.processInputs("Shoulder", inputs); + } + + public Command manualPivot(DoubleSupplier desiredSpeed) { + return Commands.run(() -> io.setShoulderSpeed(desiredSpeed)); + } + + public Command scoreL1(DoubleSupplier robotDistance) { + return Commands + .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L1)), this) + .withName("Set Shoulder to L1 Scoring position"); + } + + public Command scoreL2AndL3(DoubleSupplier robotDistance) { + return Commands + .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L23)), this) + .withName("Set Shoulder to L1 Scoring position"); + } + + public Command scoreL4(DoubleSupplier robotDistance) { + return Commands + .run(() -> io.setShoulderAngle(calculateAngleForDist(robotDistance.getAsDouble(), LevelAngle.L4)), this) + .withName("Set Shoulder to L1 Scoring position"); + } + + private double calculateAngleForDist(double robotDist, LevelAngle desiredLevel) { + return ((desiredLevel.farAngle - desiredLevel.closeAngle) + / (desiredLevel.farDist - desiredLevel.closeDist)) * (robotDist - desiredLevel.closeDist) + + desiredLevel.closeAngle; + } + +} diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderConstants.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderConstants.java new file mode 100644 index 0000000..c02fabb --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderConstants.java @@ -0,0 +1,7 @@ +package frc.robot.subsystems.shoulder; + +public class ShoulderConstants { + public static final int shoulderCanId = 8; + public static final double shoulderMotorReduction = 1.0; + public static final int currentLimit = 40; +} diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java new file mode 100644 index 0000000..d547a1d --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIO.java @@ -0,0 +1,22 @@ +package frc.robot.subsystems.shoulder; + +import java.util.function.DoubleSupplier; + +import org.littletonrobotics.junction.AutoLog; + +public interface ShoulderIO { + + @AutoLog + public static class ShoulderIOInputs { + public double shoulderAngleRad = 0.0; + public double shoulderSpeedRadsPerSec = 0.0; + public double shoulderAppliedVoltage = 0.0; + public double shoulderCurrantAmps = 0.0; + } + + public void updateInputs(ShoulderIOInputs inputs); + + public void setShoulderAngle(double angle); + + public void setShoulderSpeed(DoubleSupplier speed); +} \ No newline at end of file diff --git a/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSpark.java b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSpark.java new file mode 100644 index 0000000..00b1803 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/shoulder/ShoulderIOSpark.java @@ -0,0 +1,57 @@ +package frc.robot.subsystems.shoulder; + +import static frc.robot.subsystems.shoulder.ShoulderConstants.*; +import static frc.robot.util.SparkUtil.tryUntilOk; + +import java.util.function.DoubleSupplier; + +import static frc.robot.util.SparkUtil.ifOk; + +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.SparkBaseConfig.IdleMode; +import com.revrobotics.spark.config.SparkMaxConfig; + +public class ShoulderIOSpark implements ShoulderIO { + private final SparkMax shoulderMotor = new SparkMax(shoulderCanId, MotorType.kBrushless); + private final AbsoluteEncoder shoulderEncoder = shoulderMotor.getAbsoluteEncoder(); + + public ShoulderIOSpark() { + var shoulderMotorConfig = new SparkMaxConfig(); + shoulderMotorConfig.idleMode(IdleMode.kBrake).smartCurrentLimit(currentLimit).voltageCompensation(12.0); + shoulderMotorConfig + .encoder + .positionConversionFactor(2.0 * Math.PI / shoulderMotorReduction) + .velocityConversionFactor((2.0 * Math.PI) / 60.0 / shoulderMotorReduction) + .uvwMeasurementPeriod(10) + .uvwAverageDepth(2); + + tryUntilOk(shoulderMotor, 5, () -> shoulderMotor.configure(shoulderMotorConfig, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters)); + } + + @Override + public void updateInputs(ShoulderIOInputs inputs) { + ifOk(shoulderMotor, shoulderEncoder::getPosition, (value) -> inputs.shoulderAngleRad = value); + ifOk(shoulderMotor, shoulderEncoder::getVelocity, (value) -> inputs.shoulderSpeedRadsPerSec = value); + ifOk(shoulderMotor, shoulderMotor::getOutputCurrent, (value) -> inputs.shoulderCurrantAmps = value); + ifOk( + shoulderMotor, + new DoubleSupplier[] { shoulderMotor::getAppliedOutput, shoulderMotor::getBusVoltage }, + (values) -> inputs.shoulderAppliedVoltage = values[0] * values[1]); + } + + @Override + public void setShoulderAngle(double desiredangle) { + // TODO Auto-generated method stub + throw new UnsupportedOperationException("Unimplemented method 'setAngle'"); + } + + @Override + public void setShoulderSpeed(DoubleSupplier desiredSpeed) { + // TODO Auto-generated method stub + shoulderMotor.set(desiredSpeed.getAsDouble()); + } +} From ba027c3ea6540d9cad42630938ff2c9d0cde5fb8 Mon Sep 17 00:00:00 2001 From: Marcus Flusche Date: Wed, 22 Jan 2025 18:53:00 -0500 Subject: [PATCH 4/6] Finished subsystem motor controls --- .../java/frc/robot/subsystems/EndEffector/EndEffector.java | 2 +- .../frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java index 60c8493..01c8262 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffector.java @@ -23,7 +23,7 @@ public void periodic() { } public Command runIntake(DoubleSupplier desiredSpeed) { - return Commands.none(); + return Commands.run(() -> {io.setAlgaeSpeed(desiredSpeed.getAsDouble()); io.setCoralSpeed(desiredSpeed.getAsDouble());}); } public Command runUntilSpike(DoubleSupplier desiredSpeed) { diff --git a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java index 7d118c4..fc64954 100644 --- a/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java +++ b/src/main/java/frc/robot/subsystems/EndEffector/EndEffectorIOSpark.java @@ -58,11 +58,11 @@ public void updateInputs(EndEffectorIOInputs inputs) { @Override public void setCoralSpeed(double speed) { - + coralMotor.set(speed); } @Override public void setAlgaeSpeed(double speed) { - + algaeMotor.set(speed); } } From 5ada5e3e8693d65d1728eadda138df7eaed82a48 Mon Sep 17 00:00:00 2001 From: Alex-idk Date: Thu, 23 Jan 2025 13:45:58 -0500 Subject: [PATCH 5/6] Set up climber code --- src/main/java/frc/robot/subsystems/climber/Climber.java | 2 +- src/main/java/frc/robot/subsystems/climber/ClimberIO.java | 2 +- .../java/frc/robot/subsystems/climber/ClimberIOSpark.java | 2 +- src/main/java/frc/robot/subsystems/elevator/Elevator.java | 4 ---- 4 files changed, 3 insertions(+), 7 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index cb2a885..c03fb81 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -20,7 +20,7 @@ public void periodic() { Logger.processInputs("Climber", inputs); } - public Command setRadians(double radians) { + public Command setSpeed(double radians) { return Commands.none(); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java index 2b0685f..f82cd00 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIO.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIO.java @@ -13,5 +13,5 @@ public static class ClimberIOInputs { public default void updateInputs(ClimberIOInputs inputs) {} - public default void setPositionRadians(double position) {} + public default void setSpeed(double speed) {} } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 4d14359..99d1a12 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -49,7 +49,7 @@ public void updateInputs(ClimberIOInputs inputs) { } @Override - public void setPositionRadians(double radians) { + public void setSpeed(double speed) { // TODO: Auto-generated method stub } diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java index 7664e37..2e3288b 100644 --- a/src/main/java/frc/robot/subsystems/elevator/Elevator.java +++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java @@ -45,10 +45,6 @@ public Command elevatorMove(DoubleSupplier speed) { return Commands.run(() -> io.setSpeed(speed.getAsDouble())); } - public Command runOnJoystick() { - return Commands.none(); - } - public Command toL1(DoubleSupplier robotDistance) { return Commands.run(() -> io.setPosition(calculateAngleForDist(robotDistance.getAsDouble(), LevelHeight.L1))) .withName("Set elevator to L1 Scoring level"); From 756171af6ee9c23bfcd3b26a4f9c5d15696e77a2 Mon Sep 17 00:00:00 2001 From: Alex-idk Date: Thu, 23 Jan 2025 20:01:10 -0500 Subject: [PATCH 6/6] finished climber code --- .../java/frc/robot/subsystems/climber/Climber.java | 13 +++++++++++-- .../robot/subsystems/climber/ClimberConstants.java | 2 +- .../robot/subsystems/climber/ClimberIOSpark.java | 1 + 3 files changed, 13 insertions(+), 3 deletions(-) diff --git a/src/main/java/frc/robot/subsystems/climber/Climber.java b/src/main/java/frc/robot/subsystems/climber/Climber.java index c03fb81..87b2c31 100644 --- a/src/main/java/frc/robot/subsystems/climber/Climber.java +++ b/src/main/java/frc/robot/subsystems/climber/Climber.java @@ -1,11 +1,16 @@ package frc.robot.subsystems.climber; +import java.util.function.DoubleSupplier; + import org.littletonrobotics.junction.Logger; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.SubsystemBase; + + + public class Climber extends SubsystemBase{ private final ClimberIO io; private final ClimberIOInputsAutoLogged inputs = new ClimberIOInputsAutoLogged(); @@ -20,7 +25,11 @@ public void periodic() { Logger.processInputs("Climber", inputs); } - public Command setSpeed(double radians) { - return Commands.none(); + public Command setSpeed(double speed) { + return Commands.run(() -> io.setSpeed(speed)); + } + + public Command move(DoubleSupplier speed) { + return Commands.runEnd(() -> io.setSpeed(speed.getAsDouble()), () -> io.setSpeed(0.0)); } } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java index 52f787b..e11a5b5 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberConstants.java @@ -3,5 +3,5 @@ public class ClimberConstants { public static final int climberCanId = 5; public static final double motorReduction = 1.0; - public static final int currentLimit = 40; + public static final int currentLimit = 40; } diff --git a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java index 99d1a12..b4a053a 100644 --- a/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java +++ b/src/main/java/frc/robot/subsystems/climber/ClimberIOSpark.java @@ -51,6 +51,7 @@ public void updateInputs(ClimberIOInputs inputs) { @Override public void setSpeed(double speed) { // TODO: Auto-generated method stub + climber.set(speed); } } \ No newline at end of file