Merge branch 'cleanup' into experimental

src/main/deploy/pathplanner/JMove2.path

@@ -1 +1 @@
-{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.465685817447585,"y":2.0602977914816596},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":10.6,"y":2.118171324950245},"prevControl":{"x":10.796770013793191,"y":3.8080785022329553},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]}
+{"waypoints":[{"anchorPoint":{"x":7.66,"y":0.7},"prevControl":null,"nextControl":{"x":7.708937737734877,"y":2.3864441401993526},"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false},{"anchorPoint":{"x":11.0,"y":2.118171324950245},"prevControl":{"x":11.180129214388481,"y":3.8472372199577447},"nextControl":null,"holonomicAngle":90.0,"isReversal":false,"velOverride":null,"isLocked":false}]}

src/main/java/frc4388/robot/Constants.java

@@ -168,7 +168,7 @@ public final class Constants {
     public static final double STORAGE_SPEED = 1.0;//0.9;
   }
   public static final class LEDConstants {
-    public static final int LED_SPARK_ID = 3;
+    public static final int LED_SPARK_ID = 4;//3;
 
     public static final LEDPatterns DEFAULT_PATTERN = LEDPatterns.SOLID_RED;
     public static final LEDPatterns SHOOTING_PATTERN = LEDPatterns.SOLID_GREEN;
@@ -343,6 +343,6 @@ public final class Constants {
     public static final double SHOOTER_CORRECTION = 1.d;
 
     public static final double PIXELS_PER_DEGREE = LIME_HIXELS / H_FOV;
-    public static final double DEGREES_PER_PIXELS = 1 / PIXELS_PER_DEGREE;
+    public static final double DEGREES_PER_PIXEL = 1 / PIXELS_PER_DEGREE;
   }
 }

src/main/java/frc4388/robot/Robot.java

@@ -139,7 +139,7 @@ public class Robot extends TimedRobot {
 
   @Override
   public void disabledPeriodic() {
-    // m_robotContainer.m_robotVisionOdometry.setLEDs(false);
+    m_robotContainer.m_robotVisionOdometry.setLEDs(false);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -26,6 +26,7 @@ import edu.wpi.first.wpilibj2.command.CommandGroupBase;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
+import edu.wpi.first.wpilibj2.command.ParallelRaceGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
@@ -454,14 +455,14 @@ public class RobotContainer {
     );
   }
 
-  private SequentialCommandGroup shoot(double storageRunTime, double timeout) {
+  private ParallelRaceGroup shoot(double storageRunTime, double timeout) {
     return new SequentialCommandGroup(
-      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).withTimeout(timeout),
+      new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
       new ParallelCommandGroup(
-        new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true).until(TrackTarget::isTargetNotLocked),
+        new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
         new RunCommandForTime(new RunCommand(() -> m_robotStorage.runStorage(StorageConstants.STORAGE_SPEED), m_robotStorage), storageRunTime, true)
       )
-    );
+    ).withTimeout(timeout + storageRunTime);
   }
 
   SequentialCommandGroup weirdAutoShootingGroup = new SequentialCommandGroup(new TrackTarget(m_robotTurret, m_robotBoomBoom, m_robotHood, m_robotVisionOdometry, m_robotLED, true),
@@ -492,14 +493,14 @@ public class RobotContainer {
   SequentialCommandGroup extendThenAimTurret() {
     return new SequentialCommandGroup(
       new ExtenderIntakeGroup(m_robotIntake, m_robotExtender), 
-      new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID((180.0 / Math.PI) * Math.atan2(-(82.83 / 2.00) + 15.56, -(219.25 / 2.00) - 40.44 + 10.00)), m_robotTurret), 1.0, true) // TODO: optimize time
+      new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-180), m_robotTurret), 0.5, true) // TODO: optimize time
     );
   }
   
   ParallelDeadlineGroup idleDrumUntilShootingFirstBall() {
     return new ParallelDeadlineGroup(
       extendThenAimTurret(), 
-      new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom)
+      new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(9000), m_robotBoomBoom)
     );
   }
 
@@ -523,7 +524,7 @@ public class RobotContainer {
   // ParallelCommandGroup intakeWithPath2AndTrackTarget = new ParallelCommandGroup(intakeWithPath2, weirdAutoShootingGroup3);
 
   ParallelDeadlineGroup intakeWithPath2AndIdleShooterAndAimTurret = new ParallelDeadlineGroup(
-    intakeWithPath2(4.2),
+    intakeWithPath2(2.8),
     new RunCommand(() -> m_robotBoomBoom.runDrumShooterVelocityPID(8000), m_robotBoomBoom), 
     new RunCommandForTime(new RunCommand(() -> m_robotTurret.runShooterRotatePID(-120), m_robotTurret), 0.7, true)
   );
@@ -556,10 +557,11 @@ public class RobotContainer {
   // ! THREE BALL AUTO (ASSUMES ROBOT IS FACING DIRECTLY TOWARDS THE FIRST BALL OUTSIDE THE TARMAC, BUMPERS FLUSH WITH THE EDGE)
   SequentialCommandGroup threeBallAuto = new SequentialCommandGroup(
     idleDrumUntilShootingFirstBall(),
-    shoot(1.0), // TODO: optimize time
+    shoot(0.8), // TODO: optimize time
     brakeStorage(0.1),
-    intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
+    intakeWithPathAndTrackTarget,
-    shoot(2.3), // TODO: optimize time
+    // intakeWithPath1(3.0), // * this line and the one underneath it can be replaced with intakeWithPathAndTrackTarget
+    shoot(0.8), // TODO: optimize time
     brakeStorage(0.1),
     intakeWithPath2AndIdleShooterAndAimTurret,
     shoot(4.0), // TODO: optimize time

src/main/java/frc4388/robot/RobotMap.java

@@ -16,6 +16,7 @@ import com.revrobotics.CANSparkMaxLowLevel.MotorType;
 
 import edu.wpi.first.util.sendable.SendableRegistry;
 import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.PWM;
 import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import frc4388.robot.Constants.ClimberConstants;
@@ -47,6 +48,7 @@ public class RobotMap {
 
   /* LED Subsystem */
   public final Spark LEDController = new Spark(LEDConstants.LED_SPARK_ID);
+//   public final PWM newLED = new Servo(LEDConstants.LED_SPARK_ID);
 
   void configureLEDMotorControllers() {}
 

src/main/java/frc4388/robot/commands/ShooterCommands/TrackTarget.java

@@ -105,6 +105,7 @@ public class TrackTarget extends CommandBase {
     }
     // points = getFakePoints();
     //// points = filterPoints(points);
+
     Point average = VisionOdometry.averagePoint(points);
     // double position = m_turret.m_boomBoomRotateEncoder.getPosition();
 
@@ -116,7 +117,6 @@ public class TrackTarget extends CommandBase {
     //                           RobotContainer.getDriverController().getRightX(), RobotContainer.getDriverController().getRightY(),
     //                           true);
 
-    
     double regressedDistance = getDistance(average.y);
 
     // ! offset trig solution

src/main/java/frc4388/robot/subsystems/LED.java

@@ -6,6 +6,8 @@ package frc4388.robot.subsystems;
 
 import java.util.logging.Logger;
 
+import edu.wpi.first.wpilibj.PWM;
+import edu.wpi.first.wpilibj.Servo;
 import edu.wpi.first.wpilibj.motorcontrol.Spark;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.LEDConstants;
@@ -18,6 +20,7 @@ import frc4388.utility.LEDPatterns;
 public class LED extends SubsystemBase {
 
   private LEDPatterns m_currentPattern;
+  // private PWM newLED;
   private Spark m_LEDController;
 
   /**
@@ -25,6 +28,7 @@ public class LED extends SubsystemBase {
    */
   public LED(Spark LEDController){
     m_LEDController = LEDController;
+    // this.newLED = newLED;
     setPattern(LEDConstants.DEFAULT_PATTERN);
     updateLED();
     Logger.getLogger(LED.class.getSimpleName()).finer("In the Beginning, there was Joe.\nAnd he said, 'Let there be LEDs.'\nAnd it was good.");
@@ -39,6 +43,7 @@ public class LED extends SubsystemBase {
    * Add your docs here.
    */
   public void updateLED(){
+    // newLED.setRaw((int) m_currentPattern.percentToPWM());
     m_LEDController.set(m_currentPattern.getValue());
   }
 
@@ -47,6 +52,7 @@ public class LED extends SubsystemBase {
    */
   public void setPattern(LEDPatterns pattern){
     m_currentPattern = pattern;
+    // newLED.setRaw((int) m_currentPattern.percentToPWM());
     m_LEDController.set(m_currentPattern.getValue());
   }
 

src/main/java/frc4388/robot/subsystems/VisionOdometry.java

@@ -45,7 +45,7 @@ public class VisionOdometry extends SubsystemBase {
   public VisionOdometry(SwerveDrive drive, Turret shooter) {
     // do{
       m_camera = new PhotonCamera(VisionConstants.NAME);
-    // }while(m_camera.getLatestResult().getLatencyMillis() == 0.d);
+    // } while (m_camera.getLatestResult().getLatencyMillis() == 0.d);
     m_drive = drive;
     m_shooter = shooter;
 

src/main/java/frc4388/utility/LEDPatterns.java

@@ -42,4 +42,8 @@ public enum LEDPatterns {
   public float getValue() {
     return id;
   }
+
+  public float percentToPWM() {
+    return (1000 + (getValue() * 1000));
+  }
 }