created TimeStats class and used it in DrivetrainHardware

src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/DrivetrainHardware.scala

@@ -35,6 +35,8 @@ case class DrivetrainHardware(leftBack: CANTalon, leftFront: CANTalon,
 
   val wheelRadius = props.wheelDiameter / 2
   val track = props.track
+  val tstats = new TimeStats(200, 500)
+  val h = new Histogram(3000, 7000, 40)
 
   val rootDataStream = Stream.periodic(period)(
     DrivetrainData(
@@ -60,6 +62,15 @@ case class DrivetrainHardware(leftBack: CANTalon, leftFront: CANTalon,
 
   override lazy val turnVelocity: Stream[AngularVelocity] = rootDataStream.map(_.gyroVelocities).map(_.z)
   override lazy val turnPosition: Stream[Angle] = turnVelocity.integral
+
+  val handle = rootDataStream.originTimeStream.get.foreach((t: Time) => {
+    tstats.record(t.toMilliseconds)
+  })
+
+  val  handle2 = rootDataStream.originTimeStream.get.sliding(2).foreach(times => {
+    h.record((times(1) - times(0)).toMilliseconds)
+  })
+
 }
 
 object DrivetrainHardware {

src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/Histogram.java

@@ -0,0 +1,60 @@
+package com.lynbrookrobotics.seventeen.drivetrain;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import edu.wpi.first.wpilibj.Timer;
+
+public class Histogram {
+
+    protected final long[] histogram;
+    public final double min, max;
+    public final double interval;
+    public final int bins;
+    PrintWriter p;
+
+    public Histogram(final double min, final double max, final int bins) {
+        this.min = min;
+        this.max = max;
+        this.bins = bins;
+        interval = (max - min) / bins;
+        histogram = new long[bins + 2]; // bins and "less than min" and "greater than max"
+        try {
+            p = new PrintWriter(new File("/home/lvuser/timelog"));
+        } catch (FileNotFoundException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public void record(Double t) {
+        double value = t;
+        if (value < min) histogram[0]++;
+        else if (value > max) histogram[bins + 1]++;
+        else histogram[1 + (int) ((value - min) / interval)]++;
+    }
+
+    public String toString() {
+        final StringBuilder sb = new StringBuilder();
+        sb.append("<");
+        sb.append(min);
+        sb.append(" : ");
+        sb.append(histogram[0]);
+        sb.append('\n');
+
+        for (int i = 1; i < bins + 1; i++) {
+            sb.append((float) (min + (interval * (i - 1))));
+            sb.append(" to ");
+            sb.append((float) (min + (interval * i)));
+            sb.append(" : ");
+            sb.append(histogram[i]);
+            sb.append('\n');
+        }
+
+        sb.append(">");
+        sb.append(max);
+        sb.append(" : ");
+        sb.append(histogram[bins+1]);
+        sb.append('\n');
+
+        return sb.toString();
+    }
+}

src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/TimeStats.java

@@ -0,0 +1,47 @@
+package com.lynbrookrobotics.seventeen.drivetrain;
+import java.io.File;
+import java.io.FileNotFoundException;
+import java.io.PrintWriter;
+import edu.wpi.first.wpilibj.Timer;
+
+
+public class TimeStats {
+    double min, max, avg, sq;
+    long count, logcount, initcount;
+    double prev;
+    PrintWriter p;
+
+    public TimeStats(int l, int i) {
+        reset();
+        logcount = l; initcount = i; count = 0; prev = 0; avg = 0; sq = 0;
+        try {
+            p = new PrintWriter(new File("/home/lvuser/timelog"));
+        } catch (FileNotFoundException e) {
+            e.printStackTrace();
+        }
+    }
+
+    public void reset() {
+        min = 1E6; max = 0;
+    }
+
+    public void record(double t) {
+        double timer = t;
+        double duration = timer - prev;
+        if (count > initcount) {
+            if (duration > max) max = duration;
+            if (duration < min) min = duration;
+            double avgNew = (avg*(count-initcount) + duration) / (1.0 + count - initcount);
+            sq += (duration - avg) * (duration - avgNew);
+            avg = avgNew;
+
+            if ((count % logcount) == 0) {
+                p.println("Max " + max + "; Min " + min + "; Avg " +
+                        avg + "; Count " + count + "; stDev " + Math.sqrt(sq / (count - initcount)));
+                reset();
+            }
+        }
+        prev = timer;
+        count ++;
+    }
+}