From 408f60b32cd24d819aacc78551fa2240802205f0 Mon Sep 17 00:00:00 2001
From: joelzoto <118407587+joelzoto@users.noreply.github.com>
Date: Sat, 1 Mar 2025 11:12:22 -0500
Subject: [PATCH] Auton

---
 src/main/java/frc/robot/Robot.java          | 17 ++++++++++++++++-
 src/main/java/frc/robot/RobotContainer.java | 13 +++++++++----
 2 files changed, 25 insertions(+), 5 deletions(-)

diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 157d101..0aadf4f 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,9 +4,15 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.MetersPerSecond;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
 
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -14,7 +20,10 @@
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc.crevolib.configs.CTREConfigs;
 import frc.robot.driver.DriverXbox;
+import frc.robot.drivetrain.CommandSwerveDrivetrain;
+import frc.robot.drivetrain.TunerConstants;
 import frc.robot.operator.OperatorXbox;
+import frc.robot.RobotContainer.*;
 
 /**
  * The methods in this class are called automatically corresponding to each mode, as described in
@@ -108,12 +117,18 @@ public void autonomousInit() {
     }
   }
 
+  
+
   /**
    * This function is called periodically during autonomous.
    */
   @Override
   public void autonomousPeriodic() {
-      
+    // CommandSwerveDrivetrain.getInstance().applyRequest(() -> 
+    //    RobotContainer.drive.withVelocityX(0.25 * RobotContainer.kMaxVelocity) // Drive forward with negative Y (forward)
+    //         .withVelocityY(0.0) // Drive left with negative X (left)
+    //         .withRotationalRate(0.0) // Drive counterclockwise with negative X (left)
+    // );
   }
 
   @Override
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 24ac39b..bd9a382 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -17,6 +17,7 @@
 import frc.robot.subsystems.CoralSubsystem;
 import frc.robot.subsystems.ElevatorSubsystem;
 
+
 /**
  * This class is where the bulk of the robot should be declared. Since Command-based is a
  * "declarative" paradigm, very little robot logic should actually be handled in the {@link Robot}
@@ -24,13 +25,13 @@
  * subsystems, commands, and trigger mappings) should be declared here.
  */
 public class RobotContainer {
-    private final double kMaxVelocity = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
-    private final double kMaxAngularVelocity = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+    public static double kMaxVelocity = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+    public static double kMaxAngularVelocity = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
     
     public static boolean modeFast = true;
 
     /* Setting up bindings for necessary control of the swerve drive platform */
-    private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+    public static SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
         .withDeadband(kMaxVelocity * 0.1)
         .withRotationalDeadband(kMaxAngularVelocity * 0.1) // Add a 10% deadband
         .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
@@ -47,7 +48,11 @@ public RobotContainer() {
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        return mAutonChooser.getSelected();
+        return CommandSwerveDrivetrain.getInstance().applyRequest(() -> 
+        RobotContainer.drive.withVelocityX(0.25 * RobotContainer.kMaxVelocity) // Drive forward with negative Y (forward)
+             .withVelocityY(0.0) // Drive left with negative X (left)
+             .withRotationalRate(0.0) // Drive counterclockwise with negative X (left)
+     );
     }
 
     public void setDefaultCommands() {