calculate robotperiod

build.gradle

@@ -79,7 +79,7 @@ dependencies {
     simulationRelease wpi.sim.enableRelease()
 
     testImplementation 'org.junit.jupiter:junit-jupiter:5.10.1'
-    implementation "com.github.deepbluerobotics:lib199:3dc0356f5ea971aa24802980371effc7fc2215fc"
+    implementation "com.github.deepbluerobotics:lib199:cc93080eab462da161d0d47eda253ac8b31495ee"
 }
 
 test {

src/main/java/org/carlmontrobotics/Constants.java

@@ -40,7 +40,7 @@ public static final class Drivetrain {
 
 			public static final double driveModifier = 1;
 			public static final double wheelDiameterMeters = Units.inchesToMeters(4.0) * 7.36/7.65 /* empirical correction */;
-			public static final double mu = 0.5; /* 70/83.2;  */
+			public static final double mu = 1; /* 70/83.2;  */
 
 			public static final double NEOFreeSpeed = 5676 * (2 * Math.PI) / 60;    // radians/s
 			// Angular speed to translational speed --> v = omega * r / gearing
@@ -81,7 +81,7 @@ public static final class Drivetrain {
 			// Forward: 1.72, 1.71, 1.92, 1.94
 			// Backward: 1.92, 1.92, 2.11, 1.89
 			// Order of modules: (FL, FR, BL, BR)
-			public static final double[] drivekP = {4.5, 4.5, 4.5, 3.5}; //{1.82/100, 1.815/100, 2.015/100, 1.915/100};
+			public static final double[] drivekP = {2.8, 2.8, 2.8, 2.8}; //{1.82/100, 1.815/100, 2.015/100, 1.915/100};
 			public static final double[] drivekI = {0, 0, 0, 0};
 			public static final double[] drivekD = {0, 0, 0, 0};
 			public static final boolean[] driveInversion = {false, false, false, false};
@@ -136,11 +136,11 @@ public static final class Drivetrain {
 
 			//#region Command Constants
 
-			public static final double kNormalDriveSpeed = 1; // Percent Multiplier
-			public static final double kNormalDriveRotation = 0.5; // Percent Multiplier
-			public static final double kSlowDriveSpeed = 0.4; // Percent Multiplier
-			public static final double kSlowDriveRotation = 0.250; // Percent Multiplier
-			public static final double kAlignMultiplier = 1D/3D;
+			public static  double kNormalDriveSpeed = 1; // Percent Multiplier
+			public static  double kNormalDriveRotation = 0.5; // Percent Multiplier
+			public static  double kSlowDriveSpeed = 0.4; // Percent Multiplier
+			public static  double kSlowDriveRotation = 0.250; // Percent Multiplier
+			public static  double kAlignMultiplier = 1D/3D;
 			public static final double kAlignForward = 0.6;
 
 			public static final double wheelTurnDriveSpeed = 0.0001; // Meters / Second ; A non-zero speed just used to orient the wheels to the correct angle. This should be very small to avoid actually moving the robot.

src/main/java/org/carlmontrobotics/commands/TeleopDrive.java

@@ -11,19 +11,21 @@
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 
 public class TeleopDrive extends Command {
 
-  private static final double robotPeriod = Robot.robot.getPeriod();
+  private static double robotPeriod = Robot.robot.getPeriod();
   private final Drivetrain drivetrain;
   private DoubleSupplier fwd;
   private DoubleSupplier str;
   private DoubleSupplier rcw;
   private BooleanSupplier slow;
   private double currentForwardVel = 0;
   private double currentStrafeVel = 0;
+  private double prevTimestamp;
 
   /**
    * Creates a new TeleopDrive.
@@ -39,12 +41,26 @@ public TeleopDrive(Drivetrain drivetrain, DoubleSupplier fwd, DoubleSupplier str
   // Called when the command is initially scheduled.
   @Override
   public void initialize() {
+    SmartDashboard.putNumber("slow turn const", kSlowDriveRotation);
+    SmartDashboard.putNumber("slow speed const", kSlowDriveSpeed);
+    SmartDashboard.putNumber("normal turn const", kNormalDriveRotation);
+    SmartDashboard.putNumber("normal speed const", kNormalDriveSpeed);
+    prevTimestamp = Timer.getFPGATimestamp();
   }
 
   // Called every time the scheduler runs while the command is scheduled.
   @Override
   public void execute() {
     double[] speeds = getRequestedSpeeds();
+    double currentTime = Timer.getFPGATimestamp();
+    robotPeriod = currentTime - prevTimestamp;
+    SmartDashboard.putNumber("Elapsed time", currentTime - prevTimestamp);
+    prevTimestamp = currentTime;
+    kSlowDriveRotation = SmartDashboard.getNumber("slow turn const", kSlowDriveRotation);
+    kSlowDriveSpeed = SmartDashboard.getNumber("slow speed const", kSlowDriveSpeed);
+    kNormalDriveRotation = SmartDashboard.getNumber("normal turn const", kNormalDriveRotation);
+    kNormalDriveSpeed = SmartDashboard.getNumber("normal speed const", kNormalDriveSpeed);
+
 
     SmartDashboard.putNumber("fwd", speeds[0]);
     SmartDashboard.putNumber("strafe", speeds[1]);
@@ -88,9 +104,9 @@ public double[] getRequestedSpeeds() {
       currentForwardVel = forward;
       currentStrafeVel = strafe;
     }
-
     // ATM, there is no rotational acceleration limit
-
+    // currentForwardVel = forward;
+    // currentStrafeVel = strafe;
     // If the above math works, no velocity should be greater than the max velocity, so we don't need to limit it.
 
     return new double[] {currentForwardVel, currentStrafeVel, -rotateClockwise};