Changed bus voltage to getAppliedOutput

.gitignore

@@ -164,3 +164,6 @@ bin/
 # Logs
 *.wpilog
 *.log
+
+# SYSID 2022 CONFIG
+config.json

src/main/java/com/stuypulse/robot/Robot.java

@@ -6,6 +6,7 @@
 
 import com.stuypulse.robot.commands.leds.LEDApplyPattern;
 
+import edu.wpi.first.networktables.NetworkTableInstance;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.wpilibj.LEDPattern;
 import edu.wpi.first.wpilibj.TimedRobot;
@@ -30,6 +31,9 @@ public void robotInit() {
     @Override
     public void robotPeriodic() {
         CommandScheduler.getInstance().run();
+
+        //TODO: REMOVE THE LINE BELOW DURING COMPETITIONS TO ENSURE NO LAG OCCURS, THIS IS HERE FOR LIMELIGHT TESTING
+        NetworkTableInstance.getDefault().flush();
     }
 
     /*********************/

src/main/java/com/stuypulse/robot/RobotContainer.java

@@ -1,36 +1,39 @@
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.commands.auton.combinations.CoralgaeAuton;
 import com.stuypulse.robot.commands.auton.combinations.PushBackwardsCoralAuton;
-import com.stuypulse.robot.commands.auton.coral.DoubleCoralAuton;
+import com.stuypulse.robot.commands.auton.coral.DoubleCenterCoralAuton;
 import com.stuypulse.robot.commands.auton.coral.SingleCoralAuton;
 import com.stuypulse.robot.commands.auton.misc.DoNothingAuton;
 import com.stuypulse.robot.commands.auton.misc.MobilityAuton;
 import com.stuypulse.robot.commands.auton.push.PushBackwardsAuton;
 import com.stuypulse.robot.commands.auton.push.PushForwardsAuton;
 import com.stuypulse.robot.commands.climb.ClimbToClimb;
 import com.stuypulse.robot.commands.climb.ClimbToStow;
+import com.stuypulse.robot.commands.drive.DriveArcade;
 import com.stuypulse.robot.commands.drive.DriveDefault;
-import com.stuypulse.robot.commands.leds.LEDApplyPattern;
-import com.stuypulse.robot.commands.leds.LEDDeafultCommand;
 import com.stuypulse.robot.commands.pivot.PivotLower;
 import com.stuypulse.robot.commands.pivot.PivotRaise;
+import com.stuypulse.robot.commands.pivot.PivotReseatCoral;
 import com.stuypulse.robot.commands.pivot.PivotResetAngle;
 import com.stuypulse.robot.commands.pivot.PivotStop;
 import com.stuypulse.robot.commands.pivot.PivotToAlgaeIntake;
 import com.stuypulse.robot.commands.pivot.PivotToAlgaeStow;
 import com.stuypulse.robot.commands.pivot.PivotToCoralStow;
-import com.stuypulse.robot.commands.pivot.PivotToDirection;
 import com.stuypulse.robot.commands.pivot.SetPivotControlMode;
+import com.stuypulse.robot.commands.pivot.PivotCombos.PivotCoralScore;
+import com.stuypulse.robot.commands.pivot.PivotCombos.PivotLolipopAlgeaIntake;
+import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeHold;
 import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeIntake;
 import com.stuypulse.robot.commands.pivot.roller.PivotAlgaeOuttake;
-import com.stuypulse.robot.commands.pivot.roller.PivotCoralOuttake;
 import com.stuypulse.robot.commands.pivot.roller.PivotHoldCoral;
 import com.stuypulse.robot.commands.pivot.roller.PivotRollerStop;
-
+import com.stuypulse.robot.commands.vision.VisionDriveToNearestApriltag;
 import com.stuypulse.robot.subsystems.drivetrain.Drivetrain;
 import com.stuypulse.robot.subsystems.leds.LEDController;
 import com.stuypulse.robot.subsystems.pivot.Pivot;
 import com.stuypulse.robot.subsystems.pivot.Pivot.PivotControlMode;
+import com.stuypulse.robot.subsystems.vision.LimelightVision;
 import com.stuypulse.robot.constants.Ports;
 
 import com.stuypulse.stuylib.input.Gamepad;
@@ -50,6 +53,7 @@ public class RobotContainer {
     private final LEDController ledSubsystem = LEDController.getInstance();
     private final Drivetrain driveSubsystem = Drivetrain.getInstance();
     private final Pivot pivot = Pivot.getInstance();
+    private final LimelightVision limelightVision = LimelightVision.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
@@ -78,66 +82,83 @@ private void configureDefaultCommands() {
 
     private void configureButtonBindings() {
         // BUTTONS
-        driver.getTopButton()
-                .whileTrue(new PivotRollerStop())
-                .whileTrue(new PivotCoralOuttake())
+        driver.getTopButton()                                                   // Coral Score
+                .onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
+                .whileTrue(new PivotCoralScore())
+                .onFalse(new PivotToCoralStow())
                 .onFalse(new PivotHoldCoral());
-        driver.getLeftButton()
+        driver.getLeftButton()                                                  // Climb
                 .whileTrue(new ClimbToClimb());
-        driver.getRightButton()
+        driver.getRightButton()                                                 // Stow Climb
                 .whileTrue(new ClimbToStow());
-        driver.getBottomButton()
+        driver.getBottomButton()                                                
                 .whileTrue(new PivotAlgaeOuttake())
-                .onFalse(new PivotRollerStop());
-        // driver.getBottomButton()
-        // .whileTrue(new VisionAlignToReef())
+                .onFalse(new PivotHoldCoral());
 
         // TRIGGERS
-        driver.getRightTriggerButton()
+        driver.getRightTriggerButton()                                          // Pivot Up
                 .onTrue(new SetPivotControlMode(PivotControlMode.MANUAL))
                 .whileTrue(new PivotRaise())
                 .onFalse(new PivotStop());
-        driver.getLeftTriggerButton()
+        driver.getLeftTriggerButton()                                           // Pivot Down
                 .onTrue(new SetPivotControlMode(PivotControlMode.MANUAL))
                 .whileTrue(new PivotLower())
                 .onFalse(new PivotStop());
 
         // BUMPERS
-        driver.getRightBumper()
-                .whileTrue(new PivotAlgaeOuttake())
-                .onFalse(new PivotHoldCoral());
-        driver.getLeftBumper()
-                .whileTrue(new PivotAlgaeIntake())
-                .onFalse(new PivotRollerStop());
+        driver.getRightBumper()                                                  // Algae Outtake
+               .whileTrue(new PivotAlgaeOuttake())
+               .onFalse(new PivotHoldCoral());
+        driver.getLeftBumper()                                                   // Algae Intake
+               .whileTrue(new PivotAlgaeIntake())
+               .onFalse(new PivotAlgaeHold());
 
         // DPAD
-        driver.getDPadRight()
+        driver.getDPadRight()                                                   // Pivot to Hold Algae
                 .onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
-                .onTrue(new PivotToAlgaeStow());
-        driver.getDPadDown()
+                .onTrue(new PivotToAlgaeStow())
+                .onTrue(new PivotAlgaeHold());
+        driver.getDPadDown()                                                    // Pivot to Intake Algae
                 .onTrue(new SetPivotControlMode(PivotControlMode.USING_STATES))
                 .onTrue(new PivotToAlgaeIntake());
-
+        driver.getDPadLeft()                                                    // Pivot to Lollipop Intake
+                .onTrue(new SetPivotControlMode(Pivot.PivotControlMode.USING_STATES))
+                .onTrue(new PivotLolipopAlgeaIntake())
+                .onFalse(new PivotRollerStop())
+                .onFalse(new PivotToAlgaeStow());
+
         // MENU BUTTONS
-        driver.getRightMenuButton()
+        driver.getRightMenuButton()                                             // Reset Relative Encoder
                 .onTrue(new PivotResetAngle());
+        driver.getLeftMenuButton()                                              // Reseat Coral
+                .onTrue(new PivotReseatCoral());
+
+        //JOYSTICK BUTTONS
+        driver.getLeftStickButton()                                             // Drive to Nearest April Tag
+                .whileTrue(new VisionDriveToNearestApriltag())
+                .onFalse(new DriveArcade(0, 0, true));
     }
 
     /**************/
     /*** AUTONS ***/
     /**************/
 
     public void configureAutons() {
-        autonChooser.setDefaultOption("Misc - Mobility", new MobilityAuton());
+        // Coral
+        autonChooser.setDefaultOption("Coral Only - Single", new SingleCoralAuton());
+        autonChooser.addOption("Coral Only - Center Double", new DoubleCenterCoralAuton());
+
+        // Misc
         autonChooser.addOption("Misc - Do Nothing", new DoNothingAuton());
-
-        autonChooser.addOption("Coral Only - Single", new SingleCoralAuton());
-        autonChooser.addOption("Coral Only - Double", new DoubleCoralAuton());
-
+        autonChooser.addOption("Misc - Mobility", new MobilityAuton());
+
+        // Push
         autonChooser.addOption("Push Only - Forwards", new PushForwardsAuton());
         autonChooser.addOption("Push Only - Backwards", new PushBackwardsAuton());
 
+
         autonChooser.addOption("Combination - Coral w/ Push", new PushBackwardsCoralAuton());
+        autonChooser.addOption("Combination - Coralgae", new CoralgaeAuton());
 
         SmartDashboard.putData("Autonomous", autonChooser);
     }

src/main/java/com/stuypulse/robot/commands/auton/combinations/CoralgaeAuton.java

@@ -0,0 +1,33 @@
+package com.stuypulse.robot.commands.auton.combinations;
+
+import com.stuypulse.robot.commands.drive.DriveTank;
+import com.stuypulse.robot.commands.pivot.PivotCombos.PivotCoralScore;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+/**
+ * attempt to score two coral
+ * 
+ * @author Sebastian Waldman
+ */
+public class CoralgaeAuton extends SequentialCommandGroup {
+    public CoralgaeAuton() {
+        addCommands(
+            new DriveTank(-.45, -.45, true)
+                .withTimeout(2.2),
+            new DriveTank(0, 0, false)
+                .withTimeout(0.1),
+            new PivotCoralScore()
+                .withTimeout(2.0),
+            new DriveTank(.5,.5,true)
+                .withTimeout(.5),
+            new DriveTank(.7,.3,true)
+                .withTimeout(.45),
+            new DriveTank(-.7,-.7,true)
+                .withTimeout(1.15),
+            new DriveTank(-.7,-.3,true)
+                .withTimeout(.5)
+
+        );
+    }
+}

src/main/java/com/stuypulse/robot/commands/auton/combinations/PushBackwardsCoralAuton.java

@@ -1,13 +1,10 @@
 package com.stuypulse.robot.commands.auton.combinations;
 
 import com.stuypulse.robot.commands.drive.DriveTank;
-import com.stuypulse.robot.commands.leds.LEDApplyPattern;
 import com.stuypulse.robot.commands.pivot.roller.PivotCoralOuttake;
 import com.stuypulse.robot.commands.pivot.roller.PivotRollerStop;
-import com.stuypulse.robot.constants.Settings.LEDPatterns;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 
 /**
  * Push alliance partner's robot backwards then score a coral
@@ -17,15 +14,16 @@
 public class PushBackwardsCoralAuton extends SequentialCommandGroup {
     public PushBackwardsCoralAuton() {
         addCommands(
-            new LEDApplyPattern(LEDPatterns.PUSH_BACKWARDS_SINGLE_L1_AUTON),
-            new DriveTank(-.75, -.75, true),
-            new WaitUntilCommand(2.00),
-            new DriveTank(.3, .3, true),
-            new WaitUntilCommand(10),
-            new DriveTank(0, 0, true),
-            new PivotCoralOuttake(),
-            new WaitUntilCommand(1),
+            new DriveTank(.75, .75, true)
+                .withTimeout(2),
+            new DriveTank(-.3, -.3, true)
+                .withTimeout(10),
+            new DriveTank(0, 0, true)
+                .withTimeout(0.1),
+            new PivotCoralOuttake()
+                .withTimeout(3),
             new PivotRollerStop()
+                .withTimeout(0.1)
         );
     }
 }

src/main/java/com/stuypulse/robot/commands/auton/coral/DoubleCenterCoralAuton.java

@@ -0,0 +1,50 @@
+package com.stuypulse.robot.commands.auton.coral;
+
+import com.stuypulse.robot.commands.drive.DriveTank;
+import com.stuypulse.robot.commands.pivot.PivotCombos.PivotCoralScore;
+import com.stuypulse.robot.commands.pivot.roller.PivotHoldCoral;
+
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
+
+/**
+ * attempt to score two coral
+ * 
+ * @author Sebastian Waldman
+ */
+public class DoubleCenterCoralAuton extends SequentialCommandGroup {
+    public DoubleCenterCoralAuton() {
+        addCommands(
+            new DriveTank(-.45, -.45, true)
+                .withTimeout(2.2),
+            new DriveTank(0, 0, false)
+                .withTimeout(0.1),
+            new PivotCoralScore()
+                .withTimeout(2),
+            new PivotHoldCoral()
+                .withTimeout(0.1),
+            new DriveTank(.5,.5,true)
+                .withTimeout(.5),
+            new DriveTank(.7,.3,true)
+                .withTimeout(.445),
+            new DriveTank(-.7,-.7,true)
+                .withTimeout(0.95),
+            new DriveTank(-.7,-.3,true)
+                .withTimeout(.65),
+            new DriveTank(-.7,-.7,true)
+                .withTimeout(1.2),
+            new DriveTank(-.3,-.7,true) 
+                .withTimeout(.535),
+            new DriveTank(-.5,-.5,true)
+                .withTimeout(1.65),
+            new DriveTank(0,0,true)
+                .withTimeout(1.2),
+            new DriveTank(0.6,0.6,true)
+                .withTimeout(0.4),
+            new DriveTank(.5,-.7,true)
+                .withTimeout(0.75),
+            new DriveTank(-.6,-.6,true)
+                .withTimeout(0.2)
+
+        );
+    }
+}

src/main/java/com/stuypulse/robot/commands/auton/coral/SingleCoralAuton.java

@@ -4,13 +4,10 @@
 /**************************************************************/
 package com.stuypulse.robot.commands.auton.coral;
 
-import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 
 import com.stuypulse.robot.commands.drive.DriveTank;
-import com.stuypulse.robot.commands.leds.LEDApplyPattern;
-import com.stuypulse.robot.commands.pivot.roller.PivotCoralOuttake;
-import com.stuypulse.robot.constants.Settings.LEDPatterns;
+import com.stuypulse.robot.commands.pivot.PivotCombos.PivotCoralScore;
 
 /**
  * attempts to score one coral
@@ -21,11 +18,12 @@
 public class SingleCoralAuton extends SequentialCommandGroup {
     public SingleCoralAuton() {
         addCommands(
-            new LEDApplyPattern(LEDPatterns.SINGLE_L1_AUTON),
-            new DriveTank(.25, .25, true),
-            new WaitUntilCommand(7.00),
-            new DriveTank(0, 0, true),
-            new PivotCoralOuttake()
+            new DriveTank(-.45, -.45, true)
+                .withTimeout(2.5),
+            new DriveTank(0, 0, false)
+                .withTimeout(0.1),
+            new PivotCoralScore()
+                 .withTimeout(5.0)
         );
     }
 }