fix: Got rid of tramp command stuff and fixed trap scoring :D

src/main/java/org/team1540/robot2024/Constants.java

@@ -91,4 +91,10 @@ public enum ElevatorState {
             }
         }
     }
+
+    public static class Tramp {
+        public static final double AMP_PERCENTAGE = 0.1540; //TODO: Find these values :D
+        public static final double TRAP_PERCENTAGE = 0.1678; //TODO: Find these values :D
+        public static final double TRAP_SCORING_TIME_SECONDS = 1.114; //TODO: Find these values :D
+    }
 }

src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java

@@ -5,7 +5,6 @@
 import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
 
 public class ElevatorSetpointCommand extends Command {
-    //TODO: Write an elevator subsystem, so get rid of this later (Not David's Job)
     private final Elevator elevator;
     private final ElevatorState state;
     public ElevatorSetpointCommand(Elevator elevator, ElevatorState state) {
@@ -22,4 +21,9 @@ public void initialize() {
     public void end(boolean interrupted) {
         elevator.stop();
     }
+    @Override
+    public boolean isFinished() {
+        return elevator.isAtSetpoint();
+    }
+
 }

src/main/java/org/team1540/robot2024/commands/ClimbSequence.java → src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java

@@ -1,8 +1,9 @@
-package org.team1540.robot2024.commands;
+package org.team1540.robot2024.commands.climb;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.commands.ElevatorSetpointCommand;
 import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 
@@ -16,7 +17,7 @@ public ClimbSequence(Elevator elevator, Hooks hooks) { //TODO: Write servos no i
                 new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB),
                 //TODO: Put whatever drive/alignment command we plan on using here
                 new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
-                new DeployHooks(hooks) //TODO: Deploy hooks
+                hooks.deployHooksCommand() //TODO: Deploy hooks
         );
     }
 }

src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java → src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java

@@ -1,15 +1,16 @@
-package org.team1540.robot2024.commands;
+package org.team1540.robot2024.commands.climb;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.commands.ElevatorSetpointCommand;
 import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 
 public class DeclimbSequence extends SequentialCommandGroup {
     public DeclimbSequence(Elevator elevator, Hooks hooks) {
         addCommands(
             new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
-            new DeployHooks(hooks), //Release hooks
+            hooks.undeployHooksCommand(), //Release hooks
             new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
         );
     }

src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java → src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java

@@ -1,8 +1,9 @@
-package org.team1540.robot2024.commands;
+package org.team1540.robot2024.commands.climb;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;
 
+//TODO: Write this command Tramp people :D
 public class ScoreInTrap extends Command {
     private final Tramp tramp;
     public ScoreInTrap(Tramp tramp) {
@@ -11,10 +12,11 @@ public ScoreInTrap(Tramp tramp) {
     }
     @Override
     public void initialize() {
-        tramp.scoreInTrap();
+        //TODO: Score in trap :D
     }
 
     @Override
     public void end(boolean interrupted) {
+
     }
 }

src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java → src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java

@@ -1,8 +1,12 @@
-package org.team1540.robot2024.commands;
+package org.team1540.robot2024.commands.climb;
 
+import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 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.subsystems.fakesubsystems.Elevator;
 import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;
@@ -14,7 +18,10 @@ 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 ScoreInTrap(tramp), //TODO: Do whatever to this but not my job
+                new ParallelDeadlineGroup(
+                        new WaitCommand(Constants.Tramp.TRAP_SCORING_TIME_SECONDS),
+                        new ScoreInTrap(tramp) //TODO: Do whatever to this but not my job
+                ),
                 new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
         );
     }

src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java

@@ -11,4 +11,8 @@ public void goToSetpoint(double heightMeters) {
     public void stop() {
 
     }
+
+    public boolean isAtSetpoint() {
+        return true;
+    }
 }

src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java

@@ -1,5 +1,7 @@
 package org.team1540.robot2024.subsystems.fakesubsystems;
 
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 //TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
 public class Hooks extends SubsystemBase {
@@ -8,4 +10,21 @@ public class Hooks extends SubsystemBase {
     public void deployHooks() {
 
     }
+
+    public void undeployHooks() {
+
+    }
+
+    /**
+     * Factory method for deploying hooks
+     */
+    public Command deployHooksCommand() {
+        return new InstantCommand(this::deployHooks, this);
+    }
+    /**
+     * Factory method for undeploying hooks
+     */
+    public Command undeployHooksCommand() {
+        return new InstantCommand(this::undeployHooks, this);
+    }
 }

src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java

@@ -4,7 +4,10 @@
 //TODO: Get rid of this class and make an actual one its just a temporary thingie for David :D
 
 public class Tramp extends SubsystemBase {
-    public void scoreInTrap() {
+    public void setPercent(double percentage) {
 
     }
+    public boolean hasScored() {
+        return true;
+    }
 }