Fix: got climb and trap command to work with tramp

flamingchickens1540 · Feb 3, 2024 · a5115c6 · a5115c6
1 parent cc6bed9
commit a5115c6
Show file tree

Hide file tree

Showing 3 changed files with 10 additions and 18 deletions.
diff --git a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java
@@ -14,15 +14,16 @@ public TrampCommand(Tramp tramp) {
     }
 
     @Override
-    public void initialize() {}
+    public void initialize() {
+        tramp.setPercent(0.5);
+    }
+
+    @Override
+    public void execute() {}
 
     @Override
-    public void execute() {
-        if (tramp.isBeamBreakBlocked()) {
-            tramp.stopTramp();
-        } else {
-            tramp.scoreTrampCommand();
-        }
+    public boolean isFinished() {
+return tramp.isBeamBreakBlocked();
     }
 
     @Override

diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java
@@ -1,11 +1,11 @@
 package org.team1540.robot2024.commands.climb;
 
-import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import org.team1540.robot2024.Constants;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
 import org.team1540.robot2024.commands.ElevatorSetpointCommand;
+import org.team1540.robot2024.commands.TrampCommand;
 import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 import org.team1540.robot2024.subsystems.tramp.Tramp;
@@ -17,10 +17,7 @@ public TrapAndClimbSequence(Elevator elevator, Hooks hooks, Tramp tramp) {
                 new ClimbSequence(elevator, hooks), //Climb
                 new WaitCommand(0.1), //TODO: Perhaps remove this or change it depending on how climbing turns out to be
                 new ElevatorSetpointCommand(elevator, ElevatorState.TRAP),
-                new ParallelDeadlineGroup(
-                        new WaitCommand(Constants.Tramp.TRAP_SCORING_TIME_SECONDS),
-                        tramp.scoreTrampCommand() //TODO: Do whatever to this but not my job
-                ),
+                        new TrampCommand(tramp).withTimeout(Constants.Tramp.TRAP_SCORING_TIME_SECONDS),
                 new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
         );
     }

diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/Tramp.java
@@ -1,7 +1,6 @@
 package org.team1540.robot2024.subsystems.tramp;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.Commands;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import org.littletonrobotics.junction.Logger;
 
@@ -27,11 +26,6 @@ public void periodic() {
         Logger.processInputs("Tramp", inputs);
     }
 
-    public Command scoreTrampCommand() {
-        // TODO: Tune the percentage
-        return Commands.runOnce(() -> setPercent(0.5), this);
-    }
-
     public void stopTramp() {
         io.setVoltage(0);
     }