Setup Turn Rate and Constants to be used in Ramsete

src/main/java/frc4388/robot/Constants.java

@@ -7,6 +7,7 @@
 
 package frc4388.robot;
 
+import edu.wpi.first.wpilibj.kinematics.DifferentialDriveKinematics;
 import frc4388.utility.LEDPatterns;
 
 /**
@@ -35,6 +36,12 @@ public final class Constants {
         public static final int DRIVE_CRUISE_VELOCITY = 20000;
         public static final int DRIVE_ACCELERATION = 7000;
 
+        /* Trajectory Constants */
+        public static final double kMaxSpeedMetersPerSecond = 3;
+        public static final double kMaxAccelerationMetersPerSecondSquared = 3;
+        public static final double kTrackwidthMeters = 0.69; ///TODO: SET THIS SOON!
+        public static final DifferentialDriveKinematics kDriveKinematics = new DifferentialDriveKinematics(kTrackwidthMeters);
+
         /* Remote Sensors */
         public final static int REMOTE_0 = 0;
         public final static int REMOTE_1 = 1;

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

@@ -59,6 +59,10 @@ public class Drive extends SubsystemBase {
   
   public DoubleSolenoid speedShift;
 
+  public long m_lastTime, m_deltaTime; //in milliseconds
+
+  public double m_lastAngleYaw, m_currentAngleYaw;
+
   /**
    * Add your docs here.
    */
@@ -222,10 +226,17 @@ public class Drive extends SubsystemBase {
      * local output is PID0 - PID1, and other side Talon is PID0 + PID1
      */
     m_rightFrontMotor.configAuxPIDPolarity(false, DriveConstants.DRIVE_TIMEOUT_MS);
+
+    m_lastTime = System.currentTimeMillis();
   }
 
   @Override
   public void periodic() {
+    m_deltaTime = System.currentTimeMillis() - m_lastTime;
+    m_lastTime = System.currentTimeMillis();
+    m_lastAngleYaw = m_currentAngleYaw;
+    m_currentAngleYaw = getGyroYaw();
+    
     try {
       SmartDashboard.putNumber("Pigeon Yaw", getGyroYaw());
       //SmartDashboard.putNumber("Pigeon Pitch", getGyroPitch());
@@ -250,7 +261,7 @@ public class Drive extends SubsystemBase {
       //SmartDashboard.putNumber("PID 0 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_PRIMARY));
       //SmartDashboard.putNumber("PID 1 Pos", m_rightFrontMotor.getSelectedSensorPosition(DriveConstants.PID_TURN));
 
-      SmartDashboard.putString("Odometry Values", getPose().toString());
+      SmartDashboard.putString("Odometry Values Meters", getPose().toString());
 
     } catch (Exception e) {
       System.err.println("Error in the Drive Subsystem");
@@ -389,6 +400,8 @@ public class Drive extends SubsystemBase {
   public void resetGyroYaw() {
     m_pigeon.setYaw(0);
     m_pigeon.setAccumZAngle(0);
+    m_lastAngleYaw = 0;
+    m_currentAngleYaw = 0;
   }
 
   /**
@@ -399,6 +412,16 @@ public class Drive extends SubsystemBase {
     return Math.IEEEremainder(getGyroYaw(), 360);
   }
 
+  /**
+   * Returns the turn rate of the robot.
+   *
+   * @return The turn rate of the robot, in degrees per second
+   */
+  public double getTurnRate() {
+    double deltaYaw = m_currentAngleYaw - m_lastAngleYaw;
+    return deltaYaw / (m_deltaTime/1000);
+  }
+
   /**
    * Returns the currently-estimated pose of the robot.
    * @return The pose.