Renamed scoring commands for coral manipulators

chopshop-166 · Jan 23, 2025 · 08d348f · 08d348f
1 parent dddb9ef
commit 08d348f
Show file tree

Hide file tree

Showing 4 changed files with 41 additions and 10 deletions.
diff --git a/src/main/java/frc/robot/CommandSequences.java b/src/main/java/frc/robot/CommandSequences.java
@@ -26,11 +26,11 @@ public CommandSequences(Drive drive, Led led, AlgaeDestage algaeDestage, Outtake
     }
 
     public Command intake() {
-        return outtake.spinIn();
+        return outtake.intake();
     }
 
     public Command scoreCoral() {
-        return outtake.spinOut();
+        return outtake.score();
     }
 
     public Command setRumble(ButtonXboxController controller, int rumbleAmount) {

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
@@ -115,10 +115,10 @@ public void configureButtonBindings() {
         driveController.leftBumper()
                 .whileTrue(drive.robotCentricDrive());
 
-        driveController.a().onTrue(outtake.spinIn());
-        driveController.b().whileTrue(outtake.spinOut());
-        driveController.x().whileTrue(outtake.reverse());
-        driveController.y().whileTrue(outtake.spinOutL1());
+        driveController.a().onTrue(outtake.intake());
+        driveController.b().whileTrue(outtake.score());
+        driveController.x().whileTrue(outtake.scoreL1());
+        driveController.y().whileTrue(algaeDestage.destageAlgae());
 
     }
 

diff --git a/src/main/java/frc/robot/subsystems/CoralManip.java b/src/main/java/frc/robot/subsystems/CoralManip.java
@@ -1,20 +1,51 @@
 package frc.robot.subsystems;
 
+import static edu.wpi.first.wpilibj2.command.Commands.waitSeconds;
+
 import com.chopshop166.chopshoplib.logging.LoggedSubsystem;
 
 import frc.robot.maps.subsystems.CoralManipMap;
 import frc.robot.maps.subsystems.CoralManipMap.Data;
+import frc.robot.maps.subsystems.OuttakeMap;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class CoralManip extends LoggedSubsystem<Data, CoralManipMap> {
 
+    private final double RELEASE_SPEEDRIGHT = 0.3;
+    private final double RELEASE_SPEEDLEFT = 0.1;
+    private final double INTAKE_SPEED = 0.3;
+    private final double RELEASE_DELAY = 1;
+    private final double DELAY = 0.0;
+
     public CoralManip(CoralManipMap coralManipMap) {
         super(new Data(), coralManipMap);
     }
 
+    public Command scoreL1() {
+        return run(() -> {
+            getData().leftMotor.setpoint = RELEASE_SPEEDLEFT;
+            getData().rightMotor.setpoint = RELEASE_SPEEDRIGHT;
+        }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
+    }
+
+    public Command score() {
+        return run(() -> {
+            getData().leftMotor.setpoint = RELEASE_SPEEDRIGHT;
+            getData().rightMotor.setpoint = RELEASE_SPEEDRIGHT;
+        }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
+    }
+
+    public Command intake() {
+        return run(() -> {
+            getData().leftMotor.setpoint = INTAKE_SPEED;
+            getData().rightMotor.setpoint = INTAKE_SPEED;
+        }).until(() -> getData().gamePieceDetected).andThen(safeStateCmd());
+    }
+
     @Override
     public void safeState() {
-        // no safe state to set
+        getData().leftMotor.setpoint = 0;
+        getData().rightMotor.setpoint = 0;
     }
 
 }
diff --git a/src/main/java/frc/robot/subsystems/Outtake.java b/src/main/java/frc/robot/subsystems/Outtake.java
@@ -21,21 +21,21 @@ public Outtake(OuttakeMap outtakeMap) {
         super(new Data(), outtakeMap);
     }
 
-    public Command spinOutL1() {
+    public Command scoreL1() {
         return run(() -> {
             getData().leftWheel.setpoint = RELEASE_SPEEDLEFT;
             getData().rightWheel.setpoint = RELEASE_SPEEDRIGHT;
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
-    public Command spinOut() {
+    public Command score() {
         return run(() -> {
             getData().leftWheel.setpoint = RELEASE_SPEEDRIGHT;
             getData().rightWheel.setpoint = RELEASE_SPEEDRIGHT;
         }).until(() -> !getData().gamePieceDetected).andThen(waitSeconds(RELEASE_DELAY), safeStateCmd());
     }
 
-    public Command spinIn() {
+    public Command intake() {
         return run(() -> {
             getData().leftWheel.setpoint = INTAKE_SPEED;
             getData().rightWheel.setpoint = INTAKE_SPEED;