Add Ramsete Controller and Everything Needed to Run it

src/main/java/frc4388/robot/Robot.java

@@ -75,6 +75,8 @@ public class Robot extends TimedRobot {
     m_autonomousCommand = m_robotContainer.getAutonomousCommand();
 
     m_robotContainer.setDriveNeutralMode(NeutralMode.Brake);
+    m_robotContainer.resetOdometry();
+
     /*
      * String autoSelected = SmartDashboard.getString("Auto Selector",
      * "Default"); switch(autoSelected) { case "My Auto": autonomousCommand

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

@@ -7,13 +7,26 @@
 
 package frc4388.robot;
 
+import java.util.List;
+import java.util.function.BiConsumer;
+
 import com.ctre.phoenix.motorcontrol.NeutralMode;
 
 import edu.wpi.first.wpilibj.Joystick;
+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.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
+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.*;
@@ -146,14 +159,49 @@ public class RobotContainer {
         m_robotDrive.setDriveTrainNeutralMode(mode);
     }
 
+    public void resetOdometry() {
+        m_robotDrive.resetGyroAngles();
+        m_robotDrive.setOdometry(new Pose2d());
+    }
+
     /**
      * Use this to pass the autonomous command to the main {@link Robot} class.
      *
      * @return the command to run in autonomous
      */
     public Command getAutonomousCommand() {
-        // no auto
-        return new InstantCommand();
+
+        var wheelSpeeds = new ChassisSpeeds();
+
+        // Create config for trajectory
+        TrajectoryConfig config = new TrajectoryConfig( DriveConstants.kMaxSpeedMetersPerSecond,
+                                                        DriveConstants.kMaxAccelerationMetersPerSecondSquared)
+                                                        // Add kinematics to ensure max speed is actually obeyed
+                                                        .setKinematics(DriveConstants.kDriveKinematics);
+        
+        Trajectory exampleTrajectory = TrajectoryGenerator.generateTrajectory(
+            // Start at the origin facing the +X direction
+            new Pose2d(0, 0, new Rotation2d(0)),
+            // Pass through these two interior waypoints, making an 's' curve path
+            List.of(
+                new Translation2d(1, 1),
+                new Translation2d(2, -1)
+            ),
+            // End 3 meters straight ahead of where we started, facing forward
+            new Pose2d(3, 0, new Rotation2d(0)),
+            // Pass config
+            config);
+
+        RamseteCommand ramseteCommand = new RamseteCommand(
+            exampleTrajectory,
+            m_robotDrive::getPose,
+            new RamseteController(), 
+            DriveConstants.kDriveKinematics,
+            m_robotDrive::tankDriveVelocity,
+            m_robotDrive);
+
+        // Run path following command, then stop at the end.
+        return ramseteCommand.andThen(() -> m_robotDrive.tankDriveVelocity(0, 0));
     }
 
     /**

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

@@ -25,12 +25,14 @@ import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.drive.DifferentialDrive;
 import edu.wpi.first.wpilibj.geometry.Pose2d;
 import edu.wpi.first.wpilibj.geometry.Rotation2d;
+import edu.wpi.first.wpilibj.kinematics.ChassisSpeeds;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveOdometry;
 import edu.wpi.first.wpilibj.kinematics.DifferentialDriveWheelSpeeds;
 import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpiutil.math.MathUtil;
 import frc4388.robot.Constants.DriveConstants;
 import frc4388.robot.Gains;
 
@@ -61,7 +63,7 @@ public class Drive extends SubsystemBase {
 
   public long m_lastTime, m_deltaTime; //in milliseconds
 
-  public double m_lastAngleYaw, m_currentAngleYaw;
+  public double m_lastAngleYaw, m_currentAngleYaw, m_kinematicsTargetAngle;
 
   /**
    * Add your docs here.
@@ -364,6 +366,29 @@ public class Drive extends SubsystemBase {
     runDriveStraightVelocityPID(0, targetGyro);
   }
 
+  /**
+   * Controls the left and right sides of the drive with velocity targets.
+   *
+   * @param leftSpeed  the commanded left output
+   * @param rightSpeed the commanded right output
+   */
+  public void tankDriveVelocity(double leftSpeed, double rightSpeed) {
+    DifferentialDriveWheelSpeeds wheelSpeeds = new DifferentialDriveWheelSpeeds(leftSpeed, rightSpeed);
+    ChassisSpeeds chassisSpeeds = DriveConstants.kDriveKinematics.toChassisSpeeds(wheelSpeeds);
+    double moveVelMPS = chassisSpeeds.vxMetersPerSecond;
+    double angleVelRad = chassisSpeeds.omegaRadiansPerSecond;
+    double angleVelDeg = Math.toDegrees(angleVelRad);
+
+    m_kinematicsTargetAngle += angleVelDeg * (m_deltaTime/1000);
+    m_kinematicsTargetAngle = MathUtil.clamp( m_kinematicsTargetAngle, 
+                                              m_currentAngleYaw-(360),
+                                              m_currentAngleYaw+(360));
+    double targetGyro = (m_kinematicsTargetAngle / 360) * DriveConstants.TICKS_PER_GYRO_REV;
+    double moveVel = inchesToMeters(metersToInches(moveVelMPS))/DriveConstants.SECONDS_TO_TICK_TIME;
+
+    runDriveStraightVelocityPID(moveVel, targetGyro);
+  }
+
   /**
    * Returns the current yaw of the pigeon
    */
@@ -400,8 +425,16 @@ public class Drive extends SubsystemBase {
   public void resetGyroYaw() {
     m_pigeon.setYaw(0);
     m_pigeon.setAccumZAngle(0);
+    resetGyroAngles();
+  }
+
+  /**
+   * Add docs here
+   */
+  public void resetGyroAngles() {
     m_lastAngleYaw = 0;
     m_currentAngleYaw = 0;
+    m_kinematicsTargetAngle = 0;
   }
 
   /**
@@ -484,6 +517,15 @@ public class Drive extends SubsystemBase {
     return ticks * DriveConstants.INCHES_PER_TICK;
   }
 
+  /**
+   * Converts a value in inches to ticks.
+   * @param inches The value in inches to convert
+   * @return The converted value in ticks
+   */
+  public double inchesToTicks(double inches) {
+    return inches * DriveConstants.TICKS_PER_INCH;
+  }
+
   /**
    * Converts a value in inches to meters.
    * @param inches The value in inches to convert
@@ -492,6 +534,15 @@ public class Drive extends SubsystemBase {
   public double inchesToMeters(double inches) {
     return inches / DriveConstants.INCHES_PER_METER;
   }
+
+  /**
+   * Converts a value in meters to inches.
+   * @param meters The value in meters to convert
+   * @return The converted value in inches
+   */
+  public double metersToInches(double meters) {
+    return meters * DriveConstants.INCHES_PER_METER;
+  }
   
   /*
    * Set to high or low gear based on boolean state, true = high, false = low