Works

simgui.json

@@ -2,10 +2,17 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/SmartDashboard/Auto Chooser": "String Chooser",
       "/SmartDashboard/Field": "Field2d"
     },
     "windows": {
       "/SmartDashboard/Field": {
+        "bottom": 1476,
+        "height": 8.210550308227539,
+        "left": 150,
+        "right": 2961,
+        "top": 79,
+        "width": 16.541748046875,
         "window": {
           "visible": true
         }

src/main/deploy/pathplanner/autos/New Auto.auto

@@ -0,0 +1,50 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3470318873095533,
+      "y": 5.5216374597852385
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Intake"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "CenterNote"
+                }
+              }
+            ]
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "Shoot"
+          }
+        }
+      ]
+    }
+  },
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Center4Note.path → src/main/deploy/pathplanner/paths/CenterNote.path

@@ -46,59 +46,7 @@
   ],
   "rotationTargets": [],
   "constraintZones": [],
-  "eventMarkers": [
-    {
-      "name": "ShootPreloaded",
-      "waypointRelativePos": 0.0,
-      "command": {
-        "type": "race",
-        "data": {
-          "commands": [
-            {
-              "type": "named",
-              "data": {
-                "name": "Shoot"
-              }
-            }
-          ]
-        }
-      }
-    },
-    {
-      "name": "IntakeNote2",
-      "waypointRelativePos": 0.05,
-      "command": {
-        "type": "deadline",
-        "data": {
-          "commands": [
-            {
-              "type": "named",
-              "data": {
-                "name": "Intake"
-              }
-            }
-          ]
-        }
-      }
-    },
-    {
-      "name": "ShootNote2",
-      "waypointRelativePos": 2.0,
-      "command": {
-        "type": "deadline",
-        "data": {
-          "commands": [
-            {
-              "type": "named",
-              "data": {
-                "name": "Shoot"
-              }
-            }
-          ]
-        }
-      }
-    }
-  ],
+  "eventMarkers": [],
   "globalConstraints": {
     "maxVelocity": 3.0,
     "maxAcceleration": 3.0,

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

@@ -15,6 +15,7 @@
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj.XboxController.Button;
+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.InstantCommand;
@@ -53,10 +54,23 @@ public class RobotContainer {
 
   private final XboxController m_driverController = new XboxController(IOConstants.kDriverControllerPort);
 
+  private final SendableChooser<Command> autoChooser;
+
   /**
    * The container for the robot. Contains subsystems, IO devices, and commands.
    */
   public RobotContainer() {
+    NamedCommands.registerCommand("Shoot",
+        new SequentialCommandGroup(
+          new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Shooting),
+          new ParallelDeadlineGroup(new WaitCommand(1), new NoteOuttakeCommand(m_intakeSubsystem)), 
+          new ShooterSetSpeedCommand(m_shooterSubsystem, ShootSpeed.Off)));
+
+    NamedCommands.registerCommand("Intake",
+      new SequentialCommandGroup(
+          new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Extended),
+          new NoteIntakeCommand(m_intakeSubsystem),
+          new IntakeArmPositionCommand(m_intakeSubsystem, ArmPosition.Retracted)));
 
     AutoBuilder.configureHolonomic(m_robotDrive::getPose, m_robotDrive::resetOdometry,
         m_robotDrive::getChassisSpeeds,
@@ -72,22 +86,16 @@ public RobotContainer() {
             new ReplanningConfig(true, true)),
         () -> false, m_robotDrive);
 
-    NamedCommands.registerCommand("Shoot",
-        new SequentialCommandGroup(new InstantCommand(() -> SmartDashboard.putBoolean("shooting", true)),
-            new WaitCommand(2),
-                new InstantCommand(() -> SmartDashboard.putBoolean("shooting", false))));
-
-    NamedCommands.registerCommand("Intake",
-        new SequentialCommandGroup(new InstantCommand(() -> SmartDashboard.putBoolean("Intaking", true)),
-            new WaitCommand(2),
-                new InstantCommand(() -> SmartDashboard.putBoolean("Intaking", false))));
     // new SequentialCommandGroup(new ShooterSetSpeedCommand(m_shooterSubsystem,
     // ShootSpeed.Shooting),
     // new ParallelDeadlineGroup(new WaitCommand(0.5), new
     // NoteOuttakeCommand(m_intakeSubsystem))));
 
     m_visionSubsystem.addConsumer(m_robotDrive::addVisionMeasurement);
 
+    autoChooser = AutoBuilder.buildAutoChooser();
+    SmartDashboard.putData("Auto Chooser", autoChooser);
+
     // Configure the trigger bindings
     configureBindings();
 
@@ -180,16 +188,17 @@ public void resetAllSubsystems() {
    * @return the command to run in autonomous
    */
   public Command getAutonomousCommand() {
-    PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note");
+    // PathPlannerPath path = PathPlannerPath.fromPathFile("Center4Note");
 
-    var alliance = DriverStation.getAlliance();
-    PathPlannerPath autonPath = path;
-    if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
-      autonPath = autonPath.flipPath();
-    }
-    m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());
+    // var alliance = DriverStation.getAlliance();
+    // PathPlannerPath autonPath = path;
+    // if (alliance.isPresent() && alliance.get() == DriverStation.Alliance.Red) {
+    //   autonPath = autonPath.flipPath();
+    // }
+    // m_robotDrive.resetOdometry(autonPath.getPreviewStartingHolonomicPose());
 
-    return AutoBuilder.followPath(autonPath);
+    // return AutoBuilder.followPath(autonPath);
     // return null;
+    return autoChooser.getSelected();
   }
 }

src/main/java/frc/robot/subsystems/IntakeSubsystem.java

@@ -27,14 +27,14 @@ public class IntakeSubsystem extends SubsystemBase {
 
   private DutyCycleEncoder m_armEncoder = new DutyCycleEncoder(IntakeConstants.kArmEncoderChannel);
 
-  private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
+  //private Rev2mDistanceSensor m_distanceSensor = new Rev2mDistanceSensor(Port.kOnboard); // onboard I2C port;
 
   private double m_intakeSpeed = 0;
   private double m_armSetpoint = IntakeConstants.kIntakeRaisedAngle;
 
   /** Creates a new IntakeSubsystem */
   public IntakeSubsystem() {
-    m_distanceSensor.setAutomaticMode(true);
+    //m_distanceSensor.setAutomaticMode(true);
 
     m_armEncoder.setPositionOffset(IntakeConstants.kArmEncoderOffset);
     SmartDashboard.putNumber("arm", m_armEncoder.getAbsolutePosition());
@@ -97,7 +97,7 @@ public void stopIntake() {
    * Gets distance from Rev 2m sensor
    */
   public double getDistanceSensor() {
-    return m_distanceSensor.getRange();
+    return 0;//m_distanceSensor.getRange();
   }
 
   @Override
@@ -112,7 +112,7 @@ public void periodic() {
 
     SmartDashboard.putNumber("Arm Angle", m_armEncoder.getDistance());
     SmartDashboard.putBoolean("Have Note?", haveNote);
-    SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches));
+    //SmartDashboard.putNumber("distance sensor", m_distanceSensor.getRange(Rev2mDistanceSensor.Unit.kInches));
     SmartDashboard.putNumber("pid output", setMotorSpeed);
   }