added basic manual controls

basic manual coral scoring controls

Author: arsamulyak

src/main/java/wmironpatriots/Robot.java

@@ -48,7 +48,9 @@ public class Robot extends TimedRobot implements Logged {
   private final CommandScheduler scheduler = CommandScheduler.getInstance();
 
   private final CommandXboxController driveController;
-  private final OperatorController operatorController;
+  private final CommandXboxController operatorController;
+  
+  private final OperatorController operatorController2;
 
   private final Swerve swerve;
   private final Tail tail;
@@ -103,7 +105,9 @@ public Robot() {
 
     // * SUBSYSTEM INIT
     driveController = new CommandXboxController(0);
-    operatorController = new OperatorController(1);
+    operatorController = new CommandXboxController(1);
+
+    operatorController2 = new OperatorController(3);
 
     Optional<SwerveDriveSimulation> swerveSim =
         Robot.isSimulation()
@@ -180,6 +184,7 @@ public Robot() {
     // driveController.a().whileTrue(tail.setTargetPoseCmmd(0.0));
 
     // driveController.x().whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS));
+    /* 
     driveController
         .x()
         .whileTrue(tail.setRollerSpeedCmmd(1.4))
@@ -196,7 +201,78 @@ public Robot() {
         .whileFalse(chute.runChuteSpeedCmmd(0).alongWith(tail.setRollerSpeedCmmd(0)));
 
     driveController.b().whileTrue(elevator.setTargetPoseCmmd(Elevator.POSE_L3));
+    */
     // driveController.x().whileTrue(tail.setRollerSpeedCmmd(1)).onFalse(tail.setRollerSpeedCmmd(0.0));
+
+
+    // * ELEVATOR LEVEL 1 COMMAND
+    operatorController
+      .a()
+        .whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
+        .until(() -> tail.inSetpointRange())
+        .andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L1))
+        .alongWith(tail.setTargetPoseCmmd(Tail.POSE_L1)))
+      .whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
+        .until(() -> elevator.inSetpointRange())
+        .andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
+
+    // * ELEVATOR LEVEL 2 COMMAND
+    operatorController
+      .x()
+        .whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
+        .until(() -> tail.inSetpointRange())
+        .andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L2))
+        .alongWith(tail.setTargetPoseCmmd(Tail.POSE_L2)))
+      .whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
+        .until(() -> elevator.inSetpointRange())
+        .andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
+
+    // * ELEVATOR LEVEL 3 COMMAND
+    operatorController
+      .a()
+        .whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
+        .until(() -> tail.inSetpointRange())
+        .andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L3))
+        .alongWith(tail.setTargetPoseCmmd(Tail.POSE_L3)))
+      .whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
+        .until(() -> elevator.inSetpointRange())
+        .andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
+
+    // * ELEVATOR LEVEL 4 COMMAND
+    operatorController
+      .x()
+        .whileTrue(tail.setTargetPoseCmmd(Tail.POSE_MIN_REVS)
+        .until(() -> tail.inSetpointRange())
+        .andThen(elevator.setTargetPoseCmmd(Elevator.POSE_L4))
+        .alongWith(tail.setTargetPoseCmmd(Tail.POSE_L4)))
+      .whileFalse(elevator.setTargetPoseCmmd(Elevator.POSE_INTAKING)
+        .until(() -> elevator.inSetpointRange())
+        .andThen(tail.setTargetPoseCmmd(Tail.POSE_MAX_REVS)));
+    
+
+    // * INTAKING CORAL COMMAND
+    operatorController.leftBumper()
+      .whileTrue(chute.runChuteSpeedCmmd(Chute.INTAKE_SPEED)
+        .alongWith(tail.setRollerSpeedCmmd(Tail.INTAKING_SPEEDS)))
+      .whileFalse(chute.runChuteSpeedCmmd(0)
+        .alongWith(tail.setRollerSpeedCmmd(0)));
+      
+    // * OUTTAKING CORAL COMMAND
+    operatorController.rightBumper()
+      .whileTrue(chute.runChuteSpeedCmmd(Chute.OUTAKE_SPEED)
+        .alongWith(tail.setRollerSpeedCmmd(Tail.OUTTAKING_SPEEDS)))
+      .whileFalse(chute.runChuteSpeedCmmd(0)
+        .alongWith(tail.setRollerSpeedCmmd(0)));
+
+    
+    // * SCORING CORAL COMMAND
+    driveController
+    .rightBumper()
+      .whileTrue(tail.setRollerSpeedCmmd(Tail.OUTPUTTING_SPEEDS))
+      .whileFalse(tail.setRollerSpeedCmmd(0));
+
+    
+
     // * SUPERSTRUCTURE INIT
     Map<Requests, Trigger> triggerMap = new HashMap<Superstructure.Requests, Trigger>();
 
@@ -206,15 +282,17 @@ public Robot() {
     // * AUTON INIT
     autonChooser = Autonomous.configureAutons(swerve);
     SmartDashboard.putData("Select Auton", autonChooser);
-
+    /* 
     new Superstructure(
         elevator,
         tail,
         chute,
-        triggerMap,
-        operatorController.getBranchTarget(),
-        operatorController.getLevelTarget());
+        triggerMap
+        operatorController2.getBranchTarget(),
+        operatorController2.getLevelTarget());
+  */  
   }
+  
 
   @Override
   public void robotPeriodic() {

src/main/java/wmironpatriots/subsystems/chute/Chute.java

@@ -11,6 +11,7 @@
 
 public abstract class Chute extends SubsystemBase {
   public static final double INTAKE_SPEED = -0.13;
+  public static final double OUTAKE_SPEED = 0.13;
 
   public Command runChuteSpeedCmmd(double speed) {
     return this.run(() -> {});

src/main/java/wmironpatriots/subsystems/tail/Tail.java

@@ -25,9 +25,15 @@ public abstract class Tail implements IronSubsystem {
   public static final double POSE_MIN_REVS = -10.4;
   public static final double POSE_MAX_REVS = 0.1;
 
+  public static final double POSE_L1 = -4;
+  public static final double POSE_L2 = -4;
+  public static final double POSE_L3 = -4;
+  public static final double POSE_L4 = -4;
+
   // Roller speeds
-  public static final double INTAKING_SPEEDS = 5;
-  public static final double OUTTAKING_SPEEDS = 5;
+  public static final double INTAKING_SPEEDS = 1;
+  public static final double OUTTAKING_SPEEDS = -1;
+  public static final double OUTPUTTING_SPEEDS = 2;
 
   public static final double PIVOT_P = 1.5;
   public static final double PIVOT_I = 0.0;
@@ -60,6 +66,8 @@ public Command setTargetPoseCmmd(double poseRes) {
         });
   }
 
+  
+
   /** Runs rollers at specific speed */
   public Command setRollerSpeedCmmd(double speed) {
     return this.run(