set up stuff to run autos

build.gradle

@@ -58,6 +58,10 @@ repositories {
     }
   }
   mavenLocal()
+
+  maven {
+    url "https://maven.dacubeking.com/snapshots/"
+  }
 }
 
 configurations.all {
@@ -69,6 +73,7 @@ configurations.all {
 dependencies {
   implementation wpi.java.deps.wpilib()
   implementation wpi.java.vendor.java()
+  implementation 'org.jetbrains:annotations:23.0.0'
 
   roborioDebug wpi.java.deps.wpilibJniDebug(wpi.platforms.roborio)
   roborioDebug wpi.java.vendor.jniDebug(wpi.platforms.roborio)
@@ -90,6 +95,8 @@ dependencies {
 
   def akitJson = new groovy.json.JsonSlurper().parseText(new File(projectDir.getAbsolutePath() + "/vendordeps/AdvantageKit.json").text)
   annotationProcessor "org.littletonrobotics.akit.junction:junction-autolog:$akitJson.version"
+
+  implementation "com.dacubeking:autobuilder-robot:2.2.10-command-rewrite-beta"
 }
 
 test {

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

@@ -7,6 +7,8 @@
 import static frc.robot.Constants.LOG_DIRECTORY;
 import static frc.robot.Constants.MIN_FREE_SPACE;
 
+import com.dacubeking.AutoBuilder.robot.reflection.ClassInformationSender;
+import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
@@ -106,6 +108,8 @@ public void disabledInit() {
      */
     @Override
     public void disabledPeriodic() {
+        // Ensure that the selected autonomous command is loaded
+        AutonomousContainer.getInstance().loadAuto(robotContainer.getAutonomousCommand());
     }
 
     /**
@@ -170,6 +174,7 @@ public void testPeriodic() {
      */
     @Override
     public void simulationInit() {
+        ClassInformationSender.updateReflectionInformation("frc.robot");
     }
 
     /**

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

@@ -4,17 +4,20 @@
 
 package frc.robot;
 
+import com.dacubeking.AutoBuilder.robot.GuiAuto;
+import com.dacubeking.AutoBuilder.robot.robotinterface.AutonomousContainer;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.controller.RamseteController;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.RamseteCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import frc.robot.subsystems.drive.Drive;
 import frc.robot.subsystems.drive.DriveIO;
 import frc.robot.subsystems.drive.DriveIOFalcon500;
 import frc.robot.subsystems.drive.DriveIOSim;
+import org.jetbrains.annotations.Nullable;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 import org.littletonrobotics.junction.networktables.LoggedDashboardNumber;
 
@@ -30,7 +33,9 @@ public class RobotContainer {
     private final CommandXboxController controller = new CommandXboxController(0);
 
     // Dashboard inputs
-    private final LoggedDashboardChooser<Command> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
+    private final LoggedDashboardChooser<String> autoChooser = new LoggedDashboardChooser<>("Auto Choices");
+    private final LoggedDashboardChooser<String> sideChooser = new LoggedDashboardChooser<>("Side Chooser");
+
     private final LoggedDashboardNumber flywheelSpeedInput = new LoggedDashboardNumber("Flywheel Speed", 1500.0);
 
     /**
@@ -56,14 +61,34 @@ public RobotContainer() {
                 break;
         }
 
+        // Initialize autonomous container
+        AutonomousContainer.getInstance().initialize(
+                false,
+                info -> new RamseteCommand(
+                        info.trajectory(),
+                        drive::getPose,
+                        new RamseteController(),
+                        drive.getFeedforward(),
+                        drive.getKinematics(),
+                        drive::getWheelSpeeds,
+                        new PIDController(0, 0, 0),
+                        new PIDController(0, 0, 0),
+                        drive::driveVoltage,
+                        drive
+                ),
+                drive::resetOdometry,
+                true,
+                this
+        );
+
         // Set up auto routines
-        autoChooser.addDefaultOption("Do Nothing", new InstantCommand());
+        sideChooser.addDefaultOption("Blue", "blue");
+        sideChooser.addOption("Red", "red");
+
+        AutonomousContainer.getInstance().getAutonomousNames().forEach(name -> autoChooser.addOption(name, name));
 
         // Configure the button bindings
         configureButtonBindings();
-
-        SmartDashboard.putNumber("driveleft", 0);
-        SmartDashboard.putNumber("driveright", 0);
     }
 
     /**
@@ -85,7 +110,7 @@ private void configureButtonBindings() {
      *
      * @return the command to run in autonomous
      */
-    public Command getAutonomousCommand() {
-        return autoChooser.get();
+    public @Nullable GuiAuto getAutonomousCommand() {
+        return AutonomousContainer.getInstance().getAuto(autoChooser.get(), sideChooser.get(), true);
     }
 }

src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,8 +1,11 @@
 package frc.robot.subsystems.drive;
 
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.kinematics.DifferentialDriveKinematics;
 import edu.wpi.first.math.kinematics.DifferentialDriveOdometry;
+import edu.wpi.first.math.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -35,17 +38,28 @@ public void periodic() {
                     getRightPositionMeters(), new Pose2d());
             isFirstRun = false;
         }
-        odometry.update(new Rotation2d(-inputs.gyroYawRad), getLeftPositionMeters(), getRightPositionMeters());
+        odometry.update(getHeading(), getLeftPositionMeters(), getRightPositionMeters());
         Logger.getInstance().recordOutput("Odometry", getPose());
     }
 
+    public Rotation2d getHeading() {
+        return Rotation2d.fromDegrees(-inputs.gyroYawRad);
+    }
+
     /**
      * Run open loop at the specified percentage.
      */
     public void drivePercent(double leftPercent, double rightPercent) {
         io.setVoltage(leftPercent * 12.0, rightPercent * 12.0);
     }
 
+    /**
+     * Run open loop at the specified percentage.
+     */
+    public void driveVoltage(double leftVoltage, double rightVoltage) {
+        io.setVoltage(leftVoltage, rightVoltage);
+    }
+
     /**
      * Run open loop based on stick positions.
      */
@@ -95,4 +109,24 @@ public double getLeftVelocityMeters() {
     public double getRightVelocityMeters() {
         return inputs.rightVelocityRadPerSec * WHEEL_RADIUS_METERS;
     }
+
+    public Pose2d getPoseMeters() {
+        return odometry.getPoseMeters();
+    }
+
+    public void resetOdometry(Pose2d pose) {
+        odometry.resetPosition(getHeading(), getLeftPositionMeters(), getRightPositionMeters(), pose);
+    }
+
+    public SimpleMotorFeedforward getFeedforward() {
+        return new SimpleMotorFeedforward(0.0, 0.7, 0.2); //Temp values
+    }
+
+    public DifferentialDriveKinematics getKinematics() {
+        return new DifferentialDriveKinematics(0.6); //Temp values
+    }
+
+    public DifferentialDriveWheelSpeeds getWheelSpeeds() {
+        return new DifferentialDriveWheelSpeeds(getLeftVelocityMeters(), getRightVelocityMeters());
+    }
 }