Feat: added tramp commands

flamingchickens1540 · Feb 2, 2024 · a7ec007 · a7ec007
1 parent 4529bd8
commit a7ec007
Show file tree

Hide file tree

Showing 4 changed files with 36 additions and 7 deletions.
diff --git a/src/main/java/org/team1540/robot2024/commands/TrampCommand.java b/src/main/java/org/team1540/robot2024/commands/TrampCommand.java
@@ -0,0 +1,28 @@
+package org.team1540.robot2024.commands;
+
+import org.team1540.robot2024.subsystems.tramp.Tramp;
+
+import edu.wpi.first.wpilibj2.command.Command;
+
+public class TrampCommand extends Command {
+
+    private final Tramp tramp;
+
+    public TrampCommand(Tramp tramp) {
+        this.tramp = tramp;
+        addRequirements(tramp);
+    }
+
+    public void getNote(Tramp tramp) {
+        if (tramp.isBeamBreakBlocked()) {
+            tramp.end();
+        } else {
+            tramp.scoreTrampCommand();
+        }
+    }
+
+    public void shootTramp(Tramp tramp) {
+        tramp.scoreTrampCommand();
+    }
+
+}
diff --git a/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java b/src/main/java/org/team1540/robot2024/commands/climb/TrapAndClimbSequence.java
@@ -19,7 +19,7 @@ public TrapAndClimbSequence(Elevator elevator, Hooks hooks, Tramp tramp) {
                 new ElevatorSetpointCommand(elevator, ElevatorState.TRAP),
                 new ParallelDeadlineGroup(
                         new WaitCommand(Constants.Tramp.TRAP_SCORING_TIME_SECONDS),
-                        tramp.scoreInTrapCommand() //TODO: Do whatever to this but not my job
+                        tramp.scoreTrampCommand() //TODO: Do whatever to this but not my job
                 ),
                 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
@@ -20,19 +20,20 @@ public void setPercent(double percentage) {
     public boolean isBeamBreakBlocked() {
         return inputs.breamBreakTripped;
     }
+
     @Override
     public void periodic() {
         io.updateInputs(inputs);
         Logger.processInputs("Tramp", inputs);
     }
 
-    public Command scoreInAmpCommand() {
-        //TODO: Tune the percentage
+    public Command scoreTrampCommand() {
+        // TODO: Tune the percentage
         return Commands.runOnce(() -> setPercent(0.5), this);
     }
-    public Command scoreInTrapCommand() {
-        //TODO: Tune the percentage
-        return Commands.runOnce(() -> setPercent(0.5), this);
+
+    public void end() {
+        io.setVoltage(0);
     }
 
 }
diff --git a/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java b/src/main/java/org/team1540/robot2024/subsystems/tramp/TrampIOSparkMax.java
@@ -23,7 +23,7 @@ public void setVoltage(double volts) {
     }
 
     public void updateInputs(TrampIOInputs inputs) {
-        inputs.breamBreakTripped = !(beamBreak.get()); //I think returns false when broken...
+        inputs.breamBreakTripped = !(beamBreak.get()); //I think returns false when broken... Returns true when broken now.
         inputs.velocityRPM = neorEncoder.getVelocity();
         inputs.appliedVolts = neor.getAppliedOutput() * neor.getBusVoltage();
         inputs.currentAmps = neor.getOutputCurrent();