[WIP] Units Library

build.gradle

@@ -47,7 +47,6 @@ dependencies {
     nativeZip wpi.deps.wpilibJni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.wpilibJni(wpi.platforms.desktop)
 
-
     implementation wpi.deps.vendor.java()
     nativeZip wpi.deps.vendor.jni(wpi.platforms.roborio)
     nativeDesktopZip wpi.deps.vendor.jni(wpi.platforms.desktop)

src/main/java/org/team4159/frc/robot/Constants.java

@@ -51,5 +51,4 @@ public final static class CONTROLS {
     public static final int LEFT_JOY = 0;
     public static final int RIGHT_JOY = 1;
   }
-
 }

src/main/java/org/team4159/frc/robot/subsystems/Drivetrain.java

@@ -183,4 +183,4 @@ public DifferentialDriveWheelSpeeds getWheelSpeeds() {
   public double getDirection() {
     return pigeon.getFusedHeading();
   }
-}
+}

src/main/java/org/team4159/lib/logging/CSVWriter.java

@@ -0,0 +1,38 @@
+package org.team4159.lib.logging;
+
+import java.io.File;
+import java.io.FileWriter;
+import java.io.IOException;
+
+public class CSVWriter {
+  private FileWriter writer;
+
+  public CSVWriter(File file) {
+    try {
+      writer = new FileWriter(file);
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+
+  public void write(Object... columns) {
+    try {
+      for (Object column : columns) {
+        writer.append(column.toString());
+        writer.append(",");
+      }
+      writer.append("\n");
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+
+  public void close() {
+    try {
+      writer.flush();
+      writer.close();
+    } catch (IOException e) {
+      e.printStackTrace();
+    }
+  }
+}

src/main/java/org/team4159/lib/math/Epsilon.java

@@ -0,0 +1,5 @@
+package org.team4159.lib.math;
+
+public class Epsilon {
+  public static final double kEpsilon = 1E-9;
+}

src/main/java/org/team4159/lib/math/units/Ampere.java

@@ -0,0 +1,8 @@
+package org.team4159.lib.math.units;
+
+class Ampere extends Unit {
+  @Override
+  public String symbol() {
+    return "A";
+  }
+}

src/main/java/org/team4159/lib/math/units/Kilogram.java

@@ -0,0 +1,8 @@
+package org.team4159.lib.math.units;
+
+class Kilogram extends Unit {
+  @Override
+  public String symbol() {
+    return "kg";
+  }
+}

src/main/java/org/team4159/lib/math/units/Meter.java

@@ -0,0 +1,8 @@
+package org.team4159.lib.math.units;
+
+class Meter extends Unit {
+  @Override
+  public String symbol() {
+    return "m";
+  }
+}

src/main/java/org/team4159/lib/math/units/MultiUnit.java

@@ -0,0 +1,68 @@
+package org.team4159.lib.math.units;
+
+import java.util.*;
+
+class MultiUnit extends Unit {
+  private Map<Unit, Integer> units;
+
+  MultiUnit(Map<Unit, Integer> units) {
+    this.units = units;
+  }
+
+  @Override
+  public Unit mult(Unit other) {
+    Map<Unit, Integer> new_units = getUnits();
+    if (other instanceof MultiUnit) {
+      for (Map.Entry<Unit, Integer> unit_entry: ((MultiUnit) other).getUnits().entrySet()) {
+        new_units.merge(unit_entry.getKey(), unit_entry.getValue(), Integer::sum);
+        if (new_units.get(unit_entry.getKey()) == 0) new_units.remove(unit_entry.getKey());
+      }
+    } else {
+      new_units.merge(other, 1, Integer::sum);
+    }
+    return new MultiUnit(new_units);
+  }
+
+  @Override
+  public Unit inv() {
+    HashMap<Unit, Integer> inverse_units = new HashMap<>();
+    for (Map.Entry<Unit, Integer> unit_entry: getUnits().entrySet()) {
+      units.put(unit_entry.getKey(), -unit_entry.getValue());
+    }
+    return new MultiUnit(inverse_units);
+  }
+
+  @Override
+  public String symbol() {
+    ArrayList<Map.Entry<Unit, Integer>> sorted_units = new ArrayList<>(getUnits().entrySet());
+    sorted_units.sort((a, b) -> {
+      int compare = a.getValue() - b.getValue();
+      if (compare == 0) compare = b.getKey().symbol().compareTo(a.getKey().symbol());
+      return compare;
+    });
+    StringBuilder name = new StringBuilder();
+    boolean slashed = false;
+    for (int i =  sorted_units.size() - 1; i >= 0; i--) {
+      Map.Entry<Unit, Integer> unit_entry = sorted_units.get(i);
+      if (!slashed && unit_entry.getValue() < 0) {
+        if (i == sorted_units.size() - 1) name.append(1);
+        name.append("/");
+        slashed = true;
+      }
+      int order = Math.abs(unit_entry.getValue());
+      if (order > 0) name.append(unit_entry.getKey().symbol());
+      if (order > 1) name.append("^").append(order);
+    }
+    return name.toString();
+  }
+
+  public Map<Unit, Integer> getUnits() {
+    return new HashMap<>(units);
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (!(obj instanceof MultiUnit)) return false;
+    return units.equals(((MultiUnit) obj).units);
+  }
+}

src/main/java/org/team4159/lib/math/units/Quantity.java

@@ -0,0 +1,31 @@
+package org.team4159.lib.math.units;
+
+public class Quantity {
+  private Unit unit;
+  private double amount;
+
+  public Quantity(double amount, Unit unit) {
+    this.amount = amount;
+    this.unit = unit;
+  }
+
+  public Quantity mult(Quantity other) {
+    return new Quantity(amount * other.amount, unit.mult(other.unit));
+  }
+
+  public Quantity div(Quantity other) {
+    return mult(new Quantity(1.0 / other.amount, other.unit.inv()));
+  }
+
+  @Override
+  public String toString() {
+    return amount + " " + unit.symbol();
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (!(obj instanceof Quantity)) return false;
+    Quantity other = (Quantity) obj;
+    return this.amount == other.amount && this.unit.equals(other.unit);
+  }
+}

src/main/java/org/team4159/lib/math/units/Second.java

@@ -0,0 +1,8 @@
+package org.team4159.lib.math.units;
+
+class Second extends Unit {
+  @Override
+  public String symbol() {
+    return "s";
+  }
+}

src/main/java/org/team4159/lib/math/units/Unit.java

@@ -0,0 +1,36 @@
+package org.team4159.lib.math.units;
+
+import java.util.Map;
+
+public class Unit {
+  public String symbol() {
+    return "ul";
+  }
+
+  public Unit mult(Unit other) {
+    if (other instanceof MultiUnit) {
+      return other.mult(this);
+    } else {
+      return new MultiUnit(Map.of(this, 1, other, 1));
+    }
+  }
+
+  public Unit div(Unit other) {
+    return mult(other.inv());
+  }
+
+  public Unit inv() {
+    return new MultiUnit(Map.of(this, -1));
+  }
+
+  @Override
+  public String toString() {
+    return symbol();
+  }
+
+  @Override
+  public boolean equals(Object obj) {
+    if (!(obj instanceof Unit)) return false;
+    return symbol().equals(((Unit) obj).symbol());
+  }
+}

src/main/java/org/team4159/lib/math/units/Units.java

@@ -0,0 +1,21 @@
+package org.team4159.lib.math.units;
+
+/*
+* Inspired by https://github.com/team5419/fault/tree/master/src/main/kotlin/org/team5419/fault/math/units
+*/
+
+import org.team4159.lib.math.units.derivatives.Acceleration;
+import org.team4159.lib.math.units.derivatives.Jerk;
+
+public class Units {
+  public static final Unit UNIT = new Unit();
+
+  public static final Unit SECOND = new Second();
+  public static final Unit KILOGRAM = new Kilogram();
+  public static final Unit METER = new Meter();
+  public static final Unit AMPERE = new Ampere();
+
+  // derived
+  public static final Unit NEWTON = Acceleration.of(KILOGRAM.mult(METER));
+  public static final Unit VOLT = Jerk.of(KILOGRAM.mult(METER).mult(METER).div(AMPERE));
+}

src/main/java/org/team4159/lib/math/units/derivatives/Acceleration.java

@@ -0,0 +1,10 @@
+package org.team4159.lib.math.units.derivatives;
+
+import org.team4159.lib.math.units.Unit;
+import static org.team4159.lib.math.units.Units.*;
+
+public class Acceleration {
+  public static Unit of(Unit unit) {
+    return unit.div(SECOND).div(SECOND);
+  }
+}

src/main/java/org/team4159/lib/math/units/derivatives/Jerk.java

@@ -0,0 +1,10 @@
+package org.team4159.lib.math.units.derivatives;
+
+import org.team4159.lib.math.units.Unit;
+import static org.team4159.lib.math.units.Units.*;
+
+public class Jerk {
+  public static Unit of(Unit unit) {
+    return unit.div(SECOND).div(SECOND).div(SECOND);
+  }
+}

src/main/java/org/team4159/lib/math/units/derivatives/Velocity.java

@@ -0,0 +1,10 @@
+package org.team4159.lib.math.units.derivatives;
+
+import org.team4159.lib.math.units.Unit;
+import static org.team4159.lib.math.units.Units.*;
+
+public class Velocity {
+  public static Unit of(Unit unit) {
+    return unit.div(SECOND);
+  }
+}