Merge branch 'main' of https://git.coldlightalchemist.com/Team_2648/2025_Robot_Code

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

@@ -14,12 +14,16 @@ import frc.robot.subsystems.Drivetrain;
 import frc.robot.subsystems.Elevator;
 import frc.robot.subsystems.Indexer;
 import frc.robot.subsystems.Manipulator;
+
+import com.pathplanner.lib.auto.AutoBuilder;
+import com.pathplanner.lib.auto.NamedCommands;
+
 import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.PrintCommand;
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 
 public class RobotContainer {
@@ -40,6 +44,8 @@ public class RobotContainer {
 	private CommandXboxController driver;
 	private CommandXboxController operator;
 
+	private SendableChooser<Command> autoChooser;
+
 	public RobotContainer() {
 		arm = new Arm();
 
@@ -58,8 +64,12 @@ public class RobotContainer {
 		driver = new CommandXboxController(OIConstants.kDriverControllerPort);
 		operator = new CommandXboxController(OIConstants.kOperatorControllerPort);
 
+		autoChooser = AutoBuilder.buildAutoChooser();
+
 		configureButtonBindings();
 
+		configureNamedCommands();
+
 		configureShuffleboard();
 	}
 
@@ -148,10 +158,22 @@ public class RobotContainer {
 		);
 	}
 
+	private void configureNamedCommands() {
+		NamedCommands.registerCommand("Drivetrain Set X", drivetrain.setXCommand());
+	}
+
 	//creates tabs and transforms them on the shuffleboard
     private void configureShuffleboard() {
+		ShuffleboardTab autoTab = Shuffleboard.getTab(OIConstants.kAutoTab);
     	ShuffleboardTab sensorTab = Shuffleboard.getTab(OIConstants.kSensorsTab);
 
+		Shuffleboard.selectTab(OIConstants.kAutoTab);
+
+		autoTab.add("Auto Selection", autoChooser)
+			.withSize(2, 1)
+			.withPosition(0, 0)
+			.withWidget(BuiltInWidgets.kComboBoxChooser);
+
 		sensorTab.addDouble("ElevatorPosition", elevator::getEncoderPosition)
 			.withSize(2, 1)
 			.withPosition(0, 0)
@@ -164,7 +186,7 @@ public class RobotContainer {
   }
 
 	public Command getAutonomousCommand() {
-		return new PrintCommand("NO AUTO DEFINED");
+		return autoChooser.getSelected();
 	}
 
 	//teleop routines

src/main/java/frc/robot/constants/AutoConstants.java

@@ -1,5 +1,13 @@
 package frc.robot.constants;
 
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
+import com.pathplanner.lib.config.PIDConstants;
+import com.pathplanner.lib.config.RobotConfig;
+import com.pathplanner.lib.controllers.PPHolonomicDriveController;
+
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
 
 public class AutoConstants {
@@ -15,4 +23,21 @@ public class AutoConstants {
     // Constraint for the motion profiled robot angle controller
     public static final TrapezoidProfile.Constraints kThetaControllerConstraints = new TrapezoidProfile.Constraints(
         kMaxAngularSpeedRadiansPerSecond, kMaxAngularSpeedRadiansPerSecondSquared);
+
+    // TODO This is a constant being managed like a static rewriteable variable
+    public static RobotConfig kRobotConfig;
+
+    public static final PPHolonomicDriveController kPPDriveController = new PPHolonomicDriveController(
+        new PIDConstants(kPXController, 0, 0),
+        new PIDConstants(kPYController, 0, 0)
+    );
+
+    static {
+        try {
+            kRobotConfig = RobotConfig.fromGUISettings();
+        } catch (IOException | ParseException e) {
+            System.err.println("FAILED TO READ ROBOTCONFIG, WAS THE CONFIG SET UP IN PATHPLANNER?");
+            e.printStackTrace();
+        }
+    }
 }

src/main/java/frc/robot/constants/OIConstants.java

@@ -6,5 +6,6 @@ public class OIConstants {
     
     public static final double kDriveDeadband = 0.05;
 
-    public static final String kSensorsTab = "SensorsTab";
+    public static final String kAutoTab = "Auto Tab";
+    public static final String kSensorsTab = "Sensors Tab";
 }

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

@@ -4,9 +4,11 @@
 
 package frc.robot.subsystems;
 
+import java.util.Optional;
 import java.util.function.BooleanSupplier;
 import java.util.function.DoubleSupplier;
 
+import com.pathplanner.lib.auto.AutoBuilder;
 import com.studica.frc.AHRS;
 import com.studica.frc.AHRS.NavXComType;
 
@@ -18,10 +20,10 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
-import edu.wpi.first.wpilibj.ADIS16470_IMU;
-import edu.wpi.first.wpilibj.ADIS16470_IMU.IMUAxis;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.constants.AutoConstants;
 import frc.robot.constants.DrivetrainConstants;
 import frc.robot.constants.OIConstants;
 
@@ -75,6 +77,23 @@ public class Drivetrain extends SubsystemBase {
                 m_rearLeft.getPosition(),
                 m_rearRight.getPosition()
         });
+
+        AutoBuilder.configure(
+            this::getPose, 
+            this::resetOdometry, 
+            this::getCurrentChassisSpeeds, 
+            this::driveWithChassisSpeeds, 
+            AutoConstants.kPPDriveController, 
+            AutoConstants.kRobotConfig,
+            () -> {
+                Optional<DriverStation.Alliance> alliance = DriverStation.getAlliance();
+                    if (alliance.isPresent()) {
+                        return alliance.get() == DriverStation.Alliance.Red;
+                    }
+                    return false;
+            }, 
+            this
+        );
     }
 
     @Override
@@ -90,6 +109,23 @@ public class Drivetrain extends SubsystemBase {
         });
     }
 
+    public ChassisSpeeds getCurrentChassisSpeeds() {
+        return DrivetrainConstants.kDriveKinematics.toChassisSpeeds(
+            m_frontLeft.getState(),
+            m_frontRight.getState(),
+            m_rearLeft.getState(),
+            m_rearRight.getState()
+        );
+    }
+
+    public void driveWithChassisSpeeds(ChassisSpeeds speeds) {
+        ChassisSpeeds discreteSpeeds = ChassisSpeeds.discretize(speeds, 0.2);
+        SwerveModuleState[] newStates = DrivetrainConstants.kDriveKinematics.toSwerveModuleStates(discreteSpeeds);
+        SwerveDriveKinematics.desaturateWheelSpeeds(newStates, DrivetrainConstants.kMaxSpeedMetersPerSecond);
+
+        setModuleStates(newStates);
+    }
+
     /**
      * Returns the currently-estimated pose of the robot.
      *