Merge branch 'main' into shooting-interpolation

# Conflicts:
#	src/main/java/frc/robot/subsystems/FlywheelSubsystem.java

src/main/java/frc/robot/Constants.java

@@ -22,7 +22,7 @@
 
 import frc.robot.helper.shooter.ShooterPreset;
 import frc.robot.helper.shooter.TrainingDataPoint;
-import frc.robot.subsystems.FlywheelSubsystem.ShooterLocationPreset;
+import frc.robot.subsystems.ShooterSubsystem.ShooterLocationPreset;
 
 import java.util.List;
 
@@ -237,9 +237,9 @@ public static class IDConstants {
         public static final int I2C_COLOR_SENSOR_FIXED_ADDRESS = 0x52;
 
 
-        public static final byte BALL_COLOR_SENSOR_MUX_PORT = 3;
-        public static final byte RIGHT_ALIGN_COLOR_SENSOR_MUX_PORT = 2;
-        public static final byte LEFT_ALIGN_COLOR_SENSOR_MUX_PORT = 1;
+        public static final byte BALL_COLOR_SENSOR_MUX_PORT = 6;
+        public static final byte RIGHT_ALIGN_COLOR_SENSOR_MUX_PORT = 3;
+        public static final byte LEFT_ALIGN_COLOR_SENSOR_MUX_PORT = 5;
 
         // PWM
         public static final int LED_STRIP_PWM_PORT = 0;

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

@@ -7,11 +7,19 @@
 
 package frc.robot;
 
+import com.revrobotics.ColorSensorV3;
+import edu.wpi.first.util.datalog.DataLog;
+import edu.wpi.first.wpilibj.DataLogManager;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.helper.logging.RobotLogger;
+import frc.robot.subsystems.ColorsensorTestSubsystem;
+
+import java.awt.*;
+import java.util.logging.Logger;
 
 public class Robot extends TimedRobot {
   private static final RobotLogger logger = new RobotLogger(Robot.class.getCanonicalName());
@@ -23,7 +31,7 @@ public class Robot extends TimedRobot {
   public void robotInit() {
     RobotLogger.init();
     robotContainer = new RobotContainer();
-    LiveWindow.disableAllTelemetry();
+    SmartDashboard.putData(robotContainer.getCommandChooser());
   }
 
   @Override
@@ -36,22 +44,18 @@ public void disabledInit() {
     logger.info("Robot Disabled");
   }
 
-  @Override
-  public void disabledPeriodic() {}
-
   @Override
   public void autonomousInit() {
     logger.info("Auto Enabled");
     robotContainer.resetPose();
     autonomousCommand = robotContainer.getAutonomousCommand();
 
     if (autonomousCommand != null) {
-      autonomousCommand.schedule(false);
+      autonomousCommand.schedule();
     }
   }
 
-  @Override
-  public void autonomousPeriodic() { }
+
 
   @Override
   public void teleopInit() {
@@ -61,15 +65,10 @@ public void teleopInit() {
     }
   }
 
-  @Override
-  public void teleopPeriodic() { }
 
   @Override
   public void testInit() {
     logger.info("Test Enabled");
     CommandScheduler.getInstance().cancelAll();
   }
-
-  @Override
-  public void testPeriodic() {}
 }

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

@@ -7,11 +7,10 @@
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.CommandScheduler;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.Button;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -29,12 +28,11 @@
 import frc.robot.commands.shooter.*;
 import frc.robot.commands.transfer.TransferIndexForward;
 import frc.robot.commands.transfer.TransferManualReverse;
-import frc.robot.commands.transfer.TransferShootForward;
 import frc.robot.hardware.Limelight;
 import frc.robot.helper.ControllerUtil;
 import frc.robot.helper.DPadButton;
 import frc.robot.helper.JoystickAnalogButton;
-import frc.robot.subsystems.FlywheelSubsystem.ShooterLocationPreset;
+import frc.robot.subsystems.ShooterSubsystem.ShooterLocationPreset;
 import frc.robot.subsystems.*;
 
 import java.awt.Robot;
@@ -56,7 +54,7 @@ public class RobotContainer {
     public SwerveDrive drivetrainSubsystem = null;
     private IntakeSubsystem intakeSubsystem = null;
 
-    private FlywheelSubsystem flywheelSubsystem = null;
+    private ShooterSubsystem flywheelSubsystem = null;
     private TransferSubsystem transferSubsystem = null;
 
     private HangerSubsystem hangerSubsystem = null;
@@ -68,7 +66,8 @@ public class RobotContainer {
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
     public RobotContainer() {
-       // CommandScheduler.getInstance().schedule(new PDHFaultWatcher());
+        LiveWindow.disableAllTelemetry();
+        LiveWindow.setEnabled(false);
 
 
         // Initialize Active Subsystems
@@ -127,7 +126,7 @@ private void initializeDrivetrain() {
     }
 
     private void initializeShooter() {
-        this.flywheelSubsystem = new FlywheelSubsystem();
+        this.flywheelSubsystem = new ShooterSubsystem();
         TransferSubsystem.flywheelSubsystem = flywheelSubsystem;
     }
 
@@ -220,8 +219,8 @@ private void configureShooter() {
         if (TRANSFER) {
             new Button(() -> transferSubsystem.getCurrentBallCount() >= MAX_BALL_COUNT).whenPressed(new WaitAndVibrateCommand(driverController, 0.1, 0.1));
         }
-        new Button(() -> flywheelSubsystem.isAtSetPoint(flywheelSubsystem.getCurrentTargetSpeed())).whenPressed(new WaitAndVibrateCommand(operatorController, 0.5));
 
+        // Flywheel Vibration from the SetShooterPIDVelocityFromStateCommand
     }
 
     private void configureTransfer() {

src/main/java/frc/robot/auto/AutoChooser.java

@@ -4,17 +4,17 @@
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import frc.robot.commands.drivetrain.DefaultDriveCommandRobotOriented;
 import frc.robot.commands.shooter.ZeroHoodMotorCommand;
-import frc.robot.subsystems.FlywheelSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.SwerveDrive;
 import frc.robot.subsystems.TransferSubsystem;
 
 public class AutoChooser {
     private static SendableChooser<Command> autoChooser;
     private static TrajectoryFactory trajectoryFactory;
-    private static FlywheelSubsystem flywheelSubsystem;
+    private static ShooterSubsystem flywheelSubsystem;
 
-    public static SendableChooser<Command> getDefaultChooser(SwerveDrive drive, IntakeSubsystem intake, FlywheelSubsystem flywheel, TransferSubsystem transfer) {
+    public static SendableChooser<Command> getDefaultChooser(SwerveDrive drive, IntakeSubsystem intake, ShooterSubsystem flywheel, TransferSubsystem transfer) {
         trajectoryFactory = trajectoryFactory == null ? new TrajectoryFactory(drive) : trajectoryFactory;
         flywheelSubsystem = flywheel;
         Paths.initialize(drive, intake, flywheel, transfer);

src/main/java/frc/robot/auto/Paths.java

@@ -2,15 +2,14 @@
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj2.command.*;
-import frc.robot.commands.drivetrain.AutoAlignDriveContinuousCommand;
 import frc.robot.commands.drivetrain.AutoAlignInPlaceCommand;
 import frc.robot.commands.intake.IntakeOn;
 import frc.robot.commands.shooter.SetShooterPIDVelocityFromState;
 import frc.robot.commands.transfer.TransferIndexForward;
 import frc.robot.helper.auto.AutoCommandMarker;
 import frc.robot.helper.auto.AutoCommandRunner;
 import frc.robot.helper.shooter.ShooterState;
-import frc.robot.subsystems.FlywheelSubsystem;
+import frc.robot.subsystems.ShooterSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.SwerveDrive;
 import frc.robot.subsystems.TransferSubsystem;
@@ -21,10 +20,10 @@ public class Paths {
     private static SwerveDrive driveSubsystem;
     private static TrajectoryFactory trajectoryFactory;
     private static IntakeSubsystem intakeSubsystem;
-    private static FlywheelSubsystem flywheelSubsystem;
+    private static ShooterSubsystem flywheelSubsystem;
     private static TransferSubsystem transferSubsystem;
 
-    public static void initialize(SwerveDrive drive, IntakeSubsystem intake, FlywheelSubsystem flywheel, TransferSubsystem transfer) {
+    public static void initialize(SwerveDrive drive, IntakeSubsystem intake, ShooterSubsystem flywheel, TransferSubsystem transfer) {
         trajectoryFactory = trajectoryFactory == null ? new TrajectoryFactory(drive) : trajectoryFactory;
         driveSubsystem = drive;
         intakeSubsystem = intake;