Merge remote-tracking branch 'origin/climbing' into elevator

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

@@ -57,6 +57,12 @@ public static class Drivetrain {
     }
 
     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 static int talonId1;
         public static int talonId2;
         public static double kS = 0.25;
@@ -68,5 +74,39 @@ public static class Elevator {
         public static int motionMagicCruiseVelocity = 80;
         public static int motionMagicAcceleration = 160;
         public static int motionMagicJerk = 1600;
+
+        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;
+            }
+        }
+    }
+
+    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

@@ -0,0 +1,29 @@
+package org.team1540.robot2024.commands;
+
+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 {
+    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();
+    }
+    @Override
+    public boolean isFinished() {
+        return elevator.isAtSetpoint();
+    }
+
+}

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

@@ -0,0 +1,23 @@
+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;
+
+public class ClimbSequence extends SequentialCommandGroup {
+    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(elevator, ElevatorState.BOTTOM)
+                        //TODO: Put whatever drive/alignment command we plan on using here
+                ),
+                new ElevatorSetpointCommand(elevator, ElevatorState.CLIMB),
+                //TODO: Put whatever drive/alignment command we plan on using here
+                new ElevatorSetpointCommand(elevator, ElevatorState.BOTTOM),
+                hooks.deployHooksCommand() //TODO: Deploy hooks
+        );
+    }
+}

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

@@ -0,0 +1,17 @@
+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),
+            hooks.undeployHooksCommand(), //Release hooks
+            new ElevatorSetpointCommand(elevator, ElevatorState.TOP)
+        );
+    }
+}

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

@@ -0,0 +1,22 @@
+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) {
+        this.tramp = tramp;
+        addRequirements(tramp);
+    }
+    @Override
+    public void initialize() {
+        //TODO: Score in trap :D
+    }
+
+    @Override
+    public void end(boolean interrupted) {
+
+    }
+}

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

@@ -0,0 +1,28 @@
+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;
+
+public class TrapAndClimbSequence extends SequentialCommandGroup {
+
+    public TrapAndClimbSequence(Elevator elevator, Hooks hooks, Tramp tramp) {
+        addCommands(
+                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),
+                        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

@@ -0,0 +1,18 @@
+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() {
+
+    }
+
+    public boolean isAtSetpoint() {
+        return true;
+    }
+}

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

@@ -0,0 +1,30 @@
+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 {
+
+    //TODO: Whoever does this it should retract or deploy the hooks depending on if they are already deployed or retracted
+    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

@@ -0,0 +1,13 @@
+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 setPercent(double percentage) {
+
+    }
+    public boolean hasScored() {
+        return true;
+    }
+}