Competition Testing

Author: JoshuaAN

src/main/java/frc/paths/FourBallPartThree.java

@@ -21,6 +21,7 @@
 import frc.robot.status.commands.IdleCommand;
 import frc.robot.status.commands.SetColor;
 import frc.robot.status.groups.LEDDemoCG;
+import frc.robot.vision.commands.TurnOffLEDs;
 
 /**
  * The VM is configured to automatically run this class, and to call the
@@ -112,6 +113,9 @@ public void disabledPeriodic() {
         mRobotContainer.configureButtonBindings();
       }
     }
+
+    mRobotContainer.disableLights();
+    mRobotContainer.displaySwitch();
   }
 
   /**

src/main/java/frc/paths/FourBallPartTwo.java

@@ -92,6 +92,11 @@ public class RobotContainer {
   private final Status mStatus = Status.getInstance();
   private final Climber mClimber = new Climber();
 
+  private final DigitalInput fiveBallAuto = new DigitalInput(3);
+  private final DigitalInput twoBallSouthAuto = new DigitalInput(1);
+  private final DigitalInput twoBallEastAuto = new DigitalInput(2);
+  private final DigitalInput fourBallAuto = new DigitalInput(4);
+
   /*
    * private final Indexer mIndexer = new Indexer();
    * private final PowerDistribution mPDP = new PowerDistribution(
@@ -132,11 +137,6 @@ public Command getAutonomousCommand() {
     // return new TrajectoryFollower(mDrive, new CollectSecondBall());
     Command autoCommand = null;
 
-    DigitalInput fiveBallAuto = new DigitalInput(0);
-    DigitalInput twoBallSouthAuto = new DigitalInput(1);
-    DigitalInput twoBallEastAuto = new DigitalInput(2);
-    DigitalInput fourBallAuto = new DigitalInput(4);
-
     try {
       if(!fiveBallAuto.get()){
         autoCommand = new FiveBallAuto(mDrive, mIntake, mShooter);
@@ -156,6 +156,25 @@ public Command getAutonomousCommand() {
     return autoCommand;
   }
 
+  public void displaySwitch() {
+    // try {
+    //   if(!fiveBallAuto.get()){
+    //     SmartDashboard.putNumber("DIO", 3);
+    //   } else if (!twoBallSouthAuto.get()) {
+    //     SmartDashboard.putNumber("DIO", 1);
+    //   } else if (!twoBallEastAuto.get()) {
+    //     SmartDashboard.putNumber("DIO", 2);
+    //   } else if (!fourBallAuto.get()) {
+    //     SmartDashboard.putNumber("DIO", 4);
+    //   } else {
+    //     SmartDashboard.putNumber("DIO", -1);
+    //   }
+    // } catch (Exception e) {
+
+    // }
+    
+  }
+
   public void setLEDs() {
     new SetColor(mStatus, Color.kCornsilk).schedule();
   }

src/main/java/frc/robot/Robot.java

@@ -26,6 +26,7 @@
 import frc.robot.shooter.Shooter;
 import frc.robot.shooter.commands.FlywheelController;
 import frc.robot.shooter.commands.PullTrigger;
+import frc.robot.shooter.commands.ResetEncoder;
 import frc.robot.shooter.commands.ResetHood;
 import frc.robot.shooter.commands.StopShooter;
 import frc.robot.shooter.commands.StopTrigger;
@@ -38,12 +39,13 @@ public class FiveBallAuto extends SequentialCommandGroup {
   public FiveBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
     addCommands(
       new ResetOdometry(drive, new Pose2d(new Translation2d(0, 0), Rotation2d.fromDegrees(-90.0))),
+      new ResetEncoder(shooter),
       new ParallelDeadlineGroup(
         new SequentialCommandGroup(
             new WaitCommand(1.0), // Give shooter time to spin up & hood to move
             new PullTrigger(shooter),
             new WaitCommand(0.5)),
-        new ActionCommand(new ImageAction("THfade.png", 0.01).brightness(0.7)),
+        // new ActionCommand(new ImageAction("THfade.png", 0.01).brightness(0.7)),
         new TrajectoryFollower(drive, new FiveBallPartOne()), // Turn to point at center
         new FlywheelController(shooter, 1810, 77.90)),
     new ParallelDeadlineGroup(

src/main/java/frc/robot/RobotContainer.java

@@ -23,6 +23,7 @@
 import frc.robot.shooter.Shooter;
 import frc.robot.shooter.commands.FlywheelController;
 import frc.robot.shooter.commands.PullTrigger;
+import frc.robot.shooter.commands.ResetEncoder;
 import frc.robot.shooter.commands.ResetHood;
 import frc.robot.shooter.commands.StopShooter;
 import frc.robot.shooter.commands.StopTrigger;
@@ -35,11 +36,12 @@ public class FourBallAuto extends SequentialCommandGroup{
 
     public FourBallAuto(Drivetrain drive, Intake intake, Shooter shooter) {
         addCommands(
-            new ActionCommand(new ImageAction("fade.png",0.02).oscillate().brightness(0.7)),
+            // new ActionCommand(new ImageAction("fade.png",0.02).oscillate().brightness(0.7)),
             new ResetOdometry(drive, new Pose2d(new Translation2d(0,0),Rotation2d.fromDegrees(-90))),
+            new ResetEncoder(shooter),
             new ParallelDeadlineGroup(
                 new WaitCommand(4.25),
-                new ResetHood(shooter),
+                // new ResetHood(shooter),
                 new SequentialCommandGroup(
                     new WaitCommand(1.75),
                     new FlywheelController(shooter, 1800, 78.25)),

src/main/java/frc/robot/auto/groups/FiveBallAuto.java

@@ -112,6 +112,10 @@ public void resetHoodAngle() {
     setHoodPosition(ShooterConstants.kHoodMinAngle);
   }
 
+  public void resetHoodEncoder() {
+    hoodEncoder.setPosition(ShooterConstants.kHoodMinAngle);
+  }
+
   public boolean checkHoodCurrentLimit() {
     if ( ShooterConstants.kHoodStopCurrentLimit < hoodMotor.getOutputCurrent()) {
       highCurrentCount++;

src/main/java/frc/robot/auto/groups/FourBallAuto.java

@@ -0,0 +1,27 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.shooter.commands;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc.robot.shooter.Shooter;
+
+public class ResetEncoder extends CommandBase {
+  Shooter shooter;
+
+  public ResetEncoder(Shooter shooter) {
+    this.shooter = shooter;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    shooter.resetHoodEncoder();
+  }
+
+  @Override
+  public boolean isFinished() {
+    return true;
+  }
+}

src/main/java/frc/robot/shooter/Shooter.java

@@ -61,12 +61,25 @@ def main():
     #                 [6.35,0.0,-2.1]],
     #                 "FourBallPartTwo")
 
+    generator.generate([
+                    [0,0,-1.95],
+                    [2.5,0.85,-1.95],
+                    [5.7,-0.3,-2.1],
+                    [6.15,0.15,-2.1]],
+                    "FourBallPartTwo")
+
     # generator.generate([
     #                 [6.35,0.0,-2.1],
     #                 [2.5,0.85,-2.1],
     #                 [-0.3,-0.3,-1.95]],
     #                 "FourBallPartThree")
 
+    generator.generate([
+                    [6.15,0.15,-2.1],
+                    [2.5,0.85,-2.1],
+                    [0.2,-0.3,-1.95]],
+                    "FourBallPartThree")
+
     # generator.generate([
     #                 [0,0,-1.57],
     #                 [0,0.00000001,-1.85]],
@@ -85,10 +98,10 @@ def main():
     #                 ],
     #                 "FiveBallPartThree")
 
-    generator.generate([
-                    [6.47,0.2,-math.pi+0.7],
-                    [0.10,-0.375,-math.pi+0.9]
-                    ],
-                    "FiveBallPartFour")
+    # generator.generate([
+    #                 [6.47,0.2,-math.pi+0.7],
+    #                 [0.10,-0.375,-math.pi+0.9]
+    #                 ],
+    #                 "FiveBallPartFour")
 
 main()