Autos for competition

- this is the current code for competition at Waterbury

networktables.json

@@ -1,112 +1,40 @@
 [
   {
-    "name": "/Preferences/kSwerveModuleTurnI",
+    "name": "/Preferences/testwrist",
     "type": "double",
     "value": 0.0,
     "properties": {
       "persistent": true
     }
   },
   {
-    "name": "/Preferences/kSwerveModuleDriveV",
+    "name": "/Preferences/kShooterWristP",
     "type": "double",
-    "value": 0.145,
+    "value": 0.16,
     "properties": {
       "persistent": true
     }
   },
   {
-    "name": "/Preferences/0 Front LeftEnabled",
-    "type": "boolean",
-    "value": true,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleDriveI",
-    "type": "double",
-    "value": 0.0,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleTurnP",
-    "type": "double",
-    "value": 0.3,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleDriveP",
-    "type": "double",
-    "value": 0.01,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleDriveD",
+    "name": "/Preferences/kShooterWristI",
     "type": "double",
-    "value": 0.0,
+    "value": 0.007,
     "properties": {
       "persistent": true
     }
   },
   {
-    "name": "/Preferences/kSwerveTestTurn",
+    "name": "/Preferences/kShooterWristD",
     "type": "double",
     "value": 0.0,
     "properties": {
       "persistent": true
     }
   },
   {
-    "name": "/Preferences/kSwerveTestDrive",
+    "name": "/Preferences/kShooterWristV",
     "type": "double",
-    "value": 0.0,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleTurnD",
-    "type": "double",
-    "value": 0.0,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/kSwerveModuleTurnV",
-    "type": "double",
-    "value": 0.0,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/1 Front RightEnabled",
-    "type": "boolean",
-    "value": true,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/2 Back LeftEnabled",
-    "type": "boolean",
-    "value": true,
-    "properties": {
-      "persistent": true
-    }
-  },
-  {
-    "name": "/Preferences/3 Back RightEnabled",
-    "type": "boolean",
-    "value": true,
+    "value": 0.5,
     "properties": {
       "persistent": true
     }

simgui.json

@@ -11,6 +11,7 @@
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
+      "/Shuffleboard/Autos/Auto": "String Chooser",
       "/SmartDashboard/Auto Visualization": "Field2d",
       "/SmartDashboard/Field": "Field2d",
       "/SmartDashboard/Swerve Visualization": "Mechanism2d",

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

@@ -8,6 +8,7 @@
 import frc.robot.subsystems.shooter.Shooter;
 import frc.robot.subsystems.shooter.ShooterPosition;
 import frc.robot.commands.AutoCommand;
+import frc.robot.commands.Autos;
 import frc.robot.commands.DriveTrajectory;
 import frc.robot.commands.auto.MidDoubleAuto;
 import frc.robot.subsystems.swerve.SwerveDrive;
@@ -39,6 +40,8 @@ public RobotContainer() {
     configureBindings();
     CameraServer.startAutomaticCapture();
     Shuffleboard.getTab("Camera").add("Camera", CameraServer.getVideo().getSource());
+
+    Autos.initAutos(swerveDrive, shooter);
   }
 
   private void configureBindings() {
@@ -56,7 +59,7 @@ private void configureBindings() {
       driverController::getRightX, // use in real robot
       // altController::getLeftX, //use in simulation
       driverController.leftBumper()::getAsBoolean,
-      driverController.rightTrigger()::getAsBoolean,
+      driverController.rightBumper()::getAsBoolean,
       true
     ));
 
@@ -82,9 +85,13 @@ private void configureBindings() {
   }
 
   public Command getAutonomousCommand() {
-    DriveTrajectory testPath = new DriveTrajectory("New Path", swerveDrive, Rotation2d.fromDegrees(180));
-    swerveDrive.resetPose(testPath.getStartPose());
+    // DriveTrajectory testPath = new DriveTrajectory("New Path", swerveDrive, Rotation2d.fromDegrees(180));
+    // swerveDrive.resetPose(testPath.getStartPose());
     // return testPath;
-    return Commands.print("No current auto configured");
+    // return Commands.print("No current auto configured");
+
+    AutoCommand selectedAuto = Autos.autoChooser.getSelected();
+    swerveDrive.resetPose(selectedAuto.getStartPose());
+    return selectedAuto;
   }
 }

src/main/java/frc/robot/commands/Autos.java

@@ -10,18 +10,36 @@
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.math.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 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.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.commands.auto.MidDoubleAuto;
+import frc.robot.commands.auto.NothingAuto;
+import frc.robot.commands.auto.ShootOnly;
+import frc.robot.commands.auto.SpeakerBack;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.shooter.ShooterPosition;
 import frc.robot.subsystems.swerve.SwerveDrive;
 
 public class Autos {
     public static final SendableChooser<AutoCommand> autoChooser = new SendableChooser<>();
     public static final Field2d autoField = new Field2d();
 
-    public static final void initAutos(SwerveDrive swerveDrive) {
-        autoChooser.addOption("Mid Double Auto", new MidDoubleAuto(swerveDrive));
+    public static final void initAutos(SwerveDrive swerveDrive, Shooter shooter) {
+        autoChooser.addOption("Speaker Back", new SpeakerBack(swerveDrive, shooter));
+        autoChooser.setDefaultOption("Shoot Only", new ShootOnly(shooter));
+        // autoChooser.addOption("Shoot Only", new SpeakerOnly(shooter));
+        autoChooser.addOption("Do Nothing", new NothingAuto());
+        // autoChooser.addOption("Mid Double Auto", new MidDoubleAuto(swerveDrive));
+        Shuffleboard.getTab("Autos")
+            .add("Auto", autoChooser)
+            .withWidget(BuiltInWidgets.kSplitButtonChooser)
+            .withSize(9, 1);
     }
 
     public static Trajectory convertFromPathPlanner(PathPlannerTrajectory pathPlannerTrajectory) {
@@ -51,4 +69,17 @@ public static Trajectory getPathPlannerTrajectory(String filename, Rotation2d st
             )
         );
     }
+
+    public static Command runSpeakerShot(Shooter shooter) {
+        return Commands.sequence(
+            shooter.runSetWristPosition(ShooterPosition.SPEAKER),
+            shooter.runShooter(20),
+            new WaitCommand(1.2),
+            shooter.runIndex(-10),
+            new WaitCommand(1.5),
+            shooter.runIndex(0),
+            shooter.runShooter(0),
+            shooter.runSetWristPosition(ShooterPosition.FLAT)
+        );
+    }
 }

src/main/java/frc/robot/commands/auto/NothingAuto.java

@@ -1,19 +1,18 @@
 package frc.robot.commands.auto;
 
-import com.pathplanner.lib.commands.PathPlannerAuto;
-
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Commands;
 import frc.robot.commands.AutoCommand;
+import frc.robot.commands.Autos;
 
 public class NothingAuto extends AutoCommand {
-
-
-
     public NothingAuto() {
+        addCommands(Commands.print("Nothing auto has finished running."));
     }
 
     @Override
     public Pose2d getStartPose() {
-        return super.getStartPose();
+        return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(0)).getInitialPose();
     }
 }

src/main/java/frc/robot/commands/auto/ShootOnly.java

@@ -0,0 +1,21 @@
+package frc.robot.commands.auto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import frc.robot.commands.AutoCommand;
+import frc.robot.commands.Autos;
+import frc.robot.subsystems.shooter.Shooter;
+
+public class ShootOnly extends AutoCommand {
+    public ShootOnly(Shooter shooter) {
+        addCommands(
+            Autos.runSpeakerShot(shooter)
+        );
+        addRequirements(shooter);
+    }
+
+    @Override
+    public Pose2d getStartPose() {
+        return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(180)).getInitialPose();
+    }
+}

src/main/java/frc/robot/commands/auto/SpeakerBack.java

@@ -0,0 +1,32 @@
+package frc.robot.commands.auto;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.WaitCommand;
+import frc.robot.commands.AutoCommand;
+import frc.robot.commands.Autos;
+import frc.robot.subsystems.shooter.Shooter;
+import frc.robot.subsystems.swerve.SwerveDrive;
+
+public class SpeakerBack extends AutoCommand {
+    public SpeakerBack(SwerveDrive swerve, Shooter shooter) {
+        addCommands(
+            Autos.runSpeakerShot(shooter),
+            new WaitCommand(1),
+            Commands.runOnce(() -> {
+                swerve.rawDriveInputs(0, -2, 0, false, false);
+            }, swerve),
+            new WaitCommand(0.7),
+            Commands.runOnce(() -> {
+                swerve.rawDriveInputs(0, 0, 0, false, false);
+            }, swerve)
+        );
+        addRequirements(swerve, shooter);
+    }
+
+    @Override
+    public Pose2d getStartPose() {
+        return Autos.getPathPlannerTrajectory("Get Mid Note", Rotation2d.fromDegrees(180)).getInitialPose();
+    }
+}

src/main/java/frc/robot/subsystems/swerve/SwerveDrive.java

@@ -84,28 +84,28 @@ private void initComponents() {
             "0 Front Left",
             SwerveConstants.kFrontLeftTurningCanID,
             SwerveConstants.kFrontLeftDrivingCanID,
-            new Rotation2d(0.87),
+            new Rotation2d(0.8749),
             true
         );
         frontRightModule = new SDSSwerveModule(
             "1 Front Right",
             SwerveConstants.kFrontRightTurningCanID,
             SwerveConstants.kFrontRightDrivingCanID,
-            new Rotation2d(3.63),
+            new Rotation2d(3.6038),
             true
         );
         backLeftModule = new SDSSwerveModule(
             "2 Back Left",
             SwerveConstants.kBackLeftTurningCanID,
             SwerveConstants.kBackLeftDrivingCanID,
-            new Rotation2d(3.16),
+            new Rotation2d(3.1532),
             true
         );
         backRightModule = new SDSSwerveModule(
             "3 Back Right",
             SwerveConstants.kBackRightTurningCanID,
             SwerveConstants.kBackRightDrivingCanID,
-            new Rotation2d(2.57),
+            new Rotation2d(2.5972),
             true
         );
         modules = new SDSSwerveModule[] {frontLeftModule, frontRightModule, backLeftModule, backRightModule};
@@ -130,7 +130,7 @@ private void initMathModels() {
 
         swerveOdometry = new SwerveDrivePoseEstimator(
             SwerveConstants.kSwerveKinematics,
-            gyro.getRotation2d().times(-1), new SwerveModulePosition[]{
+            gyro.getRotation2d(), new SwerveModulePosition[]{
                 frontLeftModule.getPosition(),
                 frontRightModule.getPosition(),
                 backLeftModule.getPosition(),
@@ -286,7 +286,7 @@ public void adjustedDriveInputs(
 
     public void rawDriveInputs(double rawXSpeed, double rawYSpeed, double rawRotSpeed, boolean noOptimize, boolean robotCentric) {
         SwerveModuleState[] states = SwerveConstants.kSwerveKinematics.toSwerveModuleStates(!robotCentric ?
-            ChassisSpeeds.fromFieldRelativeSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed, gyro.getRotation2d().times(-1)) : // see if gyro is done correctly 
+            ChassisSpeeds.fromFieldRelativeSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed, gyro.getRotation2d()) : // see if gyro is done correctly 
             new ChassisSpeeds(rawXSpeed, rawYSpeed, rawRotSpeed)
         );
         // System.out.println(rawXSpeed + " " + rawYSpeed + " " + rawRotSpeed);
@@ -425,9 +425,9 @@ public void periodic() {
         backLeftModule.putInfo();
         backRightModule.putInfo();
 
-        SmartDashboard.putNumber("Gyro", gyro.getAngle());
+        SmartDashboard.putNumber("Gyro", -gyro.getAngle());
 
-        Rotation2d robotRotation = Rotation2d.fromDegrees(-gyro.getAngle());
+        Rotation2d robotRotation = gyro.getRotation2d();
         if(DriverStation.getAlliance().equals(Optional.of(Alliance.Blue))) robotRotation = robotRotation.rotateBy(Rotation2d.fromDegrees(180));
         swerveOdometry.update(
             robotRotation,