Added Trajectory Command

Need to figure out how to make it work

src/main/java/frc4388/robot/RobotContainer.java

@@ -7,13 +7,24 @@
 
 package frc4388.robot;
 
+import java.util.List;
+
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
+import edu.wpi.first.wpilibj.controller.PIDController;
+import edu.wpi.first.wpilibj.controller.RamseteController;
 import edu.wpi.first.wpilibj.controller.SimpleMotorFeedforward;
+import edu.wpi.first.wpilibj.geometry.Pose2d;
+import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.geometry.Translation2d;
+import edu.wpi.first.wpilibj.trajectory.Trajectory;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryConfig;
+import edu.wpi.first.wpilibj.trajectory.TrajectoryGenerator;
 import edu.wpi.first.wpilibj.trajectory.constraint.DifferentialDriveVoltageConstraint;
 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.JoystickButton;
 import frc4388.robot.Constants.*;
@@ -121,7 +132,53 @@ public class RobotContainer {
      */
     public Command getAutonomousCommand() {
         // no auto
-        return new InstantCommand();
+        var autoVoltageConstraint =
+            new DifferentialDriveVoltageConstraint(
+                new SimpleMotorFeedforward( DriveConstants.ksVolts, 
+                                            DriveConstants.kvVoltSecondsPerMeter,
+                                            DriveConstants.kaVoltSecondsSquaredPerMeter), 
+                DriveConstants.kDriveKinematics, 
+                10);
+
+        TrajectoryConfig config =
+            new TrajectoryConfig(   DriveConstants.kMaxSpeedMetersPerSecond,
+                                    DriveConstants.kMaxAccelerationMetersPerSecondSquared)
+
+                .setKinematics(DriveConstants.kDriveKinematics)
+
+                .addConstraint(autoVoltageConstraint);
+        
+        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+            
+            new Pose2d(0, 0, new Rotation2d(0)),
+            
+            List.of(
+                new Translation2d(1, 1),
+                new Translation2d(2, -1)
+            ),
+            
+            new Pose2d(3, 0, new Rotation2d(0)),
+            
+            config
+        );
+
+        RamseteCommand ramseteCommand = new RamseteCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            new RamseteController(DriveConstants.kRamseteB, DriveConstants.kRamseteZeta),
+            new SimpleMotorFeedforward(  DriveConstants.ksVolts,
+                                         DriveConstants.kvVoltSecondsPerMeter,
+                                         DriveConstants.kaVoltSecondsSquaredPerMeter),
+            DriveConstants.kDriveKinematics,
+            m_robotDrive::getWheelSpeeds,
+            new PIDController(DriveConstants.kPDriveVel, 0, 0),
+            new PIDController(DriveConstants.kPDriveVel, 0, 0),
+            
+            m_robotDrive::tankDriveVolts,
+            m_robotDrive
+        );
+
+        return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVolts(0, 0));
     }
 
     /**

src/main/java/frc4388/robot/subsystems/Drive.java

@@ -508,4 +508,10 @@ public class Drive extends SubsystemBase {
   public double ticksToInches(double ticks) {
     return ticks * DriveConstants.INCHES_PER_TICK;
   }
+
+  public void tankDriveVolts(double leftVolts, double rightVolts) {
+    m_leftFrontMotor.setVoltage(leftVolts);
+    m_rightFrontMotor.setVoltage(-rightVolts);
+    m_driveTrain.feedWatchdog();
+  }
 }