Ri end

.travis.yml

@@ -22,5 +22,6 @@ install:
   - sed -i 's/compiler="javac1.8"//g' wpilib/ant/build.xml
   - sed -i 's/compiler="javac1.8"//g' *.xml
   - sed -i 's/compiler="javac1.8"//g' FRamework/*.xml
+  - ant checkstyle
   - ant compile
   - ant checkstyle

install-ivy.xml

@@ -28,7 +28,7 @@
     <!-- try to load ivy here from ivy home, in case the user has not already dropped
             it into ant's lib dir (note that the latter copy will always take precedence).
             We will not fail as long as local lib dir exists (it may be empty) and
-            ivy is in at least one of ant's lib dir or the local lib dir. -->
+            ivy is in at least one of ant's lib dir or the local dir. -->
     <path id="ivy.lib.path">
       <fileset dir="${ivy.jar.dir}" includes="*.jar"/>
     </path>

ivy.xml

@@ -12,5 +12,6 @@
     <dependency org="junit" name="junit" rev="4.12"/>
     <dependency org="org.apache.ant" name="ant-junit" rev="1.9.7"/>
     <dependency org="com.puppycrawl.tools" name="checkstyle" rev="7.4"/>
+    <dependency org="com.googlecode.json-simple" name="json-simple" rev="1.1.1"/>
   </dependencies>
 </ivy-module>

src/com/libKudos254/Rotation2d.java

@@ -0,0 +1,113 @@
+package com.libKudos254;
+
+import java.text.DecimalFormat;
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ *
+ * Inspired by Sophus (https://github.com/strasdat/Sophus/tree/master/sophus)
+ */
+public class Rotation2d {
+  protected static final double kEpsilon = 1E-9;
+
+  protected double cos_angle_;
+  protected double sin_angle_;
+
+  public Rotation2d() {
+    this(1, 0, false);
+  }
+
+  public Rotation2d(double x, double y, boolean normalize) {
+    cos_angle_ = x;
+    sin_angle_ = y;
+    if (normalize) {
+      normalize();
+    }
+  }
+
+  public Rotation2d(Rotation2d other) {
+    cos_angle_ = other.cos_angle_;
+    sin_angle_ = other.sin_angle_;
+  }
+
+  public static Rotation2d fromRadians(double angle_radians) {
+    return new Rotation2d(Math.cos(angle_radians), Math.sin(angle_radians), false);
+  }
+
+  public static Rotation2d fromDegrees(double angle_degrees) {
+    return fromRadians(Math.toRadians(angle_degrees));
+  }
+
+  /**
+   * From trig, we know that sin^2 + cos^2 == 1, but as we do math on this
+   * object we might accumulate rounding errors. Normalizing forces us to
+   * re-scale the sin and cos to reset rounding errors.
+   */
+  public void normalize() {
+    double magnitude = Math.hypot(cos_angle_, sin_angle_);
+    if (magnitude > kEpsilon) {
+      sin_angle_ /= magnitude;
+      cos_angle_ /= magnitude;
+    } else {
+      sin_angle_ = 0;
+      cos_angle_ = 1;
+    }
+  }
+
+  public double cos() {
+    return cos_angle_;
+  }
+
+  public double sin() {
+    return sin_angle_;
+  }
+
+  public double tan() {
+    if (cos_angle_ < kEpsilon) {
+      if (sin_angle_ >= 0.0) {
+        return Double.POSITIVE_INFINITY;
+      } else {
+        return Double.NEGATIVE_INFINITY;
+      }
+    }
+    return sin_angle_ / cos_angle_;
+  }
+
+  public double getRadians() {
+    return Math.atan2(sin_angle_, cos_angle_);
+  }
+
+  public double getDegrees() {
+    return Math.toDegrees(getRadians());
+  }
+
+  /**
+   * We can rotate this Rotation2d by adding together the effects of it and
+   * another rotation.
+   *
+   * @param other
+   *            The other rotation. See:
+   *            https://en.wikipedia.org/wiki/Rotation_matrix
+   * @return This rotation rotated by other.
+   */
+  public Rotation2d rotateBy(Rotation2d other) {
+    return new Rotation2d(cos_angle_ * other.cos_angle_ - sin_angle_ * other.sin_angle_,
+        cos_angle_ * other.sin_angle_ + sin_angle_ * other.cos_angle_, true);
+  }
+
+  /**
+   * The inverse of a Rotation2d "undoes" the effect of this rotation.
+   *
+   * @return The opposite of this rotation.
+   */
+  public Rotation2d inverse() {
+    return new Rotation2d(cos_angle_, -sin_angle_, false);
+  }
+
+  @Override
+  public String toString() {
+    final DecimalFormat fmt = new DecimalFormat("#0.000");
+    return "(" + fmt.format(getDegrees()) + " deg)";
+  }
+}

src/com/libKudos254/vision/AdbBridge.java

@@ -0,0 +1,80 @@
+package com.libKudos254.vision;
+
+import java.io.IOException;
+import java.nio.file.Path;
+import java.nio.file.Paths;
+
+/**
+ * AdbBridge interfaces to an Android Debug Bridge (adb) binary, which is needed
+ * to communicate to Android devices over USB.
+ *
+ * adb binary provided by https://github.com/Spectrum3847/RIOdroid
+ */
+public class AdbBridge {
+    Path bin_location_;
+    public final static Path DEFAULT_LOCATION = Paths.get("/usr/bin/adb");
+
+    public AdbBridge() {
+        Path adb_location;
+        String env_val = System.getenv("FRC_ADB_LOCATION");
+        if (env_val == null || "".equals(env_val)) {
+            adb_location = DEFAULT_LOCATION;
+        } else {
+            adb_location = Paths.get(env_val);
+        }
+        bin_location_ = adb_location;
+    }
+
+    public AdbBridge(Path location) {
+        bin_location_ = location;
+    }
+
+    private boolean runCommand(String args) {
+        Runtime r = Runtime.getRuntime();
+        String cmd = bin_location_.toString() + " " + args;
+
+        try {
+            Process p = r.exec(cmd);
+            p.waitFor();
+        } catch (IOException e) {
+            System.err.println("AdbBridge: Could not run command " + cmd);
+            e.printStackTrace();
+            return false;
+        } catch (InterruptedException e) {
+            System.err.println("AdbBridge: Could not run command " + cmd);
+            e.printStackTrace();
+            return false;
+        }
+        return true;
+    }
+
+    public void start() {
+        System.out.println("Starting adb");
+        runCommand("start");
+    }
+
+    public void stop() {
+        System.out.println("Stopping adb");
+        runCommand("kill-server");
+    }
+
+    public void restartAdb() {
+        System.out.println("Restarting adb");
+        stop();
+        start();
+    }
+
+    public void portForward(int local_port, int remote_port) {
+        runCommand("forward tcp:" + local_port + " tcp:" + remote_port);
+    }
+
+    public void reversePortForward(int remote_port, int local_port) {
+        runCommand("reverse tcp:" + remote_port + " tcp:" + local_port);
+    }
+
+    public void restartApp() {
+        System.out.println("Restarting app");
+        runCommand("shell am force-stop com.team254.cheezdroid \\; "
+                + "am start com.team254.cheezdroid/com.team254.cheezdroid.VisionTrackerActivity");
+    }
+}

src/com/libKudos254/vision/CrashTracker.java

@@ -0,0 +1,66 @@
+package com.libKudos254.vision;
+
+import java.io.*;
+import java.util.Date;
+import java.util.UUID;
+
+/**
+ * Tracks start-up and caught crash events, logging them to a file which dosn't
+ * roll over
+ */
+public class CrashTracker {
+
+  private static final UUID RUN_INSTANCE_UUID = UUID.randomUUID();
+
+  public static void logRobotStartup() {
+    logMarker("robot startup");
+  }
+
+  public static void logRobotConstruction() {
+    logMarker("robot startup");
+  }
+
+  public static void logRobotInit() {
+    logMarker("robot init");
+  }
+
+  public static void logTeleopInit() {
+    logMarker("teleop init");
+  }
+
+  public static void logAutoInit() {
+    logMarker("auto init");
+  }
+
+  public static void logDisabledInit() {
+    logMarker("disabled init");
+  }
+
+  public static void logThrowableCrash(Throwable throwable) {
+    logMarker("Exception", throwable);
+  }
+
+  private static void logMarker(String mark) {
+    logMarker(mark, null);
+  }
+
+  private static void logMarker(String mark, Throwable nullableException) {
+
+    try (PrintWriter writer = new PrintWriter(new FileWriter("/home/lvuser/crash_tracking.txt", true))) {
+      writer.print(RUN_INSTANCE_UUID.toString());
+      writer.print(", ");
+      writer.print(mark);
+      writer.print(", ");
+      writer.print(new Date().toString());
+
+      if (nullableException != null) {
+        writer.print(", ");
+        nullableException.printStackTrace(writer);
+      }
+
+      writer.println();
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+}