Vision Testing

Author: JoshuaAN

src/main/java/frc/paths/NewAutoPartOne.java

@@ -122,7 +122,8 @@ public void robotPeriodic() {
     // robot's periodic
     // block in order for anything in the Command-based framework to work.
     CommandScheduler.getInstance().run();
-    mRobotContainer.enableLights();
+    // mRobotContainer.enableLights();
+
   }
 
   /** This function is called once each time the robot enters Disabled mode. */
@@ -146,7 +147,7 @@ public void disabledPeriodic() {
       }
     }
 
-    mRobotContainer.enableLights();
+    mRobotContainer.disableLights();
     mRobotContainer.displaySwitch();
   }
 

src/main/java/frc/paths/NewAutoPartTwo.java

@@ -232,7 +232,8 @@ public void configureButtonBindings() {
 
 //      new JoystickButton(driver, RMZ_E_UP).whenPressed(new ZeroHeading(mDrive));
 
-      new JoystickButton(driver, RMZ_A_IN).whenHeld(new TurnToAngle(mDrive, mLimelight, joysticks));
+      new JoystickButton(driver, RMZ_A_IN).whenHeld(new TurnToAngle(mDrive, mLimelight, joysticks)
+                                                      .alongWith(new TurnOnLEDs(mLimelight)));
 
       new JoystickButton(driver, RMZ_D_IN).whenPressed(new PullTrigger(mShooter));
       new JoystickButton(driver, RMZ_D_IN).whenReleased(new StopTrigger(mShooter));
@@ -351,10 +352,14 @@ public void configureButtonBindings() {
       //                       .alongWith(new TurnOffLEDs(mLimelight))
       //                       .alongWith(new IdleCommand()));
 
-      // xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight));
+      // xBoxX.whenHeld(new VisionShooter(mShooter, mLimelight)
+                          // .alongWith(new TurnOnLEDs(mLimelight))
+                          // .alongWith(new XBoxButtonCommand(X_BOX_X)));
       xBoxX.whenHeld(new MotionProfileTurn(mDrive, Math.PI/2));
 
-      // xBoxX.whenReleased(new StopShooter(mShooter));
+      xBoxX.whenReleased(new StopShooter(mShooter)
+                          .alongWith(new TurnOffLEDs(mLimelight))
+                          .alongWith(new IdleCommand()));
 
       JoystickButton xBoxY = new JoystickButton(operator, X_BOX_Y);
       xBoxY.whenHeld(new PresetFlywheelController(mShooter, "TUR")

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

@@ -14,6 +14,7 @@
 import frc.paths.NewAutoPartOne;
 import frc.paths.NewAutoPartThree;
 import frc.paths.NewAutoPartTwo;
+import frc.robot.Robot;
 import frc.robot.drive.Drivetrain;
 import frc.robot.drive.commands.ResetOdometry;
 import frc.robot.drive.commands.TrajectoryFollower;
@@ -26,6 +27,8 @@
 import frc.robot.shooter.commands.ResetEncoder;
 import frc.robot.shooter.commands.StopShooter;
 import frc.robot.shooter.commands.StopTrigger;
+import frc.robot.status.actions.ImageAction;
+import frc.robot.status.commands.ActionCommand;
 
 // NOTE:  Consider using this command inline, rather than writing a subclass.  For more
 // information, see:
@@ -38,6 +41,7 @@ public NewAuto(Drivetrain drive, Shooter shooter, Intake intake) {
       new ResetEncoder(shooter),
       new ParallelDeadlineGroup(
         new WaitCommand(2.9), 
+        new ActionCommand(new ImageAction(Robot.fiveBallAutoImage, 0.02, ImageAction.FOREVER).brightness(0.7).oscillate()),
         new TrajectoryFollower(drive, new NewAutoPartOne()),
         new FastIntake(intake),
         new SequentialCommandGroup(
@@ -52,21 +56,21 @@ public NewAuto(Drivetrain drive, Shooter shooter, Intake intake) {
       ),
       new WaitCommand(0.35),
       new ParallelDeadlineGroup(
-        new WaitCommand(4.2),
+        new WaitCommand(3.9),
         new TrajectoryFollower(drive, new NewAutoPartThree()),
         new SequentialCommandGroup(
           new WaitCommand(1.25),
           new FlywheelController(shooter, 1795, 76.0)
         ),
         new SequentialCommandGroup(
-          new WaitCommand(3.2),
+          new WaitCommand(2.9),
           new PullTrigger(shooter)
         ),
         new RetractIntake(intake)),
       new ParallelDeadlineGroup(
         new WaitCommand(3.0),
         new TrajectoryFollower(drive, new NewAutoPartFour()),
-        new FlywheelController(shooter, 1795, 76.0),
+        new FlywheelController(shooter, 1785, 75.5),
         new SequentialCommandGroup(
           new StopTrigger(shooter),
           new WaitCommand(1.25),

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

@@ -101,7 +101,7 @@ public Drivetrain() {
   public void periodic() {
     // Update the odometry in the periodic block
     updateOdometry();
-    // map.addPose(m_odometry.getPoseMeters(), Timer.getFPGATimestamp());
+    map.addPose(m_odometry.getPoseMeters(), Timer.getFPGATimestamp());
 
     SmartDashboard.putNumber("Heading", getHeading().getDegrees());
 

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

@@ -28,10 +28,14 @@ public class Shooter extends SubsystemBase {
     {2.95, 1980, 73.25}
   });
 
+  private static double erosion = 0.98;
+
   private boolean hoodDirection;
   int highCurrentCount = 0;
   int lowCurrentCount = 0;
 
+  
+
   private double targetVelocity = 0.0; // Target velocity of the shooter.
   private boolean triggerPull = false;
   private boolean triggerEject = false;
@@ -251,7 +255,7 @@ public ShooterState(double hoodAngle, double velocity) {
   }
 
   public static double lookupVelocity(double distance) {
-    return INTERPOLATION_TABLE.sample(distance)[0];
+    return INTERPOLATION_TABLE.sample(distance)[0] * erosion;
   }
 
   public static double lookupHoodAngle(double distance) {

src/main/java/frc/robot/drive/Drivetrain.java

@@ -64,7 +64,7 @@ void controller() {
         double voltage = flywheel.solveFeedforward(rpm, 0) + flywheelController.calculate(velocity, dt);
         // double voltage = flywheel.solveFeedforward(rpm, 0);
 
-        shooter.setShooterVoltage(voltage);
+        shooter.setShooterVoltage(Math.min(voltage, 11.5));
 
         lastPosition = position;
         lastTime = time;        

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

@@ -107,11 +107,12 @@ def main():
     generator.generate([
                     [-0,0,-2.35],
                     [1.1,0.3,-2.55],
+                    [0.95,0.15,-2.55],
                     ],
                     "NewAutoPartOne")
 
     generator.generate([
-                    [1,0.3,-2.55],
+                    [0.95,0.15,-2.55],
                     [5.2,0.7,-2.275],
                     [5.4,0.9,-2.275],
                     [5.05,0.5,-2.275]