diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/DrivetrainHardware.scala b/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/DrivetrainHardware.scala index 920c9b3..89c1277 100644 --- a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/DrivetrainHardware.scala +++ b/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 { diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/Histogram.java b/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/Histogram.java new file mode 100644 index 0000000..bdd27d0 --- /dev/null +++ b/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(); + } +} diff --git a/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/TimeStats.java b/src/main/scala/com/lynbrookrobotics/seventeen/drivetrain/TimeStats.java new file mode 100644 index 0000000..923527d --- /dev/null +++ b/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 ++; + } +} \ No newline at end of file