From 0dcb0a2bdd2f693a06e5ea1fd8065e16ecbf07a7 Mon Sep 17 00:00:00 2001
From: DaviTheDinosaur <KongD@catlin.edu>
Date: Thu, 25 Jan 2024 15:58:01 -0800
Subject: [PATCH 1/3] feat: Climb, Trap, Declimb sequences :D

---
 .../org/team1540/robot2024/Constants.java     | 36 +++++++++++++++++++
 .../robot2024/commands/ActuateHooks.java      |  9 +++++
 .../robot2024/commands/ClimbSequence.java     | 20 +++++++++++
 .../robot2024/commands/DeclimbSequence.java   | 14 ++++++++
 .../commands/ElevatorSetpointCommand.java     | 11 ++++++
 .../robot2024/commands/ScoreInTrap.java       |  7 ++++
 .../commands/TrapAndClimbSequence.java        | 17 +++++++++
 .../robot2024/commands/TrapClimbDeClimb.java  |  5 +++
 8 files changed, 119 insertions(+)
 create mode 100644 src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index a81310f2..c03b1cec 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/src/main/java/org/team1540/robot2024/Constants.java
@@ -55,4 +55,40 @@ public static class Drivetrain {
         public static final double DRIVE_BASE_RADIUS = Math.hypot(TRACK_WIDTH_X / 2.0, TRACK_WIDTH_Y / 2.0);
         public static final double MAX_ANGULAR_SPEED = MAX_LINEAR_SPEED / DRIVE_BASE_RADIUS;
     }
+
+    public static class Elevator {
+        public static final double CHAIN_HEIGHT_METERS = Units.inchesToMeters(28.25);
+        public static final double ELEVATOR_MAX_HEIGHT = Units.inchesToMeters(48.0);
+        public static final double ELEVATOR_MINIMUM_HEIGHT = Units.inchesToMeters(27.0);
+        public static final double CLIMBING_HOOKS_MINIMUM_HEIGHT = Units.inchesToMeters(12.0);
+        public static final double CLIMBING_HOOKS_MAX_HEIGHT = CLIMBING_HOOKS_MINIMUM_HEIGHT + ELEVATOR_MAX_HEIGHT - ELEVATOR_MINIMUM_HEIGHT;
+
+        public enum ElevatorState {
+            /**
+             * At max height :D
+             */
+            TOP(ELEVATOR_MAX_HEIGHT),
+            /**
+             * At minimum height :D
+             */
+            BOTTOM(ELEVATOR_MINIMUM_HEIGHT),
+            /**
+             * At height for top of initial climb :D
+             */
+            CLIMB(254.0), //TODO: Find these values :D
+            /**
+             * At height for trap doing :D
+             */
+            TRAP(254.0), //TODO: Find these values :D
+            /**
+             * At height for top of initial climb :D
+             */
+            AMP(254.0); //TODO: Find these values :D
+
+            public final double heightMeters;
+            ElevatorState(double heightMeters) {
+                this.heightMeters = heightMeters;
+            }
+        }
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java b/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
new file mode 100644
index 00000000..2e812b47
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
@@ -0,0 +1,9 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ActuateHooks extends Command {
+    public ActuateHooks(boolean isDeployed) {
+        //TODO: Not David's job
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
new file mode 100644
index 00000000..a535458f
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
@@ -0,0 +1,20 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+
+public class ClimbSequence extends SequentialCommandGroup {
+    public ClimbSequence() { //TODO: Write servos no idea how they are supposed to work for now, add them somewhere :D
+        addCommands(
+                new ParallelCommandGroup(
+                        new ElevatorSetpointCommand(ElevatorState.BOTTOM)
+                        //TODO: Put whatever drive/alignment command we plan on using here
+                ),
+                new ElevatorSetpointCommand(ElevatorState.CLIMB),
+                //TODO: Put whatever drive/alignment command we plan on using here
+                new ElevatorSetpointCommand(ElevatorState.BOTTOM),
+                new ActuateHooks(true) //TODO: Actuate hooks
+        );
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java b/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
new file mode 100644
index 00000000..471b6b2b
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
@@ -0,0 +1,14 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+
+public class DeclimbSequence extends SequentialCommandGroup {
+    public DeclimbSequence() {
+        addCommands(
+            new ElevatorSetpointCommand(ElevatorState.BOTTOM),
+            new ActuateHooks(false), //Release hooks
+            new ElevatorSetpointCommand(ElevatorState.TOP)
+        );
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
new file mode 100644
index 00000000..937c8b98
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
@@ -0,0 +1,11 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+
+public class ElevatorSetpointCommand extends Command {
+    //TODO: Write this (Not David's Job)
+    public ElevatorSetpointCommand(ElevatorState state) {
+        //TODO: Do the enum stuff (Not David's job)
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java b/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
new file mode 100644
index 00000000..e7b4b246
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
@@ -0,0 +1,7 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class ScoreInTrap extends Command {
+    //TODO: Write this(Not David's job) :D
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
new file mode 100644
index 00000000..cfacdbb6
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
@@ -0,0 +1,17 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+
+public class TrapAndClimbSequence extends SequentialCommandGroup {
+    public TrapAndClimbSequence() {
+        addCommands(
+                new ClimbSequence(), //Climb
+                new WaitCommand(0.1), //TODO: Perhaps remove this or change it depending on how climbing turns out to be
+                new ElevatorSetpointCommand(ElevatorState.TRAP),
+                new ScoreInTrap(), //TODO: Do whatever to this but not my job
+                new ElevatorSetpointCommand(ElevatorState.BOTTOM)
+        );
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java b/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java
new file mode 100644
index 00000000..a27d34be
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java
@@ -0,0 +1,5 @@
+package org.team1540.robot2024.commands;
+
+public class TrapClimbDeClimb {
+
+}

From b6f8ca30fb820844727bcad26f98b6bb7701a9d5 Mon Sep 17 00:00:00 2001
From: DaviTheDinosaur <KongD@catlin.edu>
Date: Thu, 25 Jan 2024 16:42:58 -0800
Subject: [PATCH 2/3] feat: Removed TrapClimbDeclimb, added hooks command,
 added fake dummy subsystems to make my life easier :D

---
 .../robot2024/commands/ActuateHooks.java      |  9 --------
 .../robot2024/commands/ClimbSequence.java     | 12 ++++++----
 .../robot2024/commands/DeclimbSequence.java   | 10 ++++----
 .../robot2024/commands/DeployHooks.java       | 23 +++++++++++++++++++
 .../commands/ElevatorSetpointCommand.java     | 20 +++++++++++++---
 .../robot2024/commands/ScoreInTrap.java       | 15 +++++++++++-
 .../commands/TrapAndClimbSequence.java        | 14 +++++++----
 .../robot2024/commands/TrapClimbDeClimb.java  |  5 ----
 .../subsystems/fakesubsystems/Elevator.java   | 14 +++++++++++
 .../subsystems/fakesubsystems/Hooks.java      | 11 +++++++++
 .../subsystems/fakesubsystems/Tramp.java      | 10 ++++++++
 11 files changed, 111 insertions(+), 32 deletions(-)
 delete mode 100644 src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
 create mode 100644 src/main/java/org/team1540/robot2024/commands/DeployHooks.java
 delete mode 100644 src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
 create mode 100644 src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java

diff --git a/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java b/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
deleted file mode 100644
index 2e812b47..00000000
--- a/src/main/java/org/team1540/robot2024/commands/ActuateHooks.java
+++ /dev/null
@@ -1,9 +0,0 @@
-package org.team1540.robot2024.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-public class ActuateHooks extends Command {
-    public ActuateHooks(boolean isDeployed) {
-        //TODO: Not David's job
-    }
-}
diff --git a/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
index a535458f..6838662a 100644
--- a/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
@@ -3,18 +3,20 @@
 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.subsystems.fakesubsystems.Elevator;
+import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 
 public class ClimbSequence extends SequentialCommandGroup {
-    public ClimbSequence() { //TODO: Write servos no idea how they are supposed to work for now, add them somewhere :D
+    public ClimbSequence(Elevator elevator, Hooks hooks) { //TODO: Write servos no idea how they are supposed to work for now, add them somewhere :D
         addCommands(
                 new ParallelCommandGroup(
-                        new ElevatorSetpointCommand(ElevatorState.BOTTOM)
+                        new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
                         //TODO: Put whatever drive/alignment command we plan on using here
                 ),
-                new ElevatorSetpointCommand(ElevatorState.CLIMB),
+                new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB),
                 //TODO: Put whatever drive/alignment command we plan on using here
-                new ElevatorSetpointCommand(ElevatorState.BOTTOM),
-                new ActuateHooks(true) //TODO: Actuate hooks
+                new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
+                new DeployHooks(hooks) //TODO: Deploy hooks
         );
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java b/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
index 471b6b2b..abced5fa 100644
--- a/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
@@ -2,13 +2,15 @@
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
+import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
 
 public class DeclimbSequence extends SequentialCommandGroup {
-    public DeclimbSequence() {
+    public DeclimbSequence(Elevator elevator, Hooks hooks) {
         addCommands(
-            new ElevatorSetpointCommand(ElevatorState.BOTTOM),
-            new ActuateHooks(false), //Release hooks
-            new ElevatorSetpointCommand(ElevatorState.TOP)
+            new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
+            new DeployHooks(hooks), //Release hooks
+            new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
         );
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/DeployHooks.java b/src/main/java/org/team1540/robot2024/commands/DeployHooks.java
new file mode 100644
index 00000000..abfb2fd4
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/commands/DeployHooks.java
@@ -0,0 +1,23 @@
+package org.team1540.robot2024.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
+
+public class DeployHooks extends Command {
+
+    private final Hooks hooks;
+    public DeployHooks(Hooks hooks) {
+        //TODO: Not David's job
+        this.hooks = hooks;
+        addRequirements(hooks);
+    }
+    @Override
+    public void initialize() {
+        hooks.deployHooks();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
index 937c8b98..e810167b 100644
--- a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
+++ b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
@@ -2,10 +2,24 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
 
 public class ElevatorSetpointCommand extends Command {
-    //TODO: Write this (Not David's Job)
-    public ElevatorSetpointCommand(ElevatorState state) {
-        //TODO: Do the enum stuff (Not David's job)
+    //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) {
+        this.elevator = elevator;
+        this.state = state;
+        addRequirements(elevator);
+    }
+    @Override
+    public void initialize() {
+        elevator.goToSetpoint(state.heightMeters);
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+        elevator.stop();
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java b/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
index e7b4b246..6c33ca0e 100644
--- a/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
+++ b/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
@@ -1,7 +1,20 @@
 package org.team1540.robot2024.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;
 
 public class ScoreInTrap extends Command {
-    //TODO: Write this(Not David's job) :D
+    private final Tramp tramp;
+    public ScoreInTrap(Tramp tramp) {
+        this.tramp = tramp;
+        addRequirements(tramp);
+    }
+    @Override
+    public void initialize() {
+        tramp.scoreInTrap();
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
index cfacdbb6..6cee2e46 100644
--- a/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
@@ -3,15 +3,19 @@
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import org.team1540.robot2024.Constants.Elevator.ElevatorState;
+import org.team1540.robot2024.subsystems.fakesubsystems.Elevator;
+import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
+import org.team1540.robot2024.subsystems.fakesubsystems.Tramp;
 
 public class TrapAndClimbSequence extends SequentialCommandGroup {
-    public TrapAndClimbSequence() {
+
+    public TrapAndClimbSequence(Elevator elevator, Hooks hooks, Tramp tramp) {
         addCommands(
-                new ClimbSequence(), //Climb
+                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(ElevatorState.TRAP),
-                new ScoreInTrap(), //TODO: Do whatever to this but not my job
-                new ElevatorSetpointCommand(ElevatorState.BOTTOM)
+                new ElevatorSetpointCommand(elevator, ElevatorState.TRAP),
+                new ScoreInTrap(tramp), //TODO: Do whatever to this but not my job
+                new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM)
         );
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java b/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java
deleted file mode 100644
index a27d34be..00000000
--- a/src/main/java/org/team1540/robot2024/commands/TrapClimbDeClimb.java
+++ /dev/null
@@ -1,5 +0,0 @@
-package org.team1540.robot2024.commands;
-
-public class TrapClimbDeClimb {
-
-}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java
new file mode 100644
index 00000000..ebe7ffa0
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java
@@ -0,0 +1,14 @@
+package org.team1540.robot2024.subsystems.fakesubsystems;
+
+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 Elevator extends SubsystemBase {
+    public void goToSetpoint(double heightMeters) {
+
+    }
+
+    public void stop() {
+
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
new file mode 100644
index 00000000..e9cada24
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
@@ -0,0 +1,11 @@
+package org.team1540.robot2024.subsystems.fakesubsystems;
+
+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 {
+
+    //TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted
+    public void deployHooks() {
+
+    }
+}
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java
new file mode 100644
index 00000000..8a74676d
--- /dev/null
+++ b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java
@@ -0,0 +1,10 @@
+package org.team1540.robot2024.subsystems.fakesubsystems;
+
+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 Tramp extends SubsystemBase {
+    public void scoreInTrap() {
+
+    }
+}

From af7557cdd6cbf1016ad47c0617e28c6255e864b6 Mon Sep 17 00:00:00 2001
From: DaviTheDinosaur <KongD@catlin.edu>
Date: Fri, 26 Jan 2024 07:07:59 -0800
Subject: [PATCH 3/3] fix: Got rid of tramp command stuff and fixed trap
 scoring :D

---
 .../org/team1540/robot2024/Constants.java     |  6 +++++
 .../robot2024/commands/DeployHooks.java       | 23 -------------------
 .../commands/ElevatorSetpointCommand.java     |  6 ++++-
 .../commands/{ => climb}/ClimbSequence.java   |  5 ++--
 .../commands/{ => climb}/DeclimbSequence.java |  5 ++--
 .../commands/{ => climb}/ScoreInTrap.java     |  6 +++--
 .../{ => climb}/TrapAndClimbSequence.java     | 11 +++++++--
 .../subsystems/fakesubsystems/Elevator.java   |  4 ++++
 .../subsystems/fakesubsystems/Hooks.java      | 19 +++++++++++++++
 .../subsystems/fakesubsystems/Tramp.java      |  5 +++-
 10 files changed, 57 insertions(+), 33 deletions(-)
 delete mode 100644 src/main/java/org/team1540/robot2024/commands/DeployHooks.java
 rename src/main/java/org/team1540/robot2024/commands/{ => climb}/ClimbSequence.java (85%)
 rename src/main/java/org/team1540/robot2024/commands/{ => climb}/DeclimbSequence.java (76%)
 rename src/main/java/org/team1540/robot2024/commands/{ => climb}/ScoreInTrap.java (76%)
 rename src/main/java/org/team1540/robot2024/commands/{ => climb}/TrapAndClimbSequence.java (63%)

diff --git a/src/main/java/org/team1540/robot2024/Constants.java b/src/main/java/org/team1540/robot2024/Constants.java
index c03b1cec..07d48856 100644
--- a/src/main/java/org/team1540/robot2024/Constants.java
+++ b/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
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/DeployHooks.java b/src/main/java/org/team1540/robot2024/commands/DeployHooks.java
deleted file mode 100644
index abfb2fd4..00000000
--- a/src/main/java/org/team1540/robot2024/commands/DeployHooks.java
+++ /dev/null
@@ -1,23 +0,0 @@
-package org.team1540.robot2024.commands;
-
-import edu.wpi.first.wpilibj2.command.Command;
-import org.team1540.robot2024.subsystems.fakesubsystems.Hooks;
-
-public class DeployHooks extends Command {
-
-    private final Hooks hooks;
-    public DeployHooks(Hooks hooks) {
-        //TODO: Not David's job
-        this.hooks = hooks;
-        addRequirements(hooks);
-    }
-    @Override
-    public void initialize() {
-        hooks.deployHooks();
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-
-    }
-}
diff --git a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java b/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
index e810167b..2096a1d2 100644
--- a/src/main/java/org/team1540/robot2024/commands/ElevatorSetpointCommand.java
+++ b/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();
+    }
+
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java
similarity index 85%
rename from src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
rename to src/main/java/org/team1540/robot2024/commands/climb/ClimbSequence.java
index 6838662a..6ffcbbf0 100644
--- a/src/main/java/org/team1540/robot2024/commands/ClimbSequence.java
+++ b/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
         );
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
similarity index 76%
rename from src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
rename to src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
index abced5fa..87368202 100644
--- a/src/main/java/org/team1540/robot2024/commands/DeclimbSequence.java
+++ b/src/main/java/org/team1540/robot2024/commands/climb/DeclimbSequence.java
@@ -1,7 +1,8 @@
-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;
 
@@ -9,7 +10,7 @@ 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)
         );
     }
diff --git a/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java b/src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java
similarity index 76%
rename from src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
rename to src/main/java/org/team1540/robot2024/commands/climb/ScoreInTrap.java
index 6c33ca0e..c5b5cd0a 100644
--- a/src/main/java/org/team1540/robot2024/commands/ScoreInTrap.java
+++ b/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) {
+
     }
 }
diff --git a/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java
similarity index 63%
rename from src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
rename to src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java
index 6cee2e46..f345fefa 100644
--- a/src/main/java/org/team1540/robot2024/commands/TrapAndClimbSequence.java
+++ b/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)
         );
     }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java
index ebe7ffa0..da8b5a6c 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Elevator.java
+++ b/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;
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
index e9cada24..f5a8804d 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Hooks.java
+++ b/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);
+    }
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java b/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java
index 8a74676d..bfd4dfca 100644
--- a/src/main/java/org/team1540/robot2024/subsystems/fakesubsystems/Tramp.java
+++ b/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;
+    }
 }