diff --git a/WilyCore/build.gradle b/WilyCore/build.gradle
index d852a6b04d8f..d575434548bb 100644
--- a/WilyCore/build.gradle
+++ b/WilyCore/build.gradle
@@ -19,24 +19,60 @@ kotlin {
sourceSets {
main {
java {
- srcDirs = ['src/main/java', {System.getenv('SRC_ROOT')}]
+ srcDirs = ['src/main/java', '../Sidekick/src/main/java', {System.getenv('SRC_ROOT')}]
}
}
}
+// LWJGL for gamepad input, generated from https://www.lwjgl.org/customize for just the "GLFW"
+// module for MacOS arm64 and Windows x64.
+import org.gradle.internal.os.OperatingSystem
+
+project.ext.lwjglVersion = "3.3.6"
+
+switch (OperatingSystem.current()) {
+ case OperatingSystem.LINUX:
+ project.ext.lwjglNatives = "natives-linux"
+ def osArch = System.getProperty("os.arch")
+ if (osArch.startsWith("arm") || osArch.startsWith("aarch64")) {
+ project.ext.lwjglNatives += osArch.contains("64") || osArch.startsWith("armv8") ? "-arm64" : "-arm32"
+ } else if (osArch.startsWith("ppc")) {
+ project.ext.lwjglNatives += "-ppc64le"
+ } else if (osArch.startsWith("riscv")) {
+ project.ext.lwjglNatives += "-riscv64"
+ }
+ break
+ case OperatingSystem.MAC_OS:
+ project.ext.lwjglNatives = "natives-macos-arm64"
+ break
+ case OperatingSystem.WINDOWS:
+ project.ext.lwjglNatives = "natives-windows"
+ break
+}
+
repositories {
- jcenter()
+ mavenCentral()
maven { url = 'https://maven.brott.dev/' }
}
dependencies {
implementation project(path: ':WilyWorks')
implementation "com.acmerobotics.roadrunner:core:1.0.0" // For Pose2d, PoseVelocity2d, etc.
- // implementation "uk.co.electronstudio.sdl2gdx:sdl2gdx:1.0.+" // For gamepad control
implementation "org.reflections:reflections:0.10.2" // For enumerating classes with annotations
implementation "org.jetbrains.kotlin:kotlin-stdlib-jdk8" // For enumerating all classes
implementation "com.google.code.gson:gson:2.11.0" // For serializing Java objects to JSON
+ implementation 'net.bytebuddy:byte-buddy:1.15.10' // For proxying classes rather than interfaces
+ implementation 'org.objenesis:objenesis:3.2' // Instantiate class without invoking a constructor
+ implementation 'org.json:json:20250107' // Or any recent version
+
+ // Lwjgl for gamepad input:
+ implementation platform("org.lwjgl:lwjgl-bom:$lwjglVersion")
+ implementation "org.lwjgl:lwjgl"
+ implementation "org.lwjgl:lwjgl-glfw"
+ runtimeOnly "org.lwjgl:lwjgl::$lwjglNatives"
+ runtimeOnly "org.lwjgl:lwjgl-glfw::$lwjglNatives"
}
+
compileKotlin {
kotlinOptions {
jvmTarget = "1.8"
diff --git a/WilyCore/src/main/java/android/content/Context.java b/WilyCore/src/main/java/android/content/Context.java
index 7abce111a1bf..51e337c42ec1 100644
--- a/WilyCore/src/main/java/android/content/Context.java
+++ b/WilyCore/src/main/java/android/content/Context.java
@@ -1,6 +1,8 @@
package android.content;
import android.app.Activity;
+import android.content.pm.PackageManager;
+import android.content.res.AssetManager;
import java.io.File;
@@ -13,4 +15,6 @@ public String getPackageName() {
}
public static final int MODE_PRIVATE = 0;
public File getDir(String var1, int var2) { return null; }
+ public AssetManager getAssets() { return null; }
+ public PackageManager getPackageManager() { return new PackageManager(); }
}
diff --git a/WilyCore/src/main/java/android/content/pm/PackageInfo.java b/WilyCore/src/main/java/android/content/pm/PackageInfo.java
new file mode 100644
index 000000000000..d378541d4859
--- /dev/null
+++ b/WilyCore/src/main/java/android/content/pm/PackageInfo.java
@@ -0,0 +1,10 @@
+//
+// Source code recreated from a .class file by IntelliJ IDEA
+// (powered by FernFlower decompiler)
+//
+
+package android.content.pm;
+
+public class PackageInfo {
+ public String versionName = "9.0.1";
+}
diff --git a/WilyCore/src/main/java/android/content/pm/PackageManager.java b/WilyCore/src/main/java/android/content/pm/PackageManager.java
new file mode 100644
index 000000000000..d352cff8f199
--- /dev/null
+++ b/WilyCore/src/main/java/android/content/pm/PackageManager.java
@@ -0,0 +1,12 @@
+//
+// Source code recreated from a .class file by IntelliJ IDEA
+// (powered by FernFlower decompiler)
+//
+
+package android.content.pm;
+
+import android.annotation.NonNull;
+
+public class PackageManager {
+ public PackageInfo getPackageInfo(@NonNull String var1, int var2) { return new PackageInfo(); }
+}
diff --git a/WilyCore/src/main/java/android/content/res/AssetManager.java b/WilyCore/src/main/java/android/content/res/AssetManager.java
new file mode 100644
index 000000000000..620a5840785a
--- /dev/null
+++ b/WilyCore/src/main/java/android/content/res/AssetManager.java
@@ -0,0 +1,13 @@
+package android.content.res;
+
+import android.annotation.NonNull;
+
+import java.io.IOException;
+import java.io.InputStream;
+
+public class AssetManager {
+ @NonNull
+ public InputStream open(@NonNull String fileName) throws IOException {
+ throw new RuntimeException("Wily Works Stub!");
+ }
+}
diff --git a/WilyCore/src/main/java/android/graphics/Bitmap.java b/WilyCore/src/main/java/android/graphics/Bitmap.java
index a6d567772595..6fe95dfc63a6 100644
--- a/WilyCore/src/main/java/android/graphics/Bitmap.java
+++ b/WilyCore/src/main/java/android/graphics/Bitmap.java
@@ -1,4 +1,239 @@
package android.graphics;
+import android.annotation.NonNull;
+
+import java.io.OutputStream;
+import java.nio.Buffer;
+
public class Bitmap {
+ @NonNull
+ public static final int DENSITY_NONE = 0;
+
+ Bitmap() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getDensity() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setDensity(int density) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void reconfigure(int width, int height, Config config) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setWidth(int width) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setHeight(int height) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setConfig(Config config) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void recycle() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isRecycled() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getGenerationId() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void copyPixelsToBuffer(Buffer dst) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void copyPixelsFromBuffer(Buffer src) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Bitmap copy(Config config, boolean isMutable) {
+ throw new RuntimeException("Stub!");
+ }
+
+
+ public static Bitmap createScaledBitmap(@NonNull Bitmap src, int dstWidth, int dstHeight, boolean filter) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(@NonNull Bitmap src) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(@NonNull Bitmap source, int x, int y, int width, int height) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(int width, int height, @NonNull Config config) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(int width, int height, @NonNull Config config, boolean hasAlpha) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(@NonNull int[] colors, int offset, int stride, int width, int height, @NonNull Config config) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static Bitmap createBitmap(@NonNull int[] colors, int width, int height, Config config) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public byte[] getNinePatchChunk() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean compress(CompressFormat format, int quality, OutputStream stream) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isMutable() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isPremultiplied() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setPremultiplied(boolean premultiplied) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getWidth() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getHeight() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getScaledWidth(Canvas canvas) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getScaledHeight(Canvas canvas) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getScaledWidth(int targetDensity) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getScaledHeight(int targetDensity) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getRowBytes() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getByteCount() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getAllocationByteCount() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Config getConfig() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean hasAlpha() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setHasAlpha(boolean hasAlpha) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean hasMipMap() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setHasMipMap(boolean hasMipMap) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void eraseColor(int c) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void eraseColor(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getPixel(int x, int y) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public Color getColor(int x, int y) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void getPixels(int[] pixels, int offset, int stride, int x, int y, int width, int height) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setPixel(int x, int y, int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setPixels(int[] pixels, int offset, int stride, int x, int y, int width, int height) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int describeContents() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Bitmap extractAlpha() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Bitmap extractAlpha(Paint paint, int[] offsetXY) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean sameAs(Bitmap other) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void prepareToDraw() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static enum CompressFormat {
+ JPEG,
+ PNG,
+ /** @deprecated */
+ @Deprecated
+ WEBP,
+ WEBP_LOSSY,
+ WEBP_LOSSLESS;
+ }
+
+ public static enum Config {
+ ALPHA_8,
+ RGB_565,
+ /** @deprecated */
+ @Deprecated
+ ARGB_4444,
+ ARGB_8888,
+ RGBA_F16,
+ HARDWARE;
+ }
+
}
diff --git a/WilyCore/src/main/java/android/graphics/BitmapFactory.java b/WilyCore/src/main/java/android/graphics/BitmapFactory.java
new file mode 100644
index 000000000000..043fb7891a33
--- /dev/null
+++ b/WilyCore/src/main/java/android/graphics/BitmapFactory.java
@@ -0,0 +1,9 @@
+package android.graphics;
+
+import java.io.InputStream;
+
+public class BitmapFactory {
+ public static Bitmap decodeStream(InputStream is) {
+ throw new RuntimeException("Not Wily Works implemented!");
+ }
+}
diff --git a/WilyCore/src/main/java/android/graphics/Canvas.java b/WilyCore/src/main/java/android/graphics/Canvas.java
index 006df0645204..880ac9d08639 100644
--- a/WilyCore/src/main/java/android/graphics/Canvas.java
+++ b/WilyCore/src/main/java/android/graphics/Canvas.java
@@ -1,4 +1,243 @@
package android.graphics;
+import android.annotation.NonNull;
+import android.annotation.Nullable;
+
public class Canvas {
+ public Canvas(@NonNull Bitmap bitmap) {
+ throw new RuntimeException("Wily Works Stub!");
+ }
+
+ public static final int ALL_SAVE_FLAG = 31;
+
+ public Canvas() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isHardwareAccelerated() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setBitmap(@Nullable Bitmap bitmap) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void enableZ() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void disableZ() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isOpaque() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getWidth() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getHeight() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getDensity() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setDensity(int density) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getMaximumBitmapWidth() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getMaximumBitmapHeight() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int save() {
+ throw new RuntimeException("Stub!");
+ }
+
+ /** @deprecated */
+ @Deprecated
+ public int saveLayer(float left, float top, float right, float bottom, @Nullable Paint paint, int saveFlags) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int saveLayer(float left, float top, float right, float bottom, @Nullable Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+ /** @deprecated */
+ @Deprecated
+ public int saveLayerAlpha(float left, float top, float right, float bottom, int alpha, int saveFlags) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int saveLayerAlpha(float left, float top, float right, float bottom, int alpha) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void restore() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getSaveCount() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void restoreToCount(int saveCount) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void translate(float dx, float dy) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void scale(float sx, float sy) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final void scale(float sx, float sy, float px, float py) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void rotate(float degrees) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final void rotate(float degrees, float px, float py) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void skew(float sx, float sy) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean clipRect(float left, float top, float right, float bottom) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean clipOutRect(float left, float top, float right, float bottom) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean clipRect(int left, int top, int right, int bottom) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean clipOutRect(int left, int top, int right, int bottom) {
+ throw new RuntimeException("Stub!");
+ }
+
+
+ /** @deprecated */
+ @Deprecated
+ public void drawBitmap(@NonNull int[] colors, int offset, int stride, float x, float y, int width, int height, boolean hasAlpha, @Nullable Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ /** @deprecated */
+ @Deprecated
+ public void drawBitmap(@NonNull int[] colors, int offset, int stride, int x, int y, int width, int height, boolean hasAlpha, @Nullable Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawBitmapMesh(@NonNull Bitmap bitmap, int meshWidth, int meshHeight, @NonNull float[] verts, int vertOffset, @Nullable int[] colors, int colorOffset, @Nullable Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawCircle(float cx, float cy, float radius, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawColor(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawColor(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawLine(float startX, float startY, float stopX, float stopY, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawLines(@NonNull float[] pts, int offset, int count, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawLines(@NonNull float[] pts, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+ public void drawOval(float left, float top, float right, float bottom, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawPaint(@NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawPoint(float x, float y, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawPoints(float[] pts, int offset, int count, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawPoints(@NonNull float[] pts, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ /** @deprecated */
+ @Deprecated
+ public void drawPosText(@NonNull char[] text, int index, int count, @NonNull float[] pos, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ /** @deprecated */
+ @Deprecated
+ public void drawPosText(@NonNull String text, @NonNull float[] pos, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawRect(float left, float top, float right, float bottom, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawRGB(int r, int g, int b) {
+ throw new RuntimeException("Stub!");
+ }
+ public void drawRoundRect(float left, float top, float right, float bottom, float rx, float ry, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawText(@NonNull char[] text, int index, int count, float x, float y, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawText(@NonNull String text, float x, float y, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawText(@NonNull String text, int start, int end, float x, float y, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawText(@NonNull CharSequence text, int start, int end, float x, float y, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawTextRun(@NonNull char[] text, int index, int count, int contextIndex, int contextCount, float x, float y, boolean isRtl, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void drawTextRun(@NonNull CharSequence text, int start, int end, int contextStart, int contextEnd, float x, float y, boolean isRtl, @NonNull Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
}
diff --git a/WilyCore/src/main/java/android/graphics/Color.java b/WilyCore/src/main/java/android/graphics/Color.java
index 6eb7ef13a2e1..5ba1ada90ac4 100644
--- a/WilyCore/src/main/java/android/graphics/Color.java
+++ b/WilyCore/src/main/java/android/graphics/Color.java
@@ -1,5 +1,7 @@
package android.graphics;
+import android.annotation.NonNull;
+
public class Color {
public static final int BLACK = -16777216;
public static final int BLUE = -16776961;
@@ -18,6 +20,10 @@ public static int argb(int alpha, int red, int green, int blue) {
return (alpha << 24) | (red << 16) | (green << 8) | blue;
}
+ public static int rgb(int red, int green, int blue) {
+ return (red << 16) | (green << 8) | blue;
+ }
+
public static void colorToHSV(int color, float[] hsv) {
float rNorm = ((color >> 16) & 0xff) / 255.0f;
float gNorm = ((color >> 8) & 0xff) / 255.0f;
@@ -108,4 +114,104 @@ public static int parseColor(String colorString) {
value |= 0xff000000;
return value;
}
+
+ public static float red(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static float green(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static float blue(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static float alpha(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static boolean isSrgb(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static boolean isWideGamut(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static int toArgb(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public static Color valueOf(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public static Color valueOf(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public static Color valueOf(float r, float g, float b) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public static Color valueOf(float r, float g, float b, float a) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static long pack(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static long pack(float red, float green, float blue) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static long pack(float red, float green, float blue, float alpha) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static float luminance(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static int alpha(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static int red(int color) {
+ return (color >> 16) & 0xff;
+ }
+
+ public static int green(int color) {
+ return (color >> 8) & 0xff;
+ }
+
+ public static int blue(int color) {
+ return color & 0xff;
+ }
+
+ public static int rgb(float red, float green, float blue) {
+ return rgb((int) red, (int) green, (int) blue);
+ }
+
+ public static int argb(float alpha, float red, float green, float blue) {
+ return argb((int) alpha, (int) red, (int) green, (int) blue);
+ }
+
+ public static float luminance(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static void RGBToHSV(int red, int green, int blue, float[] hsv) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static int HSVToColor(int alpha, float[] hsv) {
+ throw new RuntimeException("Stub!");
+ }
}
diff --git a/WilyCore/src/main/java/android/graphics/Paint.java b/WilyCore/src/main/java/android/graphics/Paint.java
new file mode 100644
index 000000000000..6268ac2ab7ef
--- /dev/null
+++ b/WilyCore/src/main/java/android/graphics/Paint.java
@@ -0,0 +1,509 @@
+package android.graphics;
+
+import android.annotation.NonNull;
+import android.annotation.Nullable;
+
+import java.util.Locale;
+
+public class Paint {
+ public static final int ANTI_ALIAS_FLAG = 1;
+ public static final int CURSOR_AFTER = 0;
+ public static final int CURSOR_AT = 4;
+ public static final int CURSOR_AT_OR_AFTER = 1;
+ public static final int CURSOR_AT_OR_BEFORE = 3;
+ public static final int CURSOR_BEFORE = 2;
+ public static final int DEV_KERN_TEXT_FLAG = 256;
+ public static final int DITHER_FLAG = 4;
+ public static final int EMBEDDED_BITMAP_TEXT_FLAG = 1024;
+ public static final int END_HYPHEN_EDIT_INSERT_ARMENIAN_HYPHEN = 3;
+ public static final int END_HYPHEN_EDIT_INSERT_HYPHEN = 2;
+ public static final int END_HYPHEN_EDIT_INSERT_MAQAF = 4;
+ public static final int END_HYPHEN_EDIT_INSERT_UCAS_HYPHEN = 5;
+ public static final int END_HYPHEN_EDIT_INSERT_ZWJ_AND_HYPHEN = 6;
+ public static final int END_HYPHEN_EDIT_NO_EDIT = 0;
+ public static final int END_HYPHEN_EDIT_REPLACE_WITH_HYPHEN = 1;
+ public static final int FAKE_BOLD_TEXT_FLAG = 32;
+ public static final int FILTER_BITMAP_FLAG = 2;
+ public static final int HINTING_OFF = 0;
+ public static final int HINTING_ON = 1;
+ public static final int LINEAR_TEXT_FLAG = 64;
+ public static final int START_HYPHEN_EDIT_INSERT_HYPHEN = 1;
+ public static final int START_HYPHEN_EDIT_INSERT_ZWJ = 2;
+ public static final int START_HYPHEN_EDIT_NO_EDIT = 0;
+ public static final int STRIKE_THRU_TEXT_FLAG = 16;
+ public static final int SUBPIXEL_TEXT_FLAG = 128;
+ public static final int UNDERLINE_TEXT_FLAG = 8;
+
+ public Paint() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Paint(int flags) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Paint(Paint paint) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void reset() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void set(Paint src) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getFlags() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setFlags(int flags) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getHinting() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setHinting(int mode) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isAntiAlias() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setAntiAlias(boolean aa) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isDither() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setDither(boolean dither) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isLinearText() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setLinearText(boolean linearText) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isSubpixelText() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setSubpixelText(boolean subpixelText) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isUnderlineText() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getUnderlinePosition() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getUnderlineThickness() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setUnderlineText(boolean underlineText) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isStrikeThruText() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getStrikeThruPosition() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getStrikeThruThickness() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStrikeThruText(boolean strikeThruText) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isFakeBoldText() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setFakeBoldText(boolean fakeBoldText) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public final boolean isFilterBitmap() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setFilterBitmap(boolean filter) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Style getStyle() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStyle(Style style) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getColor() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public long getColorLong() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setColor(int color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setColor(long color) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getAlpha() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setAlpha(int a) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setARGB(int a, int r, int g, int b) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getStrokeWidth() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStrokeWidth(float width) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getStrokeMiter() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStrokeMiter(float miter) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Cap getStrokeCap() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStrokeCap(Cap cap) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Join getStrokeJoin() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStrokeJoin(Join join) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setShadowLayer(float radius, float dx, float dy, int shadowColor) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setShadowLayer(float radius, float dx, float dy, long shadowColor) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void clearShadowLayer() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getShadowLayerRadius() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getShadowLayerDx() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getShadowLayerDy() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getShadowLayerColor() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public long getShadowLayerColorLong() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public Align getTextAlign() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setTextAlign(Align align) {
+ throw new RuntimeException("Stub!");
+ }
+
+ @NonNull
+ public Locale getTextLocale() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setTextLocale(@NonNull Locale locale) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean isElegantTextHeight() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setElegantTextHeight(boolean elegant) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getTextSize() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setTextSize(float textSize) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getTextScaleX() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setTextScaleX(float scaleX) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getTextSkewX() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setTextSkewX(float skewX) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getLetterSpacing() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setLetterSpacing(float letterSpacing) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getWordSpacing() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setWordSpacing(float wordSpacing) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public String getFontFeatureSettings() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setFontFeatureSettings(String settings) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public String getFontVariationSettings() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean setFontVariationSettings(String fontVariationSettings) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getStartHyphenEdit() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getEndHyphenEdit() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setStartHyphenEdit(int startHyphen) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public void setEndHyphenEdit(int endHyphen) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float ascent() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float descent() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getFontMetrics(FontMetrics metrics) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public FontMetrics getFontMetrics() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getFontMetricsInt(FontMetricsInt fmi) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public FontMetricsInt getFontMetricsInt() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getFontSpacing() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float measureText(char[] text, int index, int count) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float measureText(String text, int start, int end) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float measureText(String text) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float measureText(CharSequence text, int start, int end) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int breakText(char[] text, int index, int count, float maxWidth, float[] measuredWidth) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int breakText(CharSequence text, int start, int end, boolean measureForwards, float maxWidth, float[] measuredWidth) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int breakText(String text, boolean measureForwards, float maxWidth, float[] measuredWidth) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextWidths(char[] text, int index, int count, float[] widths) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextWidths(CharSequence text, int start, int end, float[] widths) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextWidths(String text, int start, int end, float[] widths) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextWidths(String text, float[] widths) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getTextRunAdvances(@NonNull char[] chars, int index, int count, int contextIndex, int contextCount, boolean isRtl, @Nullable float[] advances, int advancesIndex) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextRunCursor(@NonNull char[] text, int contextStart, int contextLength, boolean isRtl, int offset, int cursorOpt) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getTextRunCursor(@NonNull CharSequence text, int contextStart, int contextEnd, boolean isRtl, int offset, int cursorOpt) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean hasGlyph(String string) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getRunAdvance(char[] text, int start, int end, int contextStart, int contextEnd, boolean isRtl, int offset) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public float getRunAdvance(CharSequence text, int start, int end, int contextStart, int contextEnd, boolean isRtl, int offset) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getOffsetForAdvance(char[] text, int start, int end, int contextStart, int contextEnd, boolean isRtl, float advance) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public int getOffsetForAdvance(CharSequence text, int start, int end, int contextStart, int contextEnd, boolean isRtl, float advance) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public boolean equalsForTextMeasurement(@NonNull Paint other) {
+ throw new RuntimeException("Stub!");
+ }
+
+ public static enum Align {
+ LEFT,
+ CENTER,
+ RIGHT;
+ }
+
+ public static enum Cap {
+ BUTT,
+ ROUND,
+ SQUARE;
+ }
+
+ public static class FontMetrics {
+ public float ascent;
+ public float bottom;
+ public float descent;
+ public float leading;
+ public float top;
+
+ public FontMetrics() {
+ throw new RuntimeException("Stub!");
+ }
+ }
+
+ public static class FontMetricsInt {
+ public int ascent;
+ public int bottom;
+ public int descent;
+ public int leading;
+ public int top;
+
+ public FontMetricsInt() {
+ throw new RuntimeException("Stub!");
+ }
+
+ public String toString() {
+ throw new RuntimeException("Stub!");
+ }
+ }
+
+ public static enum Join {
+ MITER,
+ ROUND,
+ BEVEL;
+ }
+
+ public static enum Style {
+ FILL,
+ STROKE,
+ FILL_AND_STROKE;
+ }
+}
diff --git a/WilyCore/src/main/java/androidx/annotation/ColorInt.java b/WilyCore/src/main/java/androidx/annotation/ColorInt.java
new file mode 100644
index 000000000000..db72a7bfcf78
--- /dev/null
+++ b/WilyCore/src/main/java/androidx/annotation/ColorInt.java
@@ -0,0 +1,27 @@
+package androidx.annotation;
+
+import static java.lang.annotation.ElementType.FIELD;
+import static java.lang.annotation.ElementType.LOCAL_VARIABLE;
+import static java.lang.annotation.ElementType.METHOD;
+import static java.lang.annotation.ElementType.PARAMETER;
+import static java.lang.annotation.RetentionPolicy.CLASS;
+
+import java.lang.annotation.Documented;
+import java.lang.annotation.Retention;
+import java.lang.annotation.Target;
+
+/**
+ * Denotes that the annotated element represents a packed color
+ * int, {@code AARRGGBB}. If applied to an int array, every element
+ * in the array represents a color integer.
+ *
+ * Example:
+ *
{@code
+ * public abstract void setTextColor(@ColorInt int color);
+ * }
+ */
+@Documented
+@Retention(CLASS)
+@Target({PARAMETER, METHOD, LOCAL_VARIABLE, FIELD})
+public @interface ColorInt {
+}
diff --git a/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java b/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java
index ec625044b355..1f0ba0255db0 100644
--- a/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java
+++ b/WilyCore/src/main/java/com/qualcomm/ftccommon/FtcEventLoop.java
@@ -4,6 +4,6 @@
public class FtcEventLoop {
public OpModeManagerImpl getOpModeManager() {
- return null;
+ return new OpModeManagerImpl();
}
}
diff --git a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java b/WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java
similarity index 68%
rename from WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java
rename to WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java
index f37c1c38c76d..bf02a2732133 100644
--- a/WilyCore/src/main/java/org/swerverobotics/ftc/GoBildaPinpointDriver.java
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/gobilda/GoBildaPinpointDriver.java
@@ -1,28 +1,24 @@
-package org.swerverobotics.ftc;
-
-import static com.qualcomm.robotcore.util.TypeConversion.byteArrayToInt;
+package com.qualcomm.hardware.gobilda;
import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
-import com.qualcomm.robotcore.hardware.I2cAddr;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
-import com.qualcomm.robotcore.util.TypeConversion;
import com.wilyworks.simulator.WilyCore;
import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.robotcore.external.navigation.Pose2D;
+import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
-import java.util.Arrays;
@I2cDeviceType
@DeviceProperties(
- name = "goBILDA® Pinpoint Odometry Computer",
+ name = "Home-compiled goBILDA® Pinpoint Odometry Computer",
xmlTag = "goBILDAPinpoint",
description ="goBILDA® Pinpoint Odometry Computer (IMU Sensor Fusion for 2 Wheel Odometry)"
)
@@ -79,7 +75,7 @@ private enum Register {
X_VELOCITY (11),
Y_VELOCITY (12),
H_VELOCITY (13),
- MM_PER_TICK (14),
+ TICKS_PER_MM (14),
X_POD_OFFSET (15),
Y_POD_OFFSET (16),
YAW_SCALAR (17),
@@ -94,13 +90,14 @@ private enum Register {
//Device Status enum that captures the current fault condition of the device
public enum DeviceStatus{
- NOT_READY (0),
- READY (1),
- CALIBRATING (1 << 1),
- FAULT_X_POD_NOT_DETECTED (1 << 2),
- FAULT_Y_POD_NOT_DETECTED (1 << 3),
- FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3),
- FAULT_IMU_RUNAWAY (1 << 4);
+ NOT_READY(0),
+ READY(1),
+ CALIBRATING(1 << 1),
+ FAULT_X_POD_NOT_DETECTED(1 << 2),
+ FAULT_Y_POD_NOT_DETECTED(1 << 3),
+ FAULT_NO_PODS_DETECTED(1 << 2 | 1 << 3),
+ FAULT_IMU_RUNAWAY(1 << 4),
+ FAULT_BAD_READ(1 << 5);
private final int status;
@@ -121,7 +118,7 @@ public enum GoBildaOdometryPods {
goBILDA_4_BAR_POD;
}
//enum that captures a limited scope of read data. More options may be added in future update
- public enum readData {
+ public enum ReadData {
ONLY_UPDATE_HEADING,
}
@@ -130,7 +127,7 @@ public enum readData {
@param reg the register to write the int to
@param i the integer to write to the register
*/
- private void writeInt(final Register reg, int i){
+ private void writeInt(final GoBildaPinpointDriver.Register reg, int i){
}
/**
@@ -138,7 +135,7 @@ private void writeInt(final Register reg, int i){
* @param reg the register to read from
* @return returns an int that contains the value stored in the read register
*/
- private int readInt(Register reg){
+ private int readInt(GoBildaPinpointDriver.Register reg){
return 0;
}
@@ -157,7 +154,7 @@ private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){
* @return the float value stored in that register
*/
- private float readFloat(Register reg){
+ private float readFloat(GoBildaPinpointDriver.Register reg){
return 0;
}
@@ -176,7 +173,7 @@ private float readFloat(Register reg){
* @param reg the register to write to
* @param bytes the byte array to write
*/
- private void writeByteArray (Register reg, byte[] bytes){
+ private void writeByteArray (GoBildaPinpointDriver.Register reg, byte[] bytes){
}
@@ -185,7 +182,7 @@ private void writeByteArray (Register reg, byte[] bytes){
* @param reg the register to write to
* @param f the float to write
*/
- private void writeFloat (Register reg, float f){
+ private void writeFloat (GoBildaPinpointDriver.Register reg, float f){
}
/**
@@ -193,30 +190,30 @@ private void writeFloat (Register reg, float f){
* @param s int to lookup
* @return the Odometry Computer state
*/
- private DeviceStatus lookupStatus (int s){
- if ((s & DeviceStatus.CALIBRATING.status) != 0){
- return DeviceStatus.CALIBRATING;
+ private GoBildaPinpointDriver.DeviceStatus lookupStatus (int s){
+ if ((s & GoBildaPinpointDriver.DeviceStatus.CALIBRATING.status) != 0){
+ return GoBildaPinpointDriver.DeviceStatus.CALIBRATING;
}
- boolean xPodDetected = (s & DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0;
- boolean yPodDetected = (s & DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0;
+ boolean xPodDetected = (s & GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED.status) == 0;
+ boolean yPodDetected = (s & GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED.status) == 0;
if(!xPodDetected && !yPodDetected){
- return DeviceStatus.FAULT_NO_PODS_DETECTED;
+ return GoBildaPinpointDriver.DeviceStatus.FAULT_NO_PODS_DETECTED;
}
if (!xPodDetected){
- return DeviceStatus.FAULT_X_POD_NOT_DETECTED;
+ return GoBildaPinpointDriver.DeviceStatus.FAULT_X_POD_NOT_DETECTED;
}
if (!yPodDetected){
- return DeviceStatus.FAULT_Y_POD_NOT_DETECTED;
+ return GoBildaPinpointDriver.DeviceStatus.FAULT_Y_POD_NOT_DETECTED;
}
- if ((s & DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){
- return DeviceStatus.FAULT_IMU_RUNAWAY;
+ if ((s & GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY.status) != 0){
+ return GoBildaPinpointDriver.DeviceStatus.FAULT_IMU_RUNAWAY;
}
- if ((s & DeviceStatus.READY.status) != 0){
- return DeviceStatus.READY;
+ if ((s & GoBildaPinpointDriver.DeviceStatus.READY.status) != 0){
+ return GoBildaPinpointDriver.DeviceStatus.READY;
}
else {
- return DeviceStatus.NOT_READY;
+ return GoBildaPinpointDriver.DeviceStatus.NOT_READY;
}
}
@@ -232,8 +229,8 @@ public void update(){
* is supported.
* @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING
*/
- public void update(readData data) {
- if (data == readData.ONLY_UPDATE_HEADING) {
+ public void update(ReadData data) {
+ if (data == ReadData.ONLY_UPDATE_HEADING) {
hOrientation = 0;
}
}
@@ -241,14 +238,16 @@ public void update(readData data) {
/**
* Sets the odometry pod positions relative to the point that the odometry computer tracks around.
* The most common tracking position is the center of the robot.
- * The X pod offset refers to how far sideways (in mm) from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
- * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
- * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases
- * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases
- */
- public void setOffsets(double xOffset, double yOffset){
- writeFloat(Register.X_POD_OFFSET, (float) xOffset);
- writeFloat(Register.Y_POD_OFFSET, (float) yOffset);
+ * The X pod offset refers to how far sideways from the tracking point the X (forward) odometry pod is. Left of the center is a positive number, right of center is a negative number.
+ * the Y pod offset refers to how far forwards from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
+ *
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases
+ * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases
+ * @param distanceUnit the unit of distance used for offsets.
+ */
+ public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit) {
+ writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset));
+ writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset));
}
/**
@@ -256,56 +255,62 @@ public void setOffsets(double xOffset, double yOffset){
* Robot MUST be stationary
* Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.
*/
- public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);}
+ public void recalibrateIMU(){writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<0);}
/**
* Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.
* Robot MUST be stationary
* Device takes a large number of samples, and uses those as the gyroscope zero-offset. This takes approximately 0.25 seconds.
*/
- public void resetPosAndIMU(){writeInt(Register.DEVICE_CONTROL,1<<1);}
+ public void resetPosAndIMU(){writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<1);}
/**
* Can reverse the direction of each encoder.
* @param xEncoder FORWARD or REVERSED, X (forward) pod should increase when the robot is moving forward
* @param yEncoder FORWARD or REVERSED, Y (strafe) pod should increase when the robot is moving left
*/
- public void setEncoderDirections(EncoderDirection xEncoder, EncoderDirection yEncoder){
- if (xEncoder == EncoderDirection.FORWARD){
- writeInt(Register.DEVICE_CONTROL,1<<5);
+ public void setEncoderDirections(GoBildaPinpointDriver.EncoderDirection xEncoder, GoBildaPinpointDriver.EncoderDirection yEncoder){
+ if (xEncoder == GoBildaPinpointDriver.EncoderDirection.FORWARD){
+ writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<5);
}
- if (xEncoder == EncoderDirection.REVERSED) {
- writeInt(Register.DEVICE_CONTROL,1<<4);
+ if (xEncoder == GoBildaPinpointDriver.EncoderDirection.REVERSED) {
+ writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<4);
}
- if (yEncoder == EncoderDirection.FORWARD){
- writeInt(Register.DEVICE_CONTROL,1<<3);
+ if (yEncoder == GoBildaPinpointDriver.EncoderDirection.FORWARD){
+ writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<3);
}
- if (yEncoder == EncoderDirection.REVERSED){
- writeInt(Register.DEVICE_CONTROL,1<<2);
+ if (yEncoder == GoBildaPinpointDriver.EncoderDirection.REVERSED){
+ writeInt(GoBildaPinpointDriver.Register.DEVICE_CONTROL,1<<2);
}
}
+
/**
- * If you're using goBILDA odometry pods, the ticks-per-mm values are stored here for easy access.
+ * This allows you to set the encoder resolution by the type of GoBilda pod you are using.
+ * If you aren't using a GoBilda pod, use setEncoderResolution(double) instead.
+ *
* @param pods goBILDA_SWINGARM_POD or goBILDA_4_BAR_POD
*/
- public void setEncoderResolution(GoBildaOdometryPods pods){
+ public void setEncoderResolution(GoBildaOdometryPods pods) {
if (pods == GoBildaOdometryPods.goBILDA_SWINGARM_POD) {
- writeByteArray(Register.MM_PER_TICK, (floatToByteArray(goBILDA_SWINGARM_POD, ByteOrder.LITTLE_ENDIAN)));
+ setEncoderResolution(goBILDA_SWINGARM_POD, DistanceUnit.MM);
}
- if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD){
- writeByteArray(Register.MM_PER_TICK,(floatToByteArray(goBILDA_4_BAR_POD, ByteOrder.LITTLE_ENDIAN)));
+ if (pods == GoBildaOdometryPods.goBILDA_4_BAR_POD) {
+ setEncoderResolution(goBILDA_4_BAR_POD, DistanceUnit.MM);
}
}
/**
* Sets the encoder resolution in ticks per mm of the odometry pods.
* You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel.
- * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192
+ *
+ * @param ticksPerUnit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192
+ * @param distanceUnit unit used for distance
*/
- public void setEncoderResolution(double ticks_per_mm){
- writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN)));
+ public void setEncoderResolution(double ticksPerUnit, DistanceUnit distanceUnit) {
+ double resolution = distanceUnit.toMm(ticksPerUnit);
+ writeByteArray(Register.TICKS_PER_MM, (floatToByteArray((float) resolution, ByteOrder.LITTLE_ENDIAN)));
}
/**
@@ -318,7 +323,7 @@ public void setEncoderResolution(double ticks_per_mm){
* @param yawOffset A scalar for the robot's heading.
*/
public void setYawScalar(double yawOffset){
- writeByteArray(Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN)));
+ writeByteArray(GoBildaPinpointDriver.Register.YAW_SCALAR,(floatToByteArray((float) yawOffset, ByteOrder.LITTLE_ENDIAN)));
}
/**
@@ -344,9 +349,9 @@ public void setYawScalar(double yawOffset){
* @param pos a Pose2D describing the robot's new position.
*/
public Pose2D setPosition(Pose2D pos){
- writeByteArray(Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
- writeByteArray(Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN)));
- writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN)));
+ writeByteArray(GoBildaPinpointDriver.Register.X_POSITION,(floatToByteArray((float) pos.getX(DistanceUnit.MM), ByteOrder.LITTLE_ENDIAN)));
+ writeByteArray(GoBildaPinpointDriver.Register.Y_POSITION,(floatToByteArray((float) pos.getY(DistanceUnit.MM),ByteOrder.LITTLE_ENDIAN)));
+ writeByteArray(GoBildaPinpointDriver.Register.H_ORIENTATION,(floatToByteArray((float) pos.getHeading(AngleUnit.RADIANS),ByteOrder.LITTLE_ENDIAN)));
return pos;
}
@@ -354,14 +359,14 @@ public Pose2D setPosition(Pose2D pos){
* Checks the deviceID of the Odometry Computer. Should return 1.
* @return 1 if device is functional.
*/
- public int getDeviceID(){return readInt(Register.DEVICE_ID);}
+ public int getDeviceID(){return readInt(GoBildaPinpointDriver.Register.DEVICE_ID);}
/**
* @return the firmware version of the Odometry Computer
*/
- public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); }
+ public int getDeviceVersion(){return readInt(GoBildaPinpointDriver.Register.DEVICE_VERSION); }
- public float getYawScalar(){return readFloat(Register.YAW_SCALAR); }
+ public float getYawScalar(){return readFloat(GoBildaPinpointDriver.Register.YAW_SCALAR); }
/**
* Device Status stores any faults the Odometry Computer may be experiencing. These faults include:
@@ -373,7 +378,7 @@ public Pose2D setPosition(Pose2D pos){
* FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
* FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
*/
- public DeviceStatus getDeviceStatus(){return DeviceStatus.READY; }
+ public GoBildaPinpointDriver.DeviceStatus getDeviceStatus(){return GoBildaPinpointDriver.DeviceStatus.READY; }
/**
* Checks the Odometry Computer's most recent loop time.
@@ -419,34 +424,43 @@ public double getFrequency(){
/**
* @return the estimated H (heading) position of the robot in Radians
*/
- public double getHeading(){return hOrientation;}
+ public double getHeading(AngleUnit angleUnit) {
+ return angleUnit.fromRadians(hOrientation);
+ }
+
/**
- * @return the estimated X (forward) velocity of the robot in mm/sec
+ * @return the estimated X (forward) velocity of the robot in specified unit/sec
*/
- public double getVelX(){return xVelocity; }
+ public double getVelX(DistanceUnit distanceUnit) {
+ return distanceUnit.fromMm(xVelocity);
+ }
/**
- * @return the estimated Y (strafe) velocity of the robot in mm/sec
+ * @return the estimated Y (strafe) velocity of the robot in specified unit/sec
*/
- public double getVelY(){return yVelocity; }
+ public double getVelY(DistanceUnit distanceUnit) {
+ return distanceUnit.fromMm(yVelocity);
+ }
/**
- * @return the estimated H (heading) velocity of the robot in radians/sec
+ * @return the estimated H (heading) velocity of the robot in specified unit/sec
*/
- public double getHeadingVelocity(){return hVelocity; }
+ public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit) {
+ return unnormalizedAngleUnit.fromRadians(hVelocity);
+ }
/**
* This uses its own I2C read, avoid calling this every loop.
* @return the user-set offset for the X (forward) pod
*/
- public float getXOffset(){return readFloat(Register.X_POD_OFFSET);}
+ public float getXOffset(){return readFloat(GoBildaPinpointDriver.Register.X_POD_OFFSET);}
/**
* This uses its own I2C read, avoid calling this every loop.
* @return the user-set offset for the Y (strafe) pod
*/
- public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);}
+ public float getYOffset(){return readFloat(GoBildaPinpointDriver.Register.Y_POD_OFFSET);}
/**
* @return a Pose2D containing the estimated position of the robot
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java
new file mode 100644
index 000000000000..6fc9dc380ad6
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLFieldMap.java
@@ -0,0 +1,251 @@
+package com.qualcomm.hardware.limelightvision;
+
+import org.json.JSONArray;
+import org.json.JSONObject;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Represents a field map containing fiducial markers / AprilTags.
+ */
+public class LLFieldMap {
+
+ /**
+ * Represents a fiducial marker / AprilTag in the field map.
+ */
+ public static class Fiducial {
+ private int id;
+ private double size;
+ private String family;
+ private List transform;
+ private boolean isUnique;
+
+ /**
+ * Constructs a Fiducial with default values.
+ */
+ public Fiducial() {
+ this(-1, 165.1, "apriltag3_36h11_classic", new ArrayList<>(16), true);
+ }
+
+ /**
+ * Constructs a Fiducial with specified values.
+ *
+ * @param id The ID of the fiducial.
+ * @param size The size of the fiducial.
+ * @param family The family of the fiducial.
+ * @param transform The transformation of the fiducial.
+ * @param isUnique Whether the fiducial is unique.
+ */
+ public Fiducial(int id, double size, String family, List transform, boolean isUnique) {
+ this.id = id;
+ this.size = size;
+ this.family = family;
+ this.transform = new ArrayList<>(transform);
+ this.isUnique = isUnique;
+ }
+
+ /**
+ * Constructs a Fiducial from JSON data. Returns a default Fidcial if JSON is null or malformed
+ *
+ * @param json The JSON object containing fiducial data.
+ */
+ protected Fiducial(JSONObject json) {
+ this(); //empy Fiducial
+ if(json != null)
+ {
+ try{
+ this.id = json.optInt("id", -1);
+ this.size = json.optDouble("size", 165.1);
+ this.family = json.optString("family", "apriltag3_36h11_classic");
+ this.transform = new ArrayList<>();
+ JSONArray transformArray = json.optJSONArray("transform");
+ if (transformArray != null) {
+ for (int i = 0; i < transformArray.length(); i++) {
+ this.transform.add(transformArray.optDouble(i, 0.0));
+ }
+ }
+ this.isUnique = json.optBoolean("unique", true);
+ } catch (Exception e) {
+
+ }
+ }
+ }
+
+ /**
+ * Gets the ID / index of the fiducial.
+ *
+ * @return The fiducial ID.
+ */
+ public int getId() {
+ return id;
+ }
+
+ /**
+ * Gets the size of the fiducial in millimeters
+ *
+ * @return The fiducial size.
+ */
+ public double getSize() {
+ return size;
+ }
+
+ /**
+ * Gets the family of the fiducial. eg "apriltag3_36h11_classic"
+ *
+ * @return The fiducial family.
+ */
+ public String getFamily() {
+ return family;
+ }
+
+ /**
+ * Gets the 4x4 transforms matrix of the fiducial.
+ *
+ * @return A copy of the fiducial's transform.
+ */
+ public List getTransform() {
+ return new ArrayList<>(transform);
+ }
+
+ /**
+ * Checks if the fiducial is marked as unique.
+ *
+ * @return true if the fiducial is unique, false otherwise.
+ */
+ public boolean isUnique() {
+ return isUnique;
+ }
+
+ /**
+ * Converts the Fiducial to a JSONObject.
+ *
+ * @return A JSONObject representation of the Fiducial.
+ */
+ protected JSONObject toJson() {
+ JSONObject json = new JSONObject();
+ try {
+ json.put("id", id);
+ json.put("size", size);
+ json.put("family", family);
+ json.put("transform", new JSONArray(transform));
+ json.put("unique", isUnique);
+ } catch (Exception e){
+
+ }
+ return json;
+ }
+ }
+
+ private List fiducials;
+ private String type;
+
+ /**
+ * Constructs an empty LLFieldMap.
+ */
+ public LLFieldMap() {
+ this(new ArrayList<>(), "");
+ }
+
+ /**
+ * Constructs an LLFieldMap with specified fiducials and type.
+ *
+ * @param fiducials The list of fiducials in the field map.
+ * @param type The type of the field map.
+ */
+ public LLFieldMap(List fiducials, String type) {
+ this.fiducials = new ArrayList<>(fiducials);
+ this.type = type;
+ }
+
+ /**
+ * Constructs an LLFieldMap from JSON data. Returns an empty map if json is null or malformed
+ *
+ * @param json The JSON object containing field map data.
+ */
+ protected LLFieldMap(JSONObject json) {
+ this();
+ if (json != null) {
+ try{
+ this.type = json.optString("type", "");
+ JSONArray fiducialsArray = json.optJSONArray("fiducials");
+ if (fiducialsArray != null) {
+ for (int i = 0; i < fiducialsArray.length(); i++) {
+ JSONObject fiducialJson = fiducialsArray.optJSONObject(i);
+ if (fiducialJson != null) {
+ this.fiducials.add(new Fiducial(fiducialJson));
+ }
+ }
+ }
+ }
+ catch (Exception e){
+ }
+ }
+ }
+
+ /**
+ * Gets the list of fiducials in the field map.
+ *
+ * @return A copy of the list of fiducials.
+ */
+ public List getFiducials() {
+ return new ArrayList<>(fiducials);
+ }
+
+ /**
+ * Gets the type of the field map. (eg "ftc" or "frc")
+ *
+ * @return The field map type.
+ */
+ public String getType() {
+ return type;
+ }
+
+ /**
+ * Gets the number of tags (fiducials) in the field map.
+ *
+ * @return The number of tags.
+ */
+ public int getNumberOfTags() {
+ return fiducials.size();
+ }
+
+ /**
+ * Get validity of map. Maps are valid if they have more than zero tags and have a specified type
+ *
+ * @return True if valid. False otherwise
+ */
+ public boolean isValid()
+ {
+ if(getNumberOfTags() == 0)
+ {
+ //Likely user error.
+ return false;
+ }
+ if((getType() != "ftc") && (getType() != "frc"))
+ {
+ //Likely user error.
+ return false;
+ }
+ return true;
+ }
+
+ /**
+ * Converts the LLFieldMap to a JSONObject.
+ *
+ * @return A JSONObject representation of the LLFieldMap.
+ */
+ protected JSONObject toJson() {
+ JSONObject json = new JSONObject();
+ try {
+ JSONArray fiducialsArray = new JSONArray();
+ for (Fiducial fiducial : fiducials) {
+ fiducialsArray.put(fiducial.toJson());
+ }
+ json.put("fiducials", fiducialsArray);
+ json.put("type", type);
+ } catch (Exception e) {
+ }
+ return json;
+ }
+}
\ No newline at end of file
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java
new file mode 100644
index 000000000000..d53848613ed1
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResult.java
@@ -0,0 +1,532 @@
+package com.qualcomm.hardware.limelightvision;
+
+/*
+Copyright (c) 2024 Limelight Vision
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Limelight Vision nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+import com.qualcomm.hardware.limelightvision.LLResultTypes.BarcodeResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes.ClassifierResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes.ColorResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes.DetectorResult;
+import com.qualcomm.hardware.limelightvision.LLResultTypes.FiducialResult;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+import org.json.JSONArray;
+import org.json.JSONException;
+import org.json.JSONObject;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Represents the result of a Limelight Pipeline. This class parses JSON data from a Limelight
+ * in the constructor and provides easy access to the results data.
+ */
+public class LLResult {
+ private JSONObject jsonData;
+ private List barcodeResults;
+ private List classifierResults;
+ private List detectorResults;
+ private List fiducialResults;
+ private List colorResults;
+ private double parseLatency;
+ private long controlHubTimeStamp;
+
+ /**
+ * Constructs an LLResult object from a JSONObject.
+ *
+ * @param json The JSONObject containing Limelight vision data.
+ * @throws JSONException If there's an error parsing the JSON string.
+ */
+ protected LLResult(JSONObject json) throws JSONException {
+ this.jsonData = json;
+ this.barcodeResults = new ArrayList<>();
+ this.classifierResults = new ArrayList<>();
+ this.detectorResults = new ArrayList<>();
+ this.fiducialResults = new ArrayList<>();
+ this.colorResults = new ArrayList<>();
+ this.parseLatency = 0.0;
+ setControlHubTimeStamp(System.currentTimeMillis());
+ parseResults();
+ }
+
+ /**
+ * Sets the timestamp from the control hub in milliseconds.
+ *
+ * @param timestamp The timestamp to set in milliseconds.
+ */
+ void setControlHubTimeStamp(long timestamp) {
+ this.controlHubTimeStamp = timestamp;
+ }
+
+ /**
+ * Gets the control hub timestamp in milliseconds.
+ *
+ * @return The control hub timestamp in milliseconds.
+ */
+ public long getControlHubTimeStamp() {
+ return this.controlHubTimeStamp;
+ }
+
+ /**
+ * Gets the control hub timestamp in nanoseconds.
+ *
+ * @return The control hub timestamp in nanoseconds.
+ */
+ public long getControlHubTimeStampNanos() {
+ return this.controlHubTimeStamp * 1000000;
+ }
+
+
+ /**
+ * Calculates the staleness of the data.
+ *
+ * @return The staleness in milliseconds.
+ */
+ public long getStaleness() {
+ return System.currentTimeMillis() - this.controlHubTimeStamp;
+ }
+
+ /**
+ * Parses the JSON data into result objects.
+ *
+ * @throws JSONException If there's an error parsing the JSON data.
+ */
+ private void parseResults() throws JSONException {
+ long startTime = System.nanoTime();
+
+ JSONArray barcodeArray = jsonData.optJSONArray("Barcode");
+ if (barcodeArray != null) {
+ for (int i = 0; i < barcodeArray.length(); i++) {
+ barcodeResults.add(new BarcodeResult(barcodeArray.getJSONObject(i)));
+ }
+ }
+
+ JSONArray classifierArray = jsonData.optJSONArray("Classifier");
+ if (classifierArray != null) {
+ for (int i = 0; i < classifierArray.length(); i++) {
+ classifierResults.add(new ClassifierResult(classifierArray.getJSONObject(i)));
+ }
+ }
+
+ JSONArray detectorArray = jsonData.optJSONArray("Detector");
+ if (detectorArray != null) {
+ for (int i = 0; i < detectorArray.length(); i++) {
+ detectorResults.add(new DetectorResult(detectorArray.getJSONObject(i)));
+ }
+ }
+
+ JSONArray fiducialArray = jsonData.optJSONArray("Fiducial");
+ if (fiducialArray != null) {
+ for (int i = 0; i < fiducialArray.length(); i++) {
+ fiducialResults.add(new FiducialResult(fiducialArray.getJSONObject(i)));
+ }
+ }
+
+ JSONArray colorArray = jsonData.optJSONArray("Retro");
+ if (colorArray != null) {
+ for (int i = 0; i < colorArray.length(); i++) {
+ colorResults.add(new ColorResult(colorArray.getJSONObject(i)));
+ }
+ }
+
+ long endTime = System.nanoTime();
+ this.parseLatency = (endTime - startTime) / 1e6; // Convert to milliseconds
+ }
+
+ /**
+ * Gets the list of barcode results.
+ *
+ * @return A list of BarcodeResult objects.
+ */
+ public List getBarcodeResults() {
+ return barcodeResults;
+ }
+
+ /**
+ * Gets the list of classifier results.
+ *
+ * @return A list of ClassifierResult objects.
+ */
+ public List getClassifierResults() {
+ return classifierResults;
+ }
+
+ /**
+ * Gets the list of detector results.
+ *
+ * @return A list of DetectorResult objects.
+ */
+ public List getDetectorResults() {
+ return detectorResults;
+ }
+
+ /**
+ * Gets the list of fiducial/apriltag results.
+ *
+ * @return A list of FiducialResult objects.
+ */
+ public List getFiducialResults() {
+ return fiducialResults;
+ }
+
+ /**
+ * Gets the list of color results.
+ *
+ * @return A list of ColorResult objects.
+ */
+ public List getColorResults() {
+ return colorResults;
+ }
+
+ /**
+ * Gets the focus metric of the image. This is only valid if the focus pipeline is enabled
+ *
+ * @return The focus metric value.
+ */
+ public double getFocusMetric() {
+ return jsonData.optDouble("focus_metric", 0);
+ }
+
+ /**
+ * Gets the 3D botpose.
+ *
+ * @return An Pose3D representing the bot pose.
+ */
+ public Pose3D getBotpose() {
+ return createPose3DRobot(getDoubleArray("botpose", 6));
+ }
+
+ /**
+ * Gets the 3D botpose using the WPILIB Blue Alliance Coordinate System.
+ *
+ * @return An Pose3D representing the bot pose.
+ */
+ private Pose3D getBotposeWpiblue() {
+ return createPose3DRobot(getDoubleArray("botpose_wpiblue", 6));
+ }
+
+
+ /**
+ * Gets the 3D botpose using the WPILIB Red Alliance Coordinate System.
+ *
+ * @return An Pose3D representing the bot pose.
+ */
+ private Pose3D getBotposeWpired() {
+ return createPose3DRobot(getDoubleArray("botpose_wpired", 6));
+ }
+
+ /**
+ * Gets the 3D botpose using MegaTag2. You must set the orientation of the robot with your imu for this to work.
+ *
+ * @return An Pose3D representing the MT2 botpose
+ */
+ public Pose3D getBotpose_MT2() {
+ return createPose3DRobot(getDoubleArray("botpose_orb", 6));
+ }
+
+ /**
+ * Gets the 3D botpose using MegaTag2 and the WPILIB Blue Alliance Coordinate System. You must set the orientation of the robot with your imu for this to work.
+ *
+ * @return An Pose3D representing the MT2 botpose
+ */
+ private Pose3D getBotposeWpiblue_MT2() {
+ return createPose3DRobot(getDoubleArray("botpose_orb_wpiblue", 6));
+ }
+
+ /**
+ * Gets the 3D botpose using MegaTag2 and the WPILIB Red Alliance Coordinate System. You must set the orientation of the robot with your imu for this to work.
+ *
+ * @return An Pose3D representing the MT2 botpose
+ */
+ private Pose3D getBotposeOrbWpired_MT2() {
+ return createPose3DRobot(getDoubleArray("botpose_orb_wpired", 6));
+ }
+
+
+ /**
+ * Gets the standard deviation metrics for MegaTag1 (x,y,z,roll,pitch,yaw)
+ *
+ * @return MegaTag1 STDevs
+ */
+ public double[] getStddevMt1() {
+ return getDoubleArray("stdev_mt1", 6);
+ }
+
+ /**
+ * Gets the standard deviation metrics for MegaTag2 (x,y,z,roll,pitch,yaw)
+ *
+ * @return MegaTag2 STDevs
+ */
+ public double[] getStddevMt2() {
+ return getDoubleArray("stdev_mt2", 6);
+ }
+
+ /**
+ * Gets the number of tags used in the botpose calculation.
+ *
+ * @return The number of tags used in the botpose calculation.
+ */
+ public int getBotposeTagCount() {
+ return jsonData.optInt("botpose_tagcount", 0);
+ }
+
+ /**
+ * Gets the span of tags used in the botpose calculation in meters.
+ *
+ * @return The span of tags used in the botpose calculation.
+ */
+ public double getBotposeSpan() {
+ return jsonData.optDouble("botpose_span", 0);
+ }
+
+ /**
+ * Gets the average distance of tags used in the botpose calculation in meters.
+ *
+ * @return The average distance of tags used in the botpose calculation.
+ */
+ public double getBotposeAvgDist() {
+ return jsonData.optDouble("botpose_avgdist", 0);
+ }
+
+ /**
+ * Gets the average area of tags used in the botpose calculation.
+ *
+ * @return The average area of tags used in the botpose calculation.
+ */
+ public double getBotposeAvgArea() {
+ return jsonData.optDouble("botpose_avgarea", 0);
+ }
+
+ /**
+ * Gets the user-specified python snapscript output data
+ *
+ * @return An array of values from the python snapscript pipeline. Returns an array of size 32.
+ */
+ public double[] getPythonOutput() {
+ double[] output = getDoubleArray("PythonOut", 0);
+
+ // Create a new array of size 32
+ double[] result = new double[32];
+
+ // How many elements do we need to copy to the final result array?
+ int lengthToCopy = Math.min(output.length, 32);
+
+ // Copy from the python result array to the result array.
+ System.arraycopy(output, 0, result, 0, lengthToCopy);
+ return result;
+ }
+
+ /**
+ * Gets the current capture latency in milliseconds
+ *
+ * @return capture latency in millisecondss
+ */
+ public double getCaptureLatency() {
+ return jsonData.optDouble("cl", 0);
+ }
+
+ /**
+ * Gets the type of the current pipeline.
+ *
+ * @return The type of the current pipeline as a string.
+ */
+ public String getPipelineType() {
+ return jsonData.optString("pipelineType", "");
+ }
+
+ /**
+ * Gets the current tx in degrees from the crosshair
+ *
+ * @return horizontal angular offset of the primary target in degrees from the crosshair
+ */
+ public double getTx() {
+ return jsonData.optDouble("tx", 0);
+ }
+
+ /**
+ * Gets the current ty in degrees from the crosshair
+ *
+ * @return vertical angular offset of the primary target in degrees from the crosshair
+ */
+ public double getTy() {
+ return jsonData.optDouble("ty", 0);
+ }
+
+ /**
+ * Gets the current tx in degrees from the principal pixel instead of the crosshair
+ *
+ * @return horizontal angular offset of the primary target in degrees from the principal pixel instead of the crosshair
+ */
+ public double getTxNC() {
+ return jsonData.optDouble("txnc", 0);
+ }
+
+ /**
+ * Gets the current ty in degrees from the principal pixel instead of the crosshair
+ *
+ * @return vertical angular offset of the primary target in degrees from the principal pixel instead of the crosshair
+ */
+ public double getTyNC() {
+ return jsonData.optDouble("tync", 0);
+ }
+
+ /**
+ * Gets the area of the target as a percentage of the image area
+ *
+ * @return target area (0-100)
+ */
+ public double getTa() {
+ return jsonData.optDouble("ta", 0);
+ }
+
+ /**
+ * Gets the index of the currently active pipeline
+ *
+ * @return index of the currently active pipeline
+ */
+ public int getPipelineIndex() {
+ return jsonData.optInt("pID", 0);
+ }
+
+ /**
+ * Gets the 3D camera pose in robot space as configured in the UI
+ *
+ * @return An Pose3D representing the camera pose in the robot's coordinate system.
+ */
+ private Pose3D getCameraPose_RobotSpace() {
+ return createPose3DRobot(getDoubleArray("t6c_rs", 6));
+ }
+
+ /**
+ * Gets the targeting/pipeline latency in milliseconds.
+ *
+ * @return The targeting/pipeline latency.
+ */
+ public double getTargetingLatency() {
+ return jsonData.optDouble("tl", 0);
+ }
+
+ /**
+ * Gets the Limelight-local monotonic timestamp of the result.
+ *
+ * @return The limelight-local timestamp.
+ */
+ public double getTimestamp() {
+ return jsonData.optDouble("ts", 0);
+ }
+
+ /**
+ * Gets the validity of the result.
+ *
+ * @return Validity (0 or 1).
+ */
+ public boolean isValid() {
+ int v = jsonData.optInt("v", 0);
+ if(v == 1)
+ {
+ return true;
+ }
+ return false;
+ }
+
+ /**
+ * Gets the json parse latency.
+ *
+ * @return The Control Hub json parse latency in milliseconds.
+ */
+ public double getParseLatency() {
+ return parseLatency;
+ }
+
+ /**
+ * Helper method to get a double array from the JSON data.
+ *
+ * @param key The key for the array in the JSON data.
+ * @return A double array corresponding to the key.
+ */
+ private double[] getDoubleArray(String key, int defaultCount) {
+ JSONArray array = jsonData.optJSONArray(key);
+ if (array == null) {
+ return new double[defaultCount];
+ }
+ double[] result = new double[array.length()];
+ for (int i = 0; i < array.length(); i++) {
+ result[i] = array.optDouble(i);
+ }
+ return result;
+ }
+
+ /**
+ * Helper method to create an Pose3D instance from a pose array.
+ *
+ * @param pose The pose array (x, y, z, roll, pitch, yaw)
+ * @return An Pose3D instance without acquisition time.
+ */
+ protected static Pose3D createPose3DRobot(double[] pose) {
+ if (pose.length < 6) {
+ return new Pose3D(new Position(), new YawPitchRollAngles(AngleUnit.DEGREES,0,0,0,0));
+ }
+ Position position = new Position(DistanceUnit.METER, pose[0], pose[1], pose[2],0);
+ YawPitchRollAngles orientation = new YawPitchRollAngles(AngleUnit.DEGREES, pose[5], pose[4], pose[3],0);
+ return new Pose3D(position, orientation);
+ }
+
+ /**
+ * Parses a JSONObject into an LLResult object.
+ *
+ * @param json The JSON to parse.
+ * @return An LLResult object, or null if parsing fails.
+ */
+ protected static LLResult parse(JSONObject json) {
+ try {
+ return new LLResult(json);
+ } catch (JSONException e) {
+ e.printStackTrace();
+ return null;
+ }
+ }
+
+ /**
+ * Returns a string representation of the LLResult.
+ *
+ * @return A string representation of the JSON data.
+ */
+ @Override
+ public String toString() {
+ return jsonData.toString();
+ }
+}
\ No newline at end of file
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java
new file mode 100644
index 000000000000..ea4910573607
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLResultTypes.java
@@ -0,0 +1,883 @@
+package com.qualcomm.hardware.limelightvision;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Pose3D;
+import org.json.JSONArray;
+import org.json.JSONException;
+import org.json.JSONObject;
+
+import java.util.ArrayList;
+import java.util.List;
+
+/**
+ * Parent class for all Limelight Result Types
+ */
+public class LLResultTypes {
+
+ /**
+ * Parses a JSONArray of points into a List of Lists representing 2D coordinates.
+ *
+ * @param cornersArray The JSONArray containing point data. Each element should be a JSONArray with two elements representing x and y coordinates.
+ * @return A List of Lists, where each inner List contains two Double values representing x and y coordinates.
+ * Returns an empty List if the input is null or improperly formatted.
+ */
+ private static List> parsePoints(JSONArray cornersArray) {
+ List> points = new ArrayList<>();
+ if (cornersArray != null) {
+ for (int i = 0; i < cornersArray.length(); i++) {
+ JSONArray point = cornersArray.optJSONArray(i);
+ if (point != null && point.length() == 2) {
+ List cornerPoint = new ArrayList<>();
+ cornerPoint.add(point.optDouble(0, 0.0));
+ cornerPoint.add(point.optDouble(1, 0.0));
+ points.add(cornerPoint);
+ }
+ }
+ }
+ return points;
+ }
+
+ /**
+ * Parses a JSONArray into a double array representing a robot pose.
+ * The array should contain 6 elements: x, y, z, roll, pitch, yaw.
+ *
+ * @param array The JSONArray to convert.
+ * @return A double array representing the robot pose. If the input is null or has fewer than 6 elements,
+ * returns an array of 6 zeros.
+ */
+ private static double[] parsePoseArray(JSONArray array) {
+ double[] pose = new double[6]; // Initialize with zeros
+ if (array != null && array.length() >= 6) {
+ for (int i = 0; i < 6; i++) {
+ pose[i] = array.optDouble(i, 0.0);
+ }
+ }
+ return pose;
+ }
+
+ /**
+ * Represents a barcode pipeline result. A barcode pipeline may generate multiple valid results.
+ */
+ public static class BarcodeResult {
+
+ private String family;
+ private String data;
+ private double targetXPixels;
+ private double targetYPixels;
+ private double targetXDegrees;
+ private double targetYDegrees;
+ private double targetXDegreesNoCrosshair;
+ private double targetYDegreesNoCrosshair;
+ private double targetArea;
+ private List> targetCorners;
+
+ /**
+ * Constructs a BarcodeResult from JSON data.
+ *
+ * @param data The JSON object containing barcode data.
+ */
+ protected BarcodeResult(JSONObject data) {
+ this.family = data.optString("fam", "");
+ this.data = data.optString("data", "");
+ this.targetXPixels = data.optDouble("txp", 0.0);
+ this.targetYPixels = data.optDouble("typ", 0.0);
+ this.targetXDegrees = data.optDouble("tx", 0.0);
+ this.targetYDegrees = data.optDouble("ty", 0.0);
+ this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0);
+ this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0);
+ this.targetArea = data.optDouble("ta", 0.0);
+ this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts"));
+ }
+
+ /**
+ * Gets the family of the barcode.
+ *
+ * @return The barcode family.
+ */
+ public String getFamily() {
+ return family;
+ }
+
+ /**
+ * Gets the data contained in the barcode.
+ *
+ * @return The barcode data.
+ */
+ public String getData() {
+ return data;
+ }
+
+ /**
+ * Gets the horizontal offset of the barcode from the crosshair in pixels.
+ *
+ * @return The horizontal offset in pixels.
+ */
+ public double getTargetXPixels() {
+ return targetXPixels;
+ }
+
+ /**
+ * Gets the vertical offset of the barcode from the crosshair in pixels.
+ *
+ * @return The vertical offset in pixels.
+ */
+ public double getTargetYPixels() {
+ return targetYPixels;
+ }
+
+ /**
+ * Gets the horizontal offset of the barcode from the crosshair in degrees.
+ *
+ * @return The horizontal offset in degrees.
+ */
+ public double getTargetXDegrees() {
+ return targetXDegrees;
+ }
+
+ /**
+ * Gets the vertical offset of the barcode from the crosshair in degrees.
+ *
+ * @return The vertical offset in degrees.
+ */
+ public double getTargetYDegrees() {
+ return targetYDegrees;
+ }
+
+ /**
+ * Gets the horizontal offset of the barcode from the principal pixel in degrees.
+ *
+ * @return The horizontal offset in degrees.
+ */
+ public double getTargetXDegreesNoCrosshair() {
+ return targetXDegreesNoCrosshair;
+ }
+
+ /**
+ * Gets the vertical offset of the barcode from the principal pixel in degrees.
+ *
+ * @return The vertical offset in degrees.
+ */
+ public double getTargetYDegreesNoCrosshair() {
+ return targetYDegreesNoCrosshair;
+ }
+
+ /**
+ * Gets the area of the detected barcode as a percentage of the image.
+ *
+ * @return The target area (0-1).
+ */
+ public double getTargetArea() {
+ return targetArea;
+ }
+
+ /**
+ * Gets the four corner points of the detected barcode.
+ *
+ * @return A list of corner points, each point represented as a list of two doubles [x, y].
+ */
+ public List> getTargetCorners() {
+ return targetCorners;
+ }
+
+ }
+
+ /**
+ * Represents a classifier pipeline result.
+ */
+ public static class ClassifierResult {
+ private String className;
+ private int classId;
+ private double confidence;
+
+ /**
+ * Constructs a ClassifierResult from JSON data.
+ *
+ * @param data The JSON object containing classifier data.
+ */
+ protected ClassifierResult(JSONObject data) {
+ this.className = data.optString("class", "");
+ this.classId = data.optInt("classID", 0);
+ this.confidence = data.optDouble("conf", 0.0);
+ }
+
+ /**
+ * Gets the class name of the classifier result (eg book, car, gamepiece).
+ *
+ * @return The class name
+ */
+ public String getClassName() {
+ return className;
+ }
+
+ /**
+ * Gets the class index of the classifier result.
+ *
+ * @return The class index
+ */
+ public int getClassId() {
+ return classId;
+ }
+
+ /**
+ * Gets the confidence score of the classification.
+ *
+ * @return The confidence score of the classification (0-100).
+ */
+ public double getConfidence() {
+ return confidence;
+ }
+ }
+
+ /**
+ * Represents a detector pipeline result. A detector pipeline may generate multiple valid results.
+ */
+ public static class DetectorResult {
+ private String className;
+ private int classId;
+ private double confidence;
+ private double targetArea;
+ private double targetXPixels;
+ private double targetYPixels;
+ private double targetXDegrees;
+ private double targetYDegrees;
+ private double targetXDegreesNoCrosshair;
+ private double targetYDegreesNoCrosshair;
+ private List> targetCorners;
+
+ /**
+ * Constructs a DetectorResult from JSON data.
+ *
+ * @param data The JSON object containing classifier data.
+ */
+ protected DetectorResult(JSONObject data) {
+ this.className = data.optString("class", "");
+ this.classId = data.optInt("classID", 0);
+ this.confidence = data.optDouble("conf", 0.0);
+ this.targetArea = data.optDouble("ta", 0.0);
+ this.targetXPixels = data.optDouble("txp", 0.0);
+ this.targetYPixels = data.optDouble("typ", 0.0);
+ this.targetXDegrees = data.optDouble("tx", 0.0);
+ this.targetYDegrees = data.optDouble("ty", 0.0);
+ this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0);
+ this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0);
+ this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts"));
+ }
+
+ /**
+ * Gets the class name of the detector result (eg book, car, gamepiece).
+ *
+ * @return The class name
+ */
+ public String getClassName() {
+ return className;
+ }
+
+
+ /**
+ * Gets the class index of the detector result.
+ *
+ * @return The class index
+ */
+ public int getClassId() {
+ return classId;
+ }
+
+ /**
+ * Gets the confidence score of the classification.
+ *
+ * @return The confidence score of the classification (0-100).
+ */
+ public double getConfidence() {
+ return confidence;
+ }
+
+ /**
+ * Gets the four corner points of the detected result.
+ *
+ * @return A list of corner points, each point represented as a list of two doubles [x, y].
+ */
+ public List> getTargetCorners() {
+ return targetCorners;
+ }
+
+ /**
+ * Gets the undistorted area of the target as a percentage of the image area
+ *
+ * @return target area (0-100)
+ */
+ public double getTargetArea() {
+ return targetArea;
+ }
+
+ /**
+ * Gets the current tx in pixels from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXPixels() {
+ return targetXPixels;
+ }
+
+ /**
+ * Gets the current ty in pixels from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYPixels() {
+ return targetYPixels;
+ }
+
+ /**
+ * Gets the current tx in degrees from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegrees() {
+ return targetXDegrees;
+ }
+
+ /**
+ * Gets the current ty in degrees from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegrees() {
+ return targetYDegrees;
+ }
+
+ /**
+ * Gets the current tx in degrees from the principal pixel
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegreesNoCrosshair() {
+ return targetXDegreesNoCrosshair;
+ }
+
+ /**
+ * Gets the current ty in degrees from the principal pixel
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegreesNoCrosshair() {
+ return targetYDegreesNoCrosshair;
+ }
+ }
+
+ /**
+ * Represents a Fiducial/AprilTag pipeline result. A fiducial/apriltag pipeline may generate multiple valid results.
+ */
+ public static class FiducialResult {
+ private int fiducialId;
+ private String family;
+ private double skew;
+ private Pose3D cameraPoseTargetSpace;
+ private Pose3D robotPoseFieldSpace;
+ private Pose3D robotPoseTargetSpace;
+ private Pose3D targetPoseCameraSpace;
+ private Pose3D targetPoseRobotSpace;
+ private double targetArea;
+ private double targetXPixels;
+ private double targetYPixels;
+ private double targetXDegrees;
+ private double targetYDegrees;
+ private double targetXDegreesNoCrosshair;
+ private double targetYDegreesNoCrosshair;
+ private List> targetCorners;
+
+
+ /**
+ * Constructs a FiducialResult from JSON data.
+ *
+ * @param data The JSON object containing fiducial data.
+ */
+ protected FiducialResult(JSONObject data) {
+ this.fiducialId = data.optInt("fID", 0);
+ this.family = data.optString("fam", "");
+ this.skew = data.optDouble("skew", 0.0);
+ this.cameraPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6c_ts")));
+ this.robotPoseFieldSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_fs")));
+ this.robotPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_ts")));
+ this.targetPoseCameraSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_cs")));
+ this.targetPoseRobotSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_rs")));
+ this.targetArea = data.optDouble("ta", 0.0);
+ this.targetXPixels = data.optDouble("txp", 0.0);
+ this.targetYPixels = data.optDouble("typ", 0.0);
+ this.targetXDegrees = data.optDouble("tx", 0.0);
+ this.targetYDegrees = data.optDouble("ty", 0.0);
+ this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0);
+ this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0);
+ this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts"));
+ }
+
+ /**
+ * Gets the ID of the fiducial.
+ *
+ * @return The fiducial ID.
+ */
+ public int getFiducialId() {
+ return fiducialId;
+ }
+
+ /**
+ * Gets the family of the fiducial (eg 36h11).
+ *
+ * @return The fiducial family.
+ */
+ public String getFamily() {
+ return family;
+ }
+
+ /**
+ * Gets the four corner points of the detected fiducial/apriltag.
+ *
+ * @return A list of corner points, each point represented as a list of two doubles [x, y].
+ */
+ public List> getTargetCorners() {
+ return targetCorners;
+ }
+
+ /**
+ * Gets the skew of the detected fiducial. Not recommended.
+ *
+ * @return The skew value.
+ */
+ public double getSkew() {
+ return skew;
+ }
+
+ /**
+ * Gets the camera pose in target space.
+ *
+ * @return An Pose3D representing the camera pose.
+ */
+ public Pose3D getCameraPoseTargetSpace() {
+ return cameraPoseTargetSpace;
+ }
+
+ /**
+ * Gets the robot pose in field based on this fiducial/apriltag alone.
+ *
+ * @return An Pose3D representing the robot pose.
+ */
+ public Pose3D getRobotPoseFieldSpace() {
+ return robotPoseFieldSpace;
+ }
+
+ /**
+ * Gets the robot pose in target space.
+ *
+ * @return An Pose3D representing the robot pose.
+ */
+ public Pose3D getRobotPoseTargetSpace() {
+ return robotPoseTargetSpace;
+ }
+
+ /**
+ * Gets the target pose in camera space.
+ *
+ * @return An Pose3D representing the target pose.
+ */
+ public Pose3D getTargetPoseCameraSpace() {
+ return targetPoseCameraSpace;
+ }
+
+ /**
+ * Gets the target pose in robot space.
+ *
+ * @return An Pose3D representing the target pose.
+ */
+ public Pose3D getTargetPoseRobotSpace() {
+ return targetPoseRobotSpace;
+ }
+
+ /**
+ * Gets the area of the detected fiducial as a percentage of the image.
+ *
+ * @return The target area (0-100).
+ */
+ public double getTargetArea() {
+ return targetArea;
+ }
+
+ /**
+ * Gets the current tx in pixels from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXPixels() {
+ return targetXPixels;
+ }
+
+ /**
+ * Gets the current ty in pixels from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYPixels() {
+ return targetYPixels;
+ }
+
+ /**
+ * Gets the current tx in degrees from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegrees() {
+ return targetXDegrees;
+ }
+
+ /**
+ * Gets the current ty in degrees from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegrees() {
+ return targetYDegrees;
+ }
+
+ /**
+ * Gets the current tx in degrees from the principal pixel
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegreesNoCrosshair() {
+ return targetXDegreesNoCrosshair;
+ }
+
+ /**
+ * Gets the current ty in degrees from the principal pixel
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegreesNoCrosshair() {
+ return targetYDegreesNoCrosshair;
+ }
+ }
+
+ /**
+ * Represents a color pipeline result. A color pipeline may generate multiple valid results.
+ */
+ public static class ColorResult {
+ private Pose3D cameraPoseTargetSpace;
+ private Pose3D robotPoseFieldSpace;
+ private Pose3D robotPoseTargetSpace;
+ private Pose3D targetPoseCameraSpace;
+ private Pose3D targetPoseRobotSpace;
+ private double targetArea;
+ private double targetXPixels;
+ private double targetYPixels;
+ private double targetXDegrees;
+ private double targetYDegrees;
+ private double targetXDegreesNoCrosshair;
+ private double targetYDegreesNoCrosshair;
+ private List> targetCorners;
+
+ /**
+ * Constructs a Color/ColorResult from JSON data.
+ *
+ * @param data The JSON object containing color data.
+ */
+ protected ColorResult(JSONObject data) {
+ this.cameraPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6c_ts")));
+ this.robotPoseFieldSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_fs")));
+ this.robotPoseTargetSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6r_ts")));
+ this.targetPoseCameraSpace = LLResult.createPose3DRobot(LLResultTypes.parsePoseArray(data.optJSONArray("t6t_cs")));
+ this.targetPoseRobotSpace = LLResult.createPose3DRobot( LLResultTypes.parsePoseArray(data.optJSONArray("t6t_rs")));
+ this.targetArea = data.optDouble("ta", 0.0);
+ this.targetXPixels = data.optDouble("txp", 0.0);
+ this.targetYPixels = data.optDouble("typ", 0.0);
+ this.targetXDegrees = data.optDouble("tx", 0.0);
+ this.targetYDegrees = data.optDouble("ty", 0.0);
+ this.targetXDegreesNoCrosshair = data.optDouble("tx_nocross", 0.0);
+ this.targetYDegreesNoCrosshair = data.optDouble("ty_nocross", 0.0);
+ this.targetCorners = LLResultTypes.parsePoints(data.optJSONArray("pts"));
+ }
+
+ /**
+ * Gets the corner points of the detected target. The number of corners is not fixed.
+ *
+ * @return A list of corner points, each point represented as a list of two doubles [x, y].
+ */
+ public List> getTargetCorners() {
+ return targetCorners;
+ }
+
+ /**
+ * Gets the camera pose in target space.
+ *
+ * @return An Pose3D representing the camera pose.
+ */
+ public Pose3D getCameraPoseTargetSpace() {
+ return cameraPoseTargetSpace;
+ }
+
+ /**
+ * Gets the robot pose in field space based on this color target.
+ *
+ * @return An Pose3D representing the robot pose.
+ */
+ public Pose3D getRobotPoseFieldSpace() {
+ return robotPoseFieldSpace;
+ }
+
+ /**
+ * Gets the robot pose in target space.
+ *
+ * @return An Pose3D representing the robot pose.
+ */
+ public Pose3D getRobotPoseTargetSpace() {
+ return robotPoseTargetSpace;
+ }
+
+ /**
+ * Gets the target pose in camera space.
+ *
+ * @return An Pose3D representing the target pose.
+ */
+ public Pose3D getTargetPoseCameraSpace() {
+ return targetPoseCameraSpace;
+ }
+
+ /**
+ * Gets the target pose in robot space.
+ *
+ * @return An Pose3D representing the target pose.
+ */
+ public Pose3D getTargetPoseRobotSpace() {
+ return targetPoseRobotSpace;
+ }
+
+ /**
+ * Gets the area of the detected color target as a percentage of the image.
+ *
+ * @return The target area (0-100).
+ */
+ public double getTargetArea() {
+ return targetArea;
+ }
+
+ /**
+ * Gets the current tx in pixels from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXPixels() {
+ return targetXPixels;
+ }
+
+ /**
+ * Gets the current ty in pixels from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYPixels() {
+ return targetYPixels;
+ }
+
+ /**
+ * Gets the current tx in degrees from the crosshair
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegrees() {
+ return targetXDegrees;
+ }
+
+ /**
+ * Gets the current ty in degrees from the crosshair
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegrees() {
+ return targetYDegrees;
+ }
+
+ /**
+ * Gets the current tx in degrees from the principal pixel
+ *
+ * @return horizontal angular offset
+ */
+ public double getTargetXDegreesNoCrosshair() {
+ return targetXDegreesNoCrosshair;
+ }
+
+ /**
+ * Gets the current ty in degrees from the principal pixel
+ *
+ * @return vertical angular offset
+ */
+ public double getTargetYDegreesNoCrosshair() {
+ return targetYDegreesNoCrosshair;
+ }
+ }
+
+
+ /**
+ * Represents a calibration result. Calibration results are generated by the user in the UI's calibration tab.
+ */
+ public static class CalibrationResult {
+ private String displayName;
+ private double resX;
+ private double resY;
+ private double reprojectionError;
+ private double[] camMatVector;
+ private double[] distortionCoefficients;
+ private boolean valid;
+
+ private static final double DEFAULT_REPORT_VAL = 0.0;
+
+ /**
+ * Constructs a CalibrationResult with default values.
+ */
+ public CalibrationResult() {
+ this("", DEFAULT_REPORT_VAL, DEFAULT_REPORT_VAL, DEFAULT_REPORT_VAL,
+ new double[9], new double[5]);
+ }
+
+ /**
+ * Constructs a CalibrationResult with specified values.
+ *
+ * @param displayName The display name for the calibration.
+ * @param resX The X resolution of the calibration.
+ * @param resY The Y resolution of the calibration.
+ * @param reprojectionError The reprojection error of the calibration.
+ * @param camMatVector The camera matrix vector (9 elements).
+ * @param distortionCoefficients The distortion coefficients (5 elements).
+ */
+ public CalibrationResult(String displayName, double resX, double resY, double reprojectionError,
+ double[] camMatVector, double[] distortionCoefficients) {
+ this.displayName = displayName;
+ this.resX = resX;
+ this.resY = resY;
+ this.reprojectionError = reprojectionError;
+ this.camMatVector = camMatVector.clone();
+ this.distortionCoefficients = distortionCoefficients.clone();
+ this.valid = true;
+ }
+
+ /**
+ * Constructs a CalibrationResult from JSON data. (Package-private)
+ *
+ * @param json The JSON object containing calibration result data.
+ */
+ protected CalibrationResult(JSONObject json) {
+ //default value construction
+ this();
+ //parse json
+ if(json!=null)
+ {
+ try{
+ this.displayName = json.optString("DISPLAY_NAME", "");
+ this.reprojectionError = json.optDouble("REPROJECTION_ERROR", DEFAULT_REPORT_VAL);
+ this.resX = json.optDouble("RES_X", DEFAULT_REPORT_VAL);
+ this.resY = json.optDouble("RES_Y", DEFAULT_REPORT_VAL);
+
+ JSONArray intrinsicsMatrix = json.optJSONArray("INTRINSICS_MATRIX");
+ if (intrinsicsMatrix != null && intrinsicsMatrix.length() == 9) {
+ for (int i = 0; i < 9; i++) {
+ this.camMatVector[i] = intrinsicsMatrix.optDouble(i, 0.0);
+ }
+ } else {
+ this.valid = false;
+ }
+
+ JSONArray distortionCoeffs = json.optJSONArray("DISTORTION_COEFFICIENTS");
+ if (distortionCoeffs != null && distortionCoeffs.length() == 5) {
+ for (int i = 0; i < 5; i++) {
+ this.distortionCoefficients[i] = distortionCoeffs.optDouble(i, 0.0);
+ }
+ } else {
+ this.valid = false;
+ }
+
+ if (this.resX == DEFAULT_REPORT_VAL || this.resY == DEFAULT_REPORT_VAL) {
+ this.valid = false;
+ }
+ } catch (Exception e){ }
+ }
+ }
+
+ /**
+ * Checks if the calibration result is valid.
+ *
+ * @return true if the calibration result is valid, false otherwise.
+ */
+ public boolean isValid() {
+ return valid && resX != 0 && resY != 0 && camMatVector[0] != 0;
+ }
+
+ /**
+ * Gets the display name of the calibration.
+ *
+ * @return The display name.
+ */
+ public String getDisplayName() {
+ return displayName;
+ }
+
+ /**
+ * Gets the X resolution of the calibration.
+ *
+ * @return The X resolution.
+ */
+ public double getResX() {
+ return resX;
+ }
+
+ /**
+ * Gets the Y resolution of the calibration.
+ *
+ * @return The Y resolution.
+ */
+ public double getResY() {
+ return resY;
+ }
+
+ /**
+ * Gets the reprojection error of the calibration.
+ *
+ * @return The reprojection error.
+ */
+ public double getReprojectionError() {
+ return reprojectionError;
+ }
+
+ /**
+ * Gets the camera matrix vector.
+ *
+ * @return A copy of the camera matrix vector.
+ */
+ public double[] getCamMatVector() {
+ return camMatVector.clone();
+ }
+
+ /**
+ * Gets the distortion coefficients.
+ *
+ * @return A copy of the distortion coefficients.
+ */
+ public double[] getDistortionCoefficients() {
+ return distortionCoefficients.clone();
+ }
+
+ /**
+ * Converts a CalibrationResult to a JSONObject.
+ *
+ * @return A JSONObject representation of the CalibrationResult.
+ */
+ protected JSONObject toJson() {
+ JSONObject json = new JSONObject();
+ try {
+ json.put("DISPLAY_NAME", this.getDisplayName());
+ json.put("REPROJECTION_ERROR", this.getReprojectionError());
+ json.put("RES_X", this.getResX());
+ json.put("RES_Y", this.getResY());
+ json.put("INTRINSICS_MATRIX", new JSONArray(this.getCamMatVector()));
+ json.put("DISTORTION_COEFFICIENTS", new JSONArray(this.getDistortionCoefficients()));
+ } catch (JSONException e) {
+ e.printStackTrace();
+ }
+ return json;
+ }
+ }
+}
\ No newline at end of file
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java
new file mode 100644
index 000000000000..23f4bb9e3e7b
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/LLStatus.java
@@ -0,0 +1,247 @@
+package com.qualcomm.hardware.limelightvision;
+
+/*
+Copyright (c) 2024 Limelight Vision
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Limelight Vision nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
+import org.json.JSONObject;
+
+/**
+ * Represents the status of a Limelight.
+ */
+public class LLStatus {
+ private Quaternion cameraQuat;
+ private int cid;
+ private double cpu;
+ private double finalYaw;
+ private double fps;
+ private int hwType;
+ private int ignoreNT;
+ private int interfaceNeedsRefresh;
+ private String name;
+ private int pipeImgCount;
+ private int pipelineIndex;
+ private String pipelineType;
+ private double ram;
+ private int snapshotMode;
+ private double temp;
+
+ /**
+ * Constructs an LLStatus object with default values.
+ */
+ public LLStatus() {
+ this.cameraQuat = new Quaternion(1, 0, 0, 0, 0);
+ this.cid = 0;
+ this.cpu = 0.0;
+ this.finalYaw = 0.0;
+ this.fps = 0.0;
+ this.hwType = 0;
+ this.ignoreNT = 0;
+ this.interfaceNeedsRefresh = 0;
+ this.name = "";
+ this.pipeImgCount = 0;
+ this.pipelineIndex = 0;
+ this.pipelineType = "";
+ this.ram = 0.0;
+ this.snapshotMode = 0;
+ this.temp = 0.0;
+ }
+
+ /**
+ * Constructs an LLStatus object from a JSON string.
+ *
+ * @param json The JSONobject containing Limelight status data.
+ */
+ protected LLStatus(JSONObject json) {
+ this();
+ if(json != null)
+ {
+ try {
+ JSONObject quatObj = json.optJSONObject("cameraQuat");
+ if (quatObj != null) {
+ this.cameraQuat.w = (float)quatObj.optDouble("w", 1);
+ this.cameraQuat.x = (float)quatObj.optDouble("x", 0);
+ this.cameraQuat.y = (float)quatObj.optDouble("y", 0);
+ this.cameraQuat.z = (float)quatObj.optDouble("z", 0);
+ };
+ this.cid = json.optInt("cid", 0);
+ this.cpu = json.optDouble("cpu", 0.0);
+ this.finalYaw = json.optDouble("finalYaw", 0.0);
+ this.fps = json.optDouble("fps", 0.0);
+ this.hwType = json.optInt("hwType", 0);
+ this.ignoreNT = json.optInt("ignoreNT", 0);
+ this.interfaceNeedsRefresh = json.optInt("interfaceNeedsRefresh", 0);
+ this.name = json.optString("name", "");
+ this.pipeImgCount = json.optInt("pipeImgCount", 0);
+ this.pipelineIndex = json.optInt("pipelineIndex", 0);
+ this.pipelineType = json.optString("pipelineType", "");
+ this.ram = json.optDouble("ram", 0.0);
+ this.snapshotMode = json.optInt("snapshotMode", 0);
+ this.temp = json.optDouble("temp", 0.0);
+ } catch (Exception e) {
+
+ }
+ }
+ }
+
+ /**
+ * @return The camera quaternion as a double array [w, x, y, z].
+ */
+ public Quaternion getCameraQuat() {
+ return cameraQuat;
+ }
+
+ /**
+ * @return The camera sensor ID.
+ */
+ public int getCid() {
+ return cid;
+ }
+
+ /**
+ * @return The CPU usage as a percentage.
+ */
+ public double getCpu() {
+ return cpu;
+ }
+
+ /**
+ * @return The final yaw angle in degrees.
+ */
+ public double getFinalYaw() {
+ return finalYaw;
+ }
+
+ /**
+ * @return The frames per second being processed.
+ */
+ public double getFps() {
+ return fps;
+ }
+
+ /**
+ * @return The hardware type identifier.
+ */
+ public int getHwType() {
+ return hwType;
+ }
+
+ /**
+ * @return The ignore NetworkTables flag.
+ */
+ private int getIgnoreNT() {
+ return ignoreNT;
+ }
+
+ /**
+ * @return The interface needs refresh flag.
+ */
+ private int getInterfaceNeedsRefresh() {
+ return interfaceNeedsRefresh;
+ }
+
+ /**
+ * @return The name of the Limelight.
+ */
+ public String getName() {
+ return name;
+ }
+
+ /**
+ * @return The pipeline image count.
+ */
+ public int getPipeImgCount() {
+ return pipeImgCount;
+ }
+
+ /**
+ * @return The current pipeline index.
+ */
+ public int getPipelineIndex() {
+ return pipelineIndex;
+ }
+
+ /**
+ * @return The type of pipeline being used.
+ */
+ public String getPipelineType() {
+ return pipelineType;
+ }
+
+ /**
+ * @return The RAM usage as a percentage.
+ */
+ public double getRam() {
+ return ram;
+ }
+
+ /**
+ * @return The snapshot mode flag.
+ */
+ public int getSnapshotMode() {
+ return snapshotMode;
+ }
+
+ /**
+ * @return The temperature of the Limelight in degrees Celsius.
+ */
+ public double getTemp() {
+ return temp;
+ }
+
+ /**
+ * Returns a string representation of the LLStatus object.
+ *
+ * @return A string containing all the status parameters.
+ */
+ @Override
+ public String toString() {
+ return "LLStatus{" +
+ "cameraQuat=" + cameraQuat.toString() +
+ ", cid=" + cid +
+ ", cpu=" + cpu +
+ ", finalYaw=" + finalYaw +
+ ", fps=" + fps +
+ ", hwType=" + hwType +
+ ", ignoreNT=" + ignoreNT +
+ ", interfaceNeedsRefresh=" + interfaceNeedsRefresh +
+ ", name='" + name + '\'' +
+ ", pipeImgCount=" + pipeImgCount +
+ ", pipelineIndex=" + pipelineIndex +
+ ", pipelineType='" + pipelineType + '\'' +
+ ", ram=" + ram +
+ ", snapshotMode=" + snapshotMode +
+ ", temp=" + temp +
+ '}';
+ }
+}
\ No newline at end of file
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java
new file mode 100644
index 000000000000..f75a655c2f5c
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/limelightvision/Limelight3A.java
@@ -0,0 +1,732 @@
+package com.qualcomm.hardware.limelightvision;
+
+/*
+Copyright (c) 2024 Limelight Vision
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Limelight Vision nor the names of its contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+import com.qualcomm.hardware.limelightvision.LLResultTypes.CalibrationResult;
+import com.qualcomm.robotcore.hardware.HardwareDevice;
+import com.qualcomm.robotcore.util.SerialNumber;
+
+import org.firstinspires.ftc.robotcore.internal.usb.EthernetOverUsbSerialNumber;
+import org.json.JSONArray;
+import org.json.JSONObject;
+
+import java.io.BufferedReader;
+import java.io.IOException;
+import java.io.InputStreamReader;
+import java.net.HttpURLConnection;
+import java.net.InetAddress;
+import java.net.URL;
+import java.util.concurrent.Executors;
+import java.util.concurrent.ScheduledExecutorService;
+import java.util.concurrent.TimeUnit;
+import java.util.concurrent.atomic.AtomicInteger;
+
+
+/**
+ * Driver for Limelight3A Vision Sensor.
+ *
+ * {@link Limelight3A } provides support for the Limelight Vision Limelight3A Vision Sensor.
+ *
+ * @see Limelight
+ *
+ * Notes on configuration:
+ *
+ * The device presents itself, when plugged into a USB port on a Control Hub as an ethernet
+ * interface. A DHCP server running on the Limelight automatically assigns the Control Hub an
+ * ip address for the new ethernet interface.
+ *
+ * Since the Limelight is plugged into a USB port, it will be listed on the top level configuration
+ * activity along with the Control Hub Portal and other USB devices such as webcams. Typically
+ * serial numbers are displayed below the device's names. In the case of the Limelight device, the
+ * Control Hub's assigned ip address for that ethernet interface is used as the "serial number".
+ *
+ * Tapping the Limelight's name, transitions to a new screen where the user can rename the Limelight
+ * and specify the Limelight's ip address. Users should take care not to confuse the ip address of
+ * the Limelight itself, which can be configured through the Limelight settings page via a web browser,
+ * and the ip address the Limelight device assigned the Control Hub and which is displayed in small text
+ * below the name of the Limelight on the top level configuration screen.
+ */
+public class Limelight3A implements HardwareDevice
+{
+ private static final String TAG = "Limelight3A";
+ private InetAddress inetAddress;
+ private EthernetOverUsbSerialNumber serialNumber;
+ private String name;
+
+ private final String baseUrl;
+ private volatile LLResult latestResult;
+ private volatile ScheduledExecutorService executor;
+ private final Object executorLock = new Object();
+
+ private final AtomicInteger state = new AtomicInteger(0);
+ private static final int STOPPED = 0, STARTING = 1, RUNNING = 2;
+
+ private volatile boolean isExecutorInitialized = false;
+
+ private volatile long lastUpdateTime = 0;
+ private static final long ISCONNECTED_THRESHOLD = 250;
+ private static final int CONNECTION_TIMEOUT = 100;
+ private static final int GETREQUEST_TIMEOUT = 100;
+ private static final int POSTREQUEST_TIMEOUT = 15000;
+ private static final int DELETEREQUEST_TIMEOUT = 15000;
+
+ private static final int PYTHON_INPUT_MAX_SIZE = 32;
+ private long pollIntervalMs = 10; // Default poll rate = 100Hz
+ private static final int MIN_POLL_RATE_HZ = 1;
+ private static final int MAX_POLL_RATE_HZ = 250;
+
+
+ public Limelight3A(SerialNumber serialNumber, String name, InetAddress ipAddress) {
+ this.name = name;
+ this.serialNumber = (EthernetOverUsbSerialNumber)serialNumber;
+ this.inetAddress = ipAddress;
+ this.baseUrl = "http://blargh:5807";
+ this.executor = Executors.newSingleThreadScheduledExecutor();
+ }
+
+ /**
+ * Allows for re-initialization
+ */
+ private synchronized void ensureExecutorRunning() {
+ synchronized (executorLock) {
+ //lock to make sure executor doesn't change right after we check here (bh)
+ if (executor == null || executor.isShutdown()) {
+ executor = Executors.newSingleThreadScheduledExecutor();
+ isExecutorInitialized = false;
+ }
+ }
+ }
+
+ /**
+ * Starts or resumes periodic polling of Limelight data.
+ */
+ public synchronized void start() {
+ // handling rapid consecutive start calls with getandset bh
+ if (!state.compareAndSet(STOPPED, STARTING)) {
+ return;
+ }
+
+ try {
+ ensureExecutorRunning();
+ if (!isExecutorInitialized) {
+ executor.scheduleAtFixedRate(this::updateLatestResult, 0, pollIntervalMs, TimeUnit.MILLISECONDS);
+ isExecutorInitialized = true;
+ }
+ state.set(RUNNING);
+ } catch (Exception e) {
+ state.set(STOPPED);
+ }
+ }
+
+ /**
+ * Pauses polling of Limelight data.
+ */
+ public synchronized void pause() {
+ if (state.get() == RUNNING) {
+ state.set(STOPPED);
+ }
+ }
+
+
+ /**
+ * Stops polling of Limelight data.
+ */
+ public synchronized void stop() {
+ state.set(STOPPED);
+ isExecutorInitialized = false;
+ if (!executor.isShutdown()) {
+ executor.shutdownNow();
+ }
+ }
+
+ /**
+ * Checks if the polling is enabled.
+ *
+ * @return true if the polling is enabled
+ */
+ public boolean isRunning() {
+ return state.get() == RUNNING;
+ }
+
+ /**
+ * Sets the poll rate in Hertz (Hz). Must be called before start()
+ * The rate is clamped between 1 and 250 Hz.
+ *
+ * @param rateHz The desired poll rate in Hz.
+ */
+ public synchronized void setPollRateHz(int rateHz) {
+ if (state.get() == RUNNING)
+ {
+ return; // Early return if the executor is running
+ }
+
+ int clampedRate = Math.max(MIN_POLL_RATE_HZ, Math.min(rateHz, MAX_POLL_RATE_HZ));
+ this.pollIntervalMs = 1000 / clampedRate;
+ }
+
+ /**
+ * Gets the time elapsed since the last update.
+ *
+ * @return The time in milliseconds since the last update.
+ */
+ public long getTimeSinceLastUpdate() {
+ return System.currentTimeMillis() - lastUpdateTime;
+ }
+
+ /**
+ * Checks if the Limelight is currently connected.
+ *
+ * @return true if connected, false otherwise.
+ */
+ public boolean isConnected() {
+ return getTimeSinceLastUpdate() <= ISCONNECTED_THRESHOLD;
+ }
+
+ /**
+ * Updates the latest result from the Limelight.
+ */
+ private void updateLatestResult() {
+ if (state.get() != RUNNING) return;
+
+ try {
+
+ JSONObject result = sendGetRequest("/results");
+ if (result != null) {
+ this.latestResult = LLResult.parse(result);
+ this.lastUpdateTime = System.currentTimeMillis();
+ }
+ }
+ catch (Exception e){
+ //todo
+ }
+ }
+
+ /**
+ * Gets the latest result from the Limelight.
+ *
+ * @return The latest LLResult object.
+ */
+ public LLResult getLatestResult(){
+ if (this.latestResult == null) {
+ JSONObject fakeJSON = new JSONObject();
+ this.latestResult = LLResult.parse(fakeJSON);
+ }
+ return this.latestResult;
+ }
+
+ /**
+ * Gets the current status of the Limelight.
+ *
+ * @return A populated LLStatus object, or a default status object if request fails.
+ */
+ public LLStatus getStatus() {
+ JSONObject statusJson = sendGetRequest("/status");
+ if(statusJson == null)
+ {
+ return new LLStatus();
+ }
+ return new LLStatus(statusJson);
+ }
+
+ /**
+ * Gets the hardware report from the Limelight. The Hardware Report contains calibration data.
+ * This is a highly advanced feature that is not necessary for FTC Teams. Marked private for now.
+ *
+ * @return A JSONObject containing the hardware report.
+ */
+ private JSONObject getHWReport() {
+ return sendGetRequest("/hwreport");
+ }
+
+ /**
+ * Reloads the current Limelight pipeline.
+ *
+ * @return true if successful, false otherwise.
+ */
+ public boolean reloadPipeline() {
+ return sendPostRequest("/reload-pipeline", null);
+ }
+
+ /**
+ * Gets the default pipeline.
+ *
+ * @return A JSONObject containing the default pipeline configuration.
+ */
+ private JSONObject getPipelineDefault() {
+ return sendGetRequest("/pipeline-default");
+ }
+
+ /**
+ * Gets the pipeline at a specific index.
+ *
+ * @param index The index of the pipeline to retrieve.
+ * @return A JSONObject containing the pipeline configuration.
+ */
+ private JSONObject getPipelineAtIndex(int index) {
+ return sendGetRequest("/pipeline-atindex?index=" + index);
+ }
+
+ /**
+ * Switches to a pipeline at the specified index.
+ *
+ * @param index The index of the pipeline to switch to.
+ * @return true if successful, false otherwise.
+ */
+ public boolean pipelineSwitch(int index) {
+ return sendPostRequest("/pipeline-switch?index=" + index, null);
+ }
+
+ /**
+ * Gets the names of available snapscripts.
+ * This method is not necessary for FTC teams. Marked as private
+ *
+ * @return A JSONObject containing the names of available snapscripts.
+ */
+ private JSONObject getSnapscriptNames() {
+ return sendGetRequest("/getsnapscriptnames");
+ }
+
+ /**
+ * Captures a snapshot with the given name.
+ *
+ * @param snapname The name to give to the captured snapshot.
+ * @return true if successful, false otherwise.
+ */
+ public boolean captureSnapshot(String snapname) {
+ return sendPostRequest("/capture-snapshot?snapname=" + snapname, null);
+ }
+
+ /**
+ * Gets the manifest of available snapshots.
+ * This method is not necessary for FTC teams. Marked as private
+ *
+ * @return A JSONObject containing the snapshot manifest.
+ */
+ private JSONObject snapshotManifest() {
+ return sendGetRequest("/snapshotmanifest");
+ }
+
+ /**
+ * Deletes all snapshots.
+ *
+ * @return true if successful, false otherwise.
+ */
+ public boolean deleteSnapshots() {
+ return sendDeleteRequest("/delete-snapshots");
+ }
+
+ /**
+ * Deletes a specific snapshot.
+ *
+ * @param snapname The name of the snapshot to delete.
+ * @return true if successful, false otherwise.
+ */
+ public boolean deleteSnapshot(String snapname) {
+ return sendDeleteRequest("/delete-snapshot?snapname=" + snapname);
+ }
+
+ /**
+ * Updates the current pipeline configuration.
+ *
+ * @param profileJson The new pipeline configuration as a JSONObject.
+ * @param flush Whether to flush the configuration to persistent storage.
+ * @return true if successful, false otherwise.
+ */
+ private boolean updatePipeline(JSONObject profileJson, boolean flush) {
+ String url = "/update-pipeline" + (flush ? "?flush=1" : "");
+ return sendPostRequest(url, profileJson.toString());
+ }
+
+ /**
+ * Updates the Python SnapScript inputs with 8 double values.
+ *
+ * @param input1 The first input value
+ * @param input2 The second input value
+ * @param input3 The third input value
+ * @param input4 The fourth input value
+ * @param input5 The fifth input value
+ * @param input6 The sixth input value
+ * @param input7 The seventh input value
+ * @param input8 The eighth input value
+ * @return True if successful, false otherwise.
+ */
+ public boolean updatePythonInputs(double input1, double input2, double input3, double input4,
+ double input5, double input6, double input7, double input8) {
+ double[] inputs = new double[]{input1, input2, input3, input4, input5, input6, input7, input8};
+ return updatePythonInputs(inputs);
+ }
+
+ /**
+ * Updates the Python SnapScript inputs.
+ *
+ * @param inputs A double array containing the new Python inputs.
+ * @return True if successful, false otherwise. False if array is empty or contains more than 32 elements
+ */
+ public boolean updatePythonInputs(double[] inputs) {
+ if (inputs == null || inputs.length == 0 || inputs.length > PYTHON_INPUT_MAX_SIZE) {
+ return false; // Invalid input size
+ }
+
+ try {
+ JSONArray jsonArray = new JSONArray(inputs);
+ return sendPostRequest("/update-pythoninputs", jsonArray.toString());
+ } catch (Exception e) {
+ //
+ return false;
+ }
+ }
+
+ /**
+ * Updates the robot orientation for MegaTag2.
+ *
+ * @param yaw The yaw value of the robot, aligned with the field map
+ * @return true if successful, false otherwise.
+ */
+ public boolean updateRobotOrientation(double yaw) {
+ double[] orientationData = {yaw, 0, 0, 0, 0, 0};
+ try{
+ JSONArray jsonArray = new JSONArray(orientationData);
+ return sendPostRequest("/update-robotorientation", jsonArray.toString());
+ } catch (Exception e){
+ return false;
+ }
+ }
+
+ /**
+ * Uploads a pipeline to a specific slot.
+ *
+ * @param profileJson The new pipeline configuration as a JSONObject.
+ * @param index The index at which to upload the pipeline (null for default).
+ * @return true if successful, false otherwise.
+ */
+ private boolean uploadPipeline(JSONObject profileJson, Integer index) {
+ String url = "/upload-pipeline" + (index != null ? "?index=" + index : "");
+ return sendPostRequest(url, profileJson.toString());
+ }
+
+ /**
+ * Uploads a pipeline to a specific slot.
+ *
+ * @param jsonString The new pipeline configuration as a String containing JSON.
+ * @param index The index at which to upload the pipeline (null for default).
+ * @return true if successful, false otherwise.
+ */
+ public boolean uploadPipeline(String jsonString, Integer index){
+ String url = "/upload-pipeline" + (index != null ? "?index=" + index : "");
+ return sendPostRequest(url, jsonString);
+ }
+
+ /**
+ * Uploads a new fiducial field map. Early exits if map is empty or doesn't specify a type
+ *
+ * @param fieldmap The new field map configuration.
+ * @param index The index at which to upload the field map (null for default).
+ * @return true if successful, false otherwise.
+ */
+ public boolean uploadFieldmap(LLFieldMap fieldmap, Integer index) {
+ if(!fieldmap.isValid())
+ {
+ return false;
+ }
+ String url = "/upload-fieldmap" + (index != null ? "?index=" + index : "");
+ return sendPostRequest(url, fieldmap.toJson().toString());
+ }
+
+ /**
+ * Uploads new Python code.
+ *
+ * @param pythonString The Python code as a string.
+ * @param index The index at which to upload the Python code (null for default).
+ * @return true if successful, false otherwise.
+ */
+ public boolean uploadPython(String pythonString, Integer index) {
+ String url = "/upload-python" + (index != null ? "?index=" + index : "");
+ return sendPostRequest(url, pythonString);
+ }
+
+ /**
+ * Gets the default calibration data.
+ *
+ * @return A CalibrationResult containing the default calibration data.
+ */
+ public CalibrationResult getCalDefault() {
+ JSONObject calData = sendGetRequest("/cal-default");
+ return new CalibrationResult(calData);
+ }
+
+
+ /**
+ * Gets calibration data from the user-generated calibration file.
+ *
+ * @return A CalibrationResult containing the calibration data from the file.
+ */
+ public CalibrationResult getCalFile() {
+ JSONObject calData = sendGetRequest("/cal-file");
+ return new CalibrationResult(calData);
+ }
+
+ /**
+ * Gets the calibration data from the Limelight EEPROM.
+ *
+ * @return A CalibrationResult containing the calibration data from the EEPROM.
+ */
+ public CalibrationResult getCalEEPROM() {
+ JSONObject calData = sendGetRequest("/cal-eeprom");
+ return new CalibrationResult(calData);
+ }
+
+ /**
+ * Gets the latest calibration result. This result is not necessarily used by the camera in any way
+ *
+ * @return A CalibrationResult containing the latest calibration result.
+ */
+ public CalibrationResult getCalLatest() {
+ JSONObject calData = sendGetRequest("/cal-latest");
+ return new CalibrationResult(calData);
+ }
+
+ /**
+ * Upload calibration data to the EEPROM
+ *
+ * @param calResult A CalibrationResult containing the new calibration data.
+ * @return true if successful, false otherwise.
+ */
+ private boolean updateCalEEPROM(CalibrationResult calResult) {
+ return sendPostRequest("/cal-eeprom", calResult.toJson().toString());
+ }
+
+ /**
+ * Updates the calibration file stored in the Limelight filesystem
+ *
+ * @param calResult A CalibrationResult containing the new calibration data.
+ * @return true if successful, false otherwise.
+ */
+ private boolean updateCalFile(CalibrationResult calResult) {
+ return sendPostRequest("/cal-file", calResult.toJson().toString());
+ }
+
+ /**
+ * Deletes the latest calibration data.
+ *
+ * @return true if successful, false otherwise.
+ */
+ private boolean deleteCalLatest() {
+ return sendDeleteRequest("/cal-latest");
+ }
+
+ /**
+ * Deletes the calibration data from the EEPROM.
+ *
+ * @return true if successful, false otherwise.
+ */
+ private boolean deleteCalEEPROM() {
+ return sendDeleteRequest("/cal-eeprom");
+ }
+
+ /**
+ * Deletes the calibration data from the file.
+ *
+ * @return true if successful, false otherwise.
+ */
+ private boolean deleteCalFile() {
+ return sendDeleteRequest("/cal-file");
+ }
+
+ /**
+ * Sends a GET request to the specified endpoint.
+ *
+ * @param endpoint The endpoint to send the request to.
+ * @return A JSONObject containing the response, or null if the request fails.
+ */
+ private JSONObject sendGetRequest(String endpoint) {
+ HttpURLConnection connection = null;
+ try {
+ String urlString = baseUrl + endpoint;
+ URL url = new URL(urlString);
+ connection = (HttpURLConnection) url.openConnection();
+ connection.setRequestMethod("GET");
+ connection.setReadTimeout(GETREQUEST_TIMEOUT);
+ connection.setConnectTimeout(CONNECTION_TIMEOUT);
+
+ int responseCode = connection.getResponseCode();
+ if (responseCode == HttpURLConnection.HTTP_OK) {
+ String response = readResponse(connection);
+ return new JSONObject(response);
+ } else {
+ //System.out.println("HTTP GET Error: " + responseCode);
+ }
+ } catch (Exception e) {
+ //e.printStackTrace();
+ } finally {
+ if (connection != null) {
+ connection.disconnect();
+ }
+ }
+ return null;
+ }
+
+ /**
+ * Reads the response from an HTTP connection.
+ *
+ * @param connection The HttpURLConnection to read from.
+ * @return A String containing the response.
+ * @throws IOException If an I/O error occurs.
+ */
+ private String readResponse(HttpURLConnection connection) throws IOException {
+ BufferedReader reader = new BufferedReader(new InputStreamReader(connection.getInputStream()));
+ StringBuilder response = new StringBuilder();
+ String line;
+
+ while ((line = reader.readLine()) != null) {
+ response.append(line);
+ }
+ reader.close();
+
+ return response.toString();
+ }
+
+ /**
+ * Sends a POST request to the specified endpoint.
+ *
+ * @param endpoint The endpoint to send the request to.
+ * @param data The data to send in the request body.
+ * @return true if successful, false otherwise.
+ */
+ private boolean sendPostRequest(String endpoint, String data) {
+ return false; // Wily Works override without a timeout
+
+// HttpURLConnection connection = null;
+// try {
+// String urlString = baseUrl + endpoint;
+// URL url = new URL(urlString);
+// connection = (HttpURLConnection) url.openConnection();
+// connection.setRequestMethod("POST");
+// connection.setDoOutput(true);
+// connection.setRequestProperty("Content-Type", "application/json");
+// connection.setReadTimeout(POSTREQUEST_TIMEOUT);
+// connection.setConnectTimeout(CONNECTION_TIMEOUT);
+//
+// if (data != null) {
+// try (OutputStream os = connection.getOutputStream()) {
+// byte[] input = data.getBytes(StandardCharsets.UTF_8);
+// os.write(input, 0, input.length);
+// }
+// }
+//
+// int responseCode = connection.getResponseCode();
+// if (responseCode == HttpURLConnection.HTTP_OK) {
+// return true;
+// } else {
+// //System.out.println("HTTP POST Error: " + responseCode);
+// }
+// } catch (Exception e) {
+// //e.printStackTrace();
+// } finally {
+// if (connection != null) {
+// connection.disconnect();
+// }
+// }
+// return false;
+ }
+
+ /**
+ * Sends a DELETE request to the specified endpoint.
+ *
+ * @param endpoint The endpoint to send the request to.
+ * @return true if successful, false otherwise.
+ */
+ private boolean sendDeleteRequest(String endpoint) {
+ HttpURLConnection connection = null;
+ try {
+ String urlString = baseUrl + endpoint;
+ URL url = new URL(urlString);
+ connection = (HttpURLConnection) url.openConnection();
+ connection.setRequestMethod("DELETE");
+ connection.setReadTimeout(DELETEREQUEST_TIMEOUT);
+ connection.setConnectTimeout(CONNECTION_TIMEOUT);
+
+ int responseCode = connection.getResponseCode();
+ if (responseCode == HttpURLConnection.HTTP_OK) {
+ return true;
+ } else {
+ //System.out.println("HTTP DELETE Error: " + responseCode);
+ }
+ } catch (Exception e) {
+ //e.printStackTrace();
+ } finally{
+ if (connection != null) {
+ connection.disconnect();
+ }
+ }
+ return false;
+ }
+
+ /**
+ * Shuts down the Limelight connection and stops all ongoing processes.
+ */
+ public void shutdown() {
+ stop();
+ }
+
+ @Override
+ public Manufacturer getManufacturer() {
+ return Manufacturer.LimelightVision;
+ }
+
+ @Override
+ public String getDeviceName() {
+ return name;
+ }
+
+ @Override
+ public String getConnectionInfo() {
+ return "";
+ }
+
+ @Override
+ public int getVersion() {
+ return 0;
+ }
+
+ @Override
+ public void resetDeviceConfigurationForOpMode() {
+ }
+
+ @Override
+ public void close() {
+ stop();
+ }
+
+}
+
diff --git a/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java b/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java
new file mode 100644
index 000000000000..330449ec8e50
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/hardware/sparkfun/SparkFunLEDStick.java
@@ -0,0 +1,195 @@
+package com.qualcomm.hardware.sparkfun;
+
+import android.graphics.Color;
+
+import androidx.annotation.ColorInt;
+
+import com.qualcomm.robotcore.hardware.I2cAddr;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchDevice;
+import com.qualcomm.robotcore.hardware.I2cDeviceSynchSimple;
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType;
+
+/**
+ * Support for the Sparkfun QWIIC LED Stick
+ * To connect it directly, you need this cable
+ */
+@I2cDeviceType()
+@DeviceProperties(name = "@string/sparkfun_led_stick_name",
+ description = "@string/sparkfun_led_stick_description",
+ xmlTag = "QWIIC_LED_STICK")
+public class SparkFunLEDStick extends I2cDeviceSynchDevice {
+ private final static boolean LIMIT_TO_ONE_STICK = true;
+ private final static int MAX_SEGMENT_LENGTH = 12;
+ private enum Commands {
+ CHANGE_LED_LENGTH(0x70),
+ WRITE_SINGLE_LED_COLOR(0x71),
+ WRITE_ALL_LED_COLOR(0x72),
+ WRITE_RED_ARRAY(0x73),
+ WRITE_GREEN_ARRAY(0x74),
+ WRITE_BLUE_ARRAY(0x75),
+ WRITE_SINGLE_LED_BRIGHTNESS(0x76),
+ WRITE_ALL_LED_BRIGHTNESS(0x77),
+ WRITE_ALL_LED_OFF(0x78);
+ final int bVal;
+
+ Commands(int bVal) {
+ this.bVal = bVal;
+ }
+ }
+
+ /**
+ * Change the color of a specific LED
+ *
+ * @param position which LED to change (1 - 255)
+ * @param color what color to set it to
+ */
+ public void setColor(int position, @ColorInt int color) {
+ byte[] data = new byte[4];
+ data[0] = (byte) position;
+ data[1] = (byte) Color.red(color);
+ data[2] = (byte) Color.green(color);
+ data[3] = (byte) Color.blue(color);
+ writeI2C(Commands.WRITE_SINGLE_LED_COLOR, data);
+ }
+
+ /**
+ * Change the color of all LEDs to a single color
+ *
+ * @param color what the color should be
+ */
+ public void setColor(@ColorInt int color) {
+ byte[] data = new byte[3];
+ data[0] = (byte) Color.red(color);
+ data[1] = (byte) Color.green(color);
+ data[2] = (byte) Color.blue(color);
+ writeI2C(Commands.WRITE_ALL_LED_COLOR, data);
+ }
+
+ /**
+ * Send a segment of the LED array
+ *
+ * @param cmd command to send
+ * @param array the values (limited from 0..255)
+ * @param offset the starting value (LED only, array starts at 0)
+ * @param length the length to send
+ */
+ private void sendSegment(Commands cmd, int[] array, int offset, int length) {
+ byte[] data = new byte[length + 2];
+ data[0] = (byte) length;
+ data[1] = (byte) offset;
+
+ for (int i = 0; i < length; i++) {
+ data[2 + i] = (byte) array[i];
+ }
+ writeI2C(cmd, data);
+ }
+
+ /**
+ * Change the color of an LED color segment
+ *
+ * @param colors what the colors should be
+ * @param offset where in the array to start
+ * @param length length to send (limited to 12)
+ */
+ private void setLEDColorSegment(@ColorInt int[] colors, int offset, int length) {
+ // Copy a segment of elements from the colors array into separate arrays for red, green, and blue.
+ int[] redArray = new int[length];
+ int[] greenArray = new int[length];
+ int[] blueArray = new int[length];
+
+ // Here we iterate [0, length) because we are filling the red, green, and blue arrays.
+ for (int i = 0; i < length; i++) {
+ // Use the offset when indexing into the colors array so we get the appropriate segement of elements.
+ redArray[i] = Color.red(colors[i + offset]);
+ greenArray[i] = Color.green(colors[i + offset]);
+ blueArray[i] = Color.blue(colors[i + offset]);
+ }
+ sendSegment(Commands.WRITE_RED_ARRAY, redArray, offset, length);
+ sendSegment(Commands.WRITE_GREEN_ARRAY, greenArray, offset, length);
+ sendSegment(Commands.WRITE_BLUE_ARRAY, blueArray, offset, length);
+ }
+
+ /**
+ * Change the color of all LEDs using arrays
+ *
+ * @param colors array of colors to set lights to
+ */
+ public void setColors(@ColorInt int[] colors) {
+ int length = colors.length;
+
+ if (LIMIT_TO_ONE_STICK && length > 10) {
+ length = 10;
+ }
+
+ int numInLastSegment = length % MAX_SEGMENT_LENGTH;
+ int numSegments = length / MAX_SEGMENT_LENGTH;
+ for (int i = 0; i < numSegments; i++) {
+ setLEDColorSegment(colors, i * MAX_SEGMENT_LENGTH, MAX_SEGMENT_LENGTH);
+ }
+ if (numInLastSegment > 0) {
+ setLEDColorSegment(colors, numSegments * MAX_SEGMENT_LENGTH, numInLastSegment);
+ }
+ }
+
+ /**
+ * Set the brightness of an individual LED
+ *
+ * @param position which LED to change (1-255)
+ * @param brightness brightness level (0-31)
+ */
+ public void setBrightness(int position, int brightness) {
+ byte[] data = new byte[2];
+ data[0] = (byte) position;
+ data[1] = (byte) brightness;
+ writeI2C(Commands.WRITE_SINGLE_LED_BRIGHTNESS, data);
+ }
+
+ /**
+ * Set the brightness of all LEDs
+ *
+ * @param brightness brightness level (0-31)
+ */
+ public void setBrightness(int brightness) {
+ byte[] data = new byte[1];
+ data[0] = (byte) brightness;
+ writeI2C(Commands.WRITE_ALL_LED_BRIGHTNESS, data);
+ }
+
+ /**
+ * Turn all LEDS off...
+ */
+ public void turnAllOff() {
+ setColor(0);
+ }
+
+ private void writeI2C(Commands cmd, byte[] data) {}
+// {
+// deviceClient.write(cmd.bVal, data);
+// }
+
+ @Override
+ public Manufacturer getManufacturer() {
+ return Manufacturer.SparkFun;
+ }
+
+ @Override
+ protected synchronized boolean doInitialize() {
+ return true;
+ }
+
+ @Override
+ public String getDeviceName() {
+ return "SparkFun Qwiic LED Strip";
+ }
+
+ private final static I2cAddr ADDRESS_I2C_DEFAULT = I2cAddr.create7bit(0x23);
+
+ public SparkFunLEDStick(I2cDeviceSynchSimple deviceClient, boolean deviceClientIsOwned) {
+ super(deviceClient, deviceClientIsOwned);
+
+// deviceClient.setI2cAddress(ADDRESS_I2C_DEFAULT);
+// super.registerArmingStateCallback(false);
+ }
+
+}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java
new file mode 100644
index 000000000000..6dd1455ca25d
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/EventLoopManager.java
@@ -0,0 +1,15 @@
+package com.qualcomm.robotcore.eventloop;
+
+// public class EventLoopManager implements RecvLoopRunnable.RecvLoopCallback, NetworkConnection.NetworkConnectionCallback, PeerStatusCallback, SyncdDevice.Manager {
+public class EventLoopManager implements SyncdDevice.Manager {
+
+ @Override
+ public void registerSyncdDevice(SyncdDevice device) {
+
+ }
+
+ @Override
+ public void unregisterSyncdDevice(SyncdDevice device) {
+
+ }
+}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java
new file mode 100644
index 000000000000..bf2743f648ec
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/SyncdDevice.java
@@ -0,0 +1,9 @@
+package com.qualcomm.robotcore.eventloop;
+
+public interface SyncdDevice {
+
+ interface Manager {
+ void registerSyncdDevice(SyncdDevice device);
+ void unregisterSyncdDevice(SyncdDevice device);
+ }
+}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java
index 698f94cc0afe..c4f3a7c1f6a2 100644
--- a/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/eventloop/opmode/OpModeManagerImpl.java
@@ -2,8 +2,13 @@
import androidx.annotation.Nullable;
-public class OpModeManagerImpl {
+public class OpModeManagerImpl implements OpModeManagerNotifier {
public @Nullable OpMode registerListener(OpModeManagerNotifier.Notifications listener) {
return null;
}
+
+ @Override
+ public void unregisterListener(Notifications listener) {
+
+ }
}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java
new file mode 100644
index 000000000000..958389666839
--- /dev/null
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/AnalogInput.java
@@ -0,0 +1,35 @@
+package com.qualcomm.robotcore.hardware;
+
+import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties;
+
+/**
+ * Control a single analog device
+ */
+@DeviceProperties(name = "@string/configTypeAnalogInput", xmlTag = "AnalogInput", builtIn = true)
+public class AnalogInput implements HardwareDevice {
+ @Override public Manufacturer getManufacturer() { return Manufacturer.Other; }
+
+ public double getVoltage() { return 0; }
+
+ public double getMaxVoltage() { return 0; }
+
+ @Override
+ public String getDeviceName() { return ""; }
+
+ @Override
+ public String getConnectionInfo() { return ""; }
+
+ @Override
+ public int getVersion() {
+ return 1;
+ }
+
+ @Override
+ public void resetDeviceConfigurationForOpMode() {
+ }
+
+ @Override
+ public void close() {
+ // take no action
+ }
+}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java
index 0e7c9601d17d..cace5ab66d4d 100644
--- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareDevice.java
@@ -37,7 +37,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
public interface HardwareDevice {
enum Manufacturer {
- Unknown, Other, Lego, HiTechnic, ModernRobotics, Adafruit, Matrix, Lynx, AMS, STMicroelectronics, Broadcom, DFRobot, SparkFun
+ Unknown, Other, Lego, HiTechnic, ModernRobotics, Adafruit, Matrix, Lynx, AMS, STMicroelectronics, Broadcom, DFRobot, SparkFun, LimelightVision
}
/**
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java
index 4535f06a70b6..246943823f1a 100644
--- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/HardwareMap.java
@@ -1,6 +1,15 @@
package com.qualcomm.robotcore.hardware;
+import android.content.Context;
+
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
import com.wilyworks.simulator.framework.WilyHardwareMap;
public class HardwareMap extends WilyHardwareMap {
+ public HardwareMap() {
+ super(null, null);
+ }
+ public HardwareMap(Context appContext, OpModeManagerNotifier notifier) {
+ super(appContext, notifier);
+ }
}
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java
index d9fe88193759..e0c14ea86bd4 100644
--- a/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/hardware/LED.java
@@ -34,10 +34,9 @@
@DeviceProperties(name = "@string/configTypeLED", xmlTag = "Led", builtIn = true, description = "@string/led_description")
public class LED implements HardwareDevice, SwitchableLight {
-
@Override
public Manufacturer getManufacturer() {
- return null;
+ return Manufacturer.Other;
}
@Override
@@ -73,6 +72,8 @@ public void enableLight(boolean enable) {
public boolean isLightOn() {
return false;
}
+
+ public void enable(boolean enableLed) { }
/**
* Turns the light on
*/
diff --git a/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java b/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java
index 3ff07af8eaed..6f1cbd51cbef 100644
--- a/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java
+++ b/WilyCore/src/main/java/com/qualcomm/robotcore/util/SerialNumber.java
@@ -31,5 +31,198 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
package com.qualcomm.robotcore.util;
-public abstract class SerialNumber {
+/* Copyright (c) 2014, 2015 Qualcomm Technologies Inc
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Qualcomm Technologies Inc nor the names of its contributors
+may be used to endorse or promote products derived from this software without
+specific prior written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */
+
+import androidx.annotation.Nullable;
+
+import com.google.gson.TypeAdapter;
+import com.google.gson.annotations.JsonAdapter;
+import com.google.gson.stream.JsonReader;
+import com.google.gson.stream.JsonWriter;
+
+import org.firstinspires.ftc.robotcore.internal.system.Misc;
+
+import java.io.IOException;
+import java.io.Serializable;
+import java.util.HashMap;
+import java.util.UUID;
+
+/**
+ */
+@SuppressWarnings("WeakerAccess")
+@JsonAdapter(SerialNumber.GsonTypeAdapter.class)
+public abstract class SerialNumber implements Serializable {
+
+ protected SerialNumber(String serialNumberString) {
+ }
+
+ public static SerialNumber createFake() {
+ return null;
+ }
+
+ public static SerialNumber createEmbedded() { return null; }
+
+ public static SerialNumber fromString(@Nullable String serialNumberString) {
+ return null;
+ }
+
+ public static @Nullable SerialNumber fromStringOrNull(@Nullable String serialNumberString) {
+ return null;
+ }
+
+ public static @Nullable SerialNumber fromUsbOrNull(@Nullable String serialNumberString) {
+ return null;
+ }
+
+ /** Makes up a serial-number-like-thing for USB devices that internally lack a serial number. */
+ public static SerialNumber fromVidPid(int vid, int pid, String connectionPath) {
+ return null;
+ }
+
+ //------------------------------------------------------------------------------------------------
+ // Gson
+ //------------------------------------------------------------------------------------------------
+
+ static class GsonTypeAdapter extends TypeAdapter {
+ @Override public void write(JsonWriter writer, SerialNumber serialNumber) throws IOException {
+ if (serialNumber==null) {
+ writer.nullValue();
+ } else {
+ writer.value(serialNumber.getString());
+ }
+ }
+
+ @Override public SerialNumber read(JsonReader reader) throws IOException {
+ return SerialNumber.fromStringOrNull(reader.nextString());
+ }
+ }
+
+ //------------------------------------------------------------------------------------------------
+ // Accessing
+ //------------------------------------------------------------------------------------------------
+
+ public boolean isVendorProduct() {
+ return false;
+ }
+
+ /**
+ * Returns whether the indicated serial number is one of the legacy
+ * fake serial number forms or not.
+ * @return whether the the serial number is a legacy fake form of serial number
+ */
+ public boolean isFake() {
+ return false;
+ }
+
+ /**
+ * Returns whether the serial number is one of an actual USB device.
+ */
+ public boolean isUsb() {
+ return false;
+ }
+
+ /**
+ * Returns whether the serial number is the one used for the embedded
+ * Expansion Hub inside a Rev Control Hub.
+ */
+ public boolean isEmbedded() {
+ return false;
+ }
+
+ /**
+ * Returns the string contents of the serial number. Result is not intended to be
+ * displayed to humans.
+ * @see #toString()
+ */
+ public String getString() { return ""; }
+
+
+ /**
+ */
+ public SerialNumber getScannableDeviceSerialNumber() {
+ return this;
+ }
+
+ //------------------------------------------------------------------------------------------------
+ // Comparison
+ //------------------------------------------------------------------------------------------------
+
+ public boolean matches(Object pattern) {
+ return this.equals(pattern);
+ }
+
+ @Override
+ public boolean equals(Object object) {
+ if (object == null) return false;
+ if (object == this) return true;
+
+ if (object instanceof SerialNumber) {
+ return false;
+ }
+
+ if (object instanceof String) {
+ return this.equals((String)object);
+ }
+
+ return false;
+ }
+
+ // separate method to avoid annoying Android Studio inspection warnings when comparing SerialNumber against String
+ public boolean equals(String string) {
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return 0;
+ }
+
+ //------------------------------------------------------------------------------------------------
+ // Serial number display name management
+ //------------------------------------------------------------------------------------------------
+
+ protected static final HashMap deviceDisplayNames = new HashMap();
+
+ public static void noteSerialNumberType(SerialNumber serialNumber, String typeName) {
+ synchronized (deviceDisplayNames) {
+ deviceDisplayNames.put(serialNumber.getString(), Misc.formatForUser("%s [%s]", typeName, serialNumber));
+ }
+ }
+
+ public static String getDeviceDisplayName(SerialNumber serialNumber) {
+ synchronized (deviceDisplayNames) {
+ String result = deviceDisplayNames.get(serialNumber.getString());
+ return result;
+ }
+ }
+
}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java
index 49297f3ab5ac..9c214b87cf6b 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/WilyCore.java
@@ -1,5 +1,6 @@
package com.wilyworks.simulator;
+import static java.lang.ClassLoader.getSystemClassLoader;
import static java.lang.System.nanoTime;
import static java.lang.Thread.currentThread;
import static java.lang.Thread.sleep;
@@ -11,15 +12,15 @@
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.wilyworks.common.Wily;
import com.wilyworks.common.WilyWorks;
+import com.wilyworks.simulator.framework.Field;
import com.wilyworks.simulator.framework.InputManager;
import com.wilyworks.simulator.framework.Simulation;
import com.wilyworks.simulator.framework.WilyTelemetry;
-import com.qualcomm.robotcore.hardware.Gamepad;
-
-import com.wilyworks.simulator.framework.Field;
+import com.wilyworks.simulator.framework.mechsim.MechSim;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.reflections.Reflections;
@@ -30,8 +31,6 @@
import java.awt.Dimension;
import java.awt.Graphics2D;
import java.awt.Image;
-import java.awt.event.ItemEvent;
-import java.awt.event.ItemListener;
import java.awt.event.WindowAdapter;
import java.awt.event.WindowEvent;
import java.awt.image.BufferStrategy;
@@ -41,6 +40,7 @@
import java.io.StringWriter;
import java.lang.reflect.Constructor;
import java.lang.reflect.InvocationTargetException;
+import java.lang.reflect.Method;
import java.util.ArrayList;
import java.util.Comparator;
import java.util.HashSet;
@@ -105,7 +105,7 @@ public void windowClosing(WindowEvent e) {
});
setSize(WINDOW_WIDTH, WINDOW_HEIGHT);
- setLocation(400, 0);
+ setLocation(250, 0);
setResizable(false);
Choice dropDown = new Choice();
@@ -148,10 +148,13 @@ public void windowClosing(WindowEvent e) {
button.addActionListener(actionEvent -> {
switch (WilyCore.status.state) {
- case STOPPED:
- // Inform the main thread of the choice and save the preference:
+ case STOPPED: // It was in STOPPED state, now transition to INITIALIZED
OpModeChoice opModeChoice = opModeChoices.get(dropDown.getSelectedIndex());
- WilyCore.status = new WilyCore.Status(WilyCore.State.INITIALIZED, opModeChoice.klass, button);
+ OpMode opMode = WilyCore.createOpMode(opModeChoice.klass);
+
+ // Inform the main thread of the choice and save the preference:
+ WilyCore.opModeNotification("onOpModePreInit", opMode);
+ WilyCore.status = new WilyCore.Status(WilyCore.State.INITIALIZED, opMode, button);
WilyCore.startTime = 0;
dropDown.setMaximumSize(new Dimension(0, 0));
dropDown.setVisible(false); // Needed for long opMode names, for whatever reason
@@ -162,15 +165,21 @@ public void windowClosing(WindowEvent e) {
label.setText(opModeName);
break;
- case INITIALIZED:
- WilyCore.status = new WilyCore.Status(WilyCore.State.STARTED, WilyCore.status.klass, button);
+ case INITIALIZED: // It was in INITIALIZED state, now transition to STARTED
+ WilyCore.opModeNotification("onOpModePreStart", WilyCore.status.opMode);
+ WilyCore.status = new WilyCore.Status(WilyCore.State.STARTED, null, button);
WilyCore.startTime = WilyCore.wallClockTime();
button.setText("Stop");
break;
- case STARTED:
- WilyCore.opModeThread.interrupt();
+ case STARTED: // It was in STARTED state, now transition to STOPPED
+ // I used to call WilyCore.opModeThread.interrupt(); here, but that prevented
+ // any user code from running after the Stop button was pressed. But maybe
+ // that's how it works on the robot, too?
+ // WilyCore.opModeThread.interrupt();
WilyCore.status = new WilyCore.Status(WilyCore.State.STOPPED, null, null);
+ WilyCore.opModeNotification("onOpModePostStop", WilyCore.status.opMode);
+ WilyCore.terminateOpMode();
button.setText("Init");
dropDown.setMaximumSize(new Dimension(400, 100));
dropDown.setVisible(true);
@@ -262,6 +271,7 @@ public class WilyCore {
public static Gamepad gamepad1;
public static Gamepad gamepad2;
public static HardwareMap hardwareMap;
+ public static MechSim mechSim;
public static InputManager inputManager;
public static DashboardWindow dashboardWindow;
public static Telemetry telemetry;
@@ -291,11 +301,11 @@ public static double time() {
*/
public enum State { STOPPED, INITIALIZED, STARTED }
public static class Status {
- public Class klass;
+ public OpMode opMode;
public State state;
public JButton stopButton;
- public Status(State state, Class klass, JButton button) {
- this.state = state; this.klass = klass; this.stopButton = button;
+ public Status(State state, OpMode opMode, JButton button) {
+ this.state = state; this.opMode = opMode; this.stopButton = button;
}
}
@@ -318,7 +328,7 @@ static public void render(boolean startScreenOverlay) {
dashboardWindow.errorLabel.setText(simulation.getErrorLabel());
}
- // Advance the time:
+ // Advance the time. Zero means to use the realtime clock.
static double advanceTime(double deltaT) {
if (deltaT <= 0) {
deltaT = nanoTime() * 1e-9 - lastUpdateWallClockTime;
@@ -339,18 +349,6 @@ static double advanceTime(double deltaT) {
// Callbacks provided to the guest. These are all called via reflection from the WilyWorks
// class.
- // The guest can specify the delta-t (which is handy when single stepping):
- static public void updateSimulation(double deltaT) {
- // Advance the time then advance the simulation:
- deltaT = advanceTime(deltaT);
- simulation.advance(deltaT);
-
- // Render everything:
- render();
-
- simulationUpdated = true;
- }
-
// Set the robot to a given pose and (optional) velocity in the simulation. The
// localizer will not register a move.
static public void setStartPose(Pose2d pose, PoseVelocity2d velocity) {
@@ -358,6 +356,17 @@ static public void setStartPose(Pose2d pose, PoseVelocity2d velocity) {
simulation.setStartPose(pose, velocity);
}
+ // The guest can specify the delta-t (which is handy when single stepping). Zero means to
+ // use the real-time clock.
+ static public void updateSimulation(double deltaT) {
+ // Advance the time then advance the simulation:
+ deltaT = advanceTime(deltaT);
+ simulation.advance(deltaT);
+ mechSim.advance(deltaT);
+ simulationUpdated = true;
+ render();
+ }
+
// MecanumDrive uses this while running a trajectory to update the simulator as to its
// current intermediate pose and velocity. This update will be reflected in the localizer
// results.
@@ -365,6 +374,7 @@ static public void runTo(Pose2d pose, PoseVelocity2d velocity) {
// If the user didn't explicitly call the simulation update() API, do it now:
double deltaT = advanceTime(0);
simulation.runTo(deltaT, pose, velocity);
+ mechSim.advance(deltaT);
simulationUpdated = true;
render();
}
@@ -479,25 +489,34 @@ static WilyWorks.Config getConfig(Class configKlass) {
return new WilyWorks.Config();
}
- // Call from the window manager to invoke the user's chosen "runOpMode" method:
- static void runOpMode(Class klass) {
+ // Create the opMode from the user's choice of class.
+ static OpMode createOpMode(Class opModeClass) {
OpMode opMode;
try {
//noinspection deprecation
- opMode = (OpMode) klass.newInstance();
- } catch (InstantiationException|IllegalAccessException e) {
+ opMode = (OpMode) opModeClass.newInstance();
+ } catch (InstantiationException | IllegalAccessException e) {
throw new RuntimeException(e);
}
+ // Before we initialize the HardwareMap, create this year's game simulation to go with
+ // the new opMode. It may override some of the HardwareMap.
+ mechSim = MechSim.create();
+
// We need to re-instantiate hardware map on every run:
hardwareMap = new HardwareMap();
-
opMode.hardwareMap = hardwareMap;
opMode.gamepad1 = gamepad1;
opMode.gamepad2 = gamepad2;
opMode.telemetry = telemetry;
- if (LinearOpMode.class.isAssignableFrom(klass)) {
+ return opMode;
+ }
+
+ // Call from the window manager to invoke the user's chosen "runOpMode" method. Instantiates
+ // the user's chosen opMode and invokes it.
+ static void runOpMode(OpMode opMode) {
+ if (LinearOpMode.class.isAssignableFrom(opMode.getClass())) {
LinearOpMode linearOpMode = (LinearOpMode) opMode;
try {
linearOpMode.runOpMode();
@@ -527,15 +546,15 @@ static void runOpMode(Class klass) {
// Thread dedicated to running the user's opMode:
static class OpModeThread extends Thread {
- Class opModeClass;
- OpModeThread(Class opModeClass) {
- this.opModeClass = opModeClass;
+ OpMode opMode;
+ OpModeThread(OpMode opMode) {
+ this.opMode = opMode;
setName("Wily OpMode thread");
start();
}
@Override
public void run() {
- WilyCore.runOpMode(status.klass);
+ WilyCore.runOpMode(opMode);
}
}
@@ -556,6 +575,30 @@ public static Image getImage(String imagePath, int width, int height) {
return image;
}
+ // Do some cleanup when an OpMode is terminated:
+ public static void terminateOpMode() {
+ telemetry.clearAll();
+ telemetry.log().clear();
+ }
+
+ // Call Sidekick Core's "onOpModePreInit", "onOpModePreStart", "onOpModePostStop" notifications.
+ public static void opModeNotification(String method, OpMode opMode) {
+ Class sidekickCore;
+ try {
+ sidekickCore = getSystemClassLoader().loadClass("com.loonybot.sidekick.Sidekick");
+ } catch (ClassNotFoundException e) {
+ return;
+ }
+ try {
+ Method getInstance = sidekickCore.getMethod("getWilyWorksInstance");
+ Object core = getInstance.invoke(null);
+ Method notification = sidekickCore.getMethod(method, OpMode.class);
+ notification.invoke(core, opMode);
+ } catch (IllegalAccessException | InvocationTargetException | NoSuchMethodException e) {
+ throw new RuntimeException(e);
+ }
+ }
+
////////////////////////////////////////////////////////////////////////////////////////////////
// This is the application entry point that starts up all of Wily Works!
public static void main(String[] args)
@@ -593,7 +636,7 @@ public static void main(String[] args)
// noinspection InfiniteLoopStatement
while (true) {
// Wait for the DashboardWindow UI to tell us what opMode to run:
- while (status.state == State.STOPPED) {
+ while ((status.state == State.STOPPED) || (status.opMode == null)) {
try {
//noinspection BusyWait
sleep(30);
@@ -605,7 +648,7 @@ public static void main(String[] args)
// The user has selected an opMode and pressed Init! Run the opMode on a dedicated
// thread so that it can be interrupted as necessary:
simulation.totalDistance = 0;
- opModeThread = new OpModeThread(status.klass);
+ opModeThread = new OpModeThread(status.opMode);
try {
// Wait for the opMode thread to complete:
opModeThread.join();
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java
index 5beddcc7c733..ecf9230946e6 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/Field.java
@@ -6,9 +6,6 @@
import android.annotation.SuppressLint;
import com.acmerobotics.dashboard.FtcDashboard;
-import com.acmerobotics.roadrunner.Pose2d;
-import com.qualcomm.robotcore.hardware.HardwareMap;
-import com.qualcomm.robotcore.hardware.LED;
import com.wilyworks.common.WilyWorks;
import com.wilyworks.simulator.WilyCore;
import com.wilyworks.simulator.helpers.Point;
@@ -19,19 +16,14 @@
import java.awt.GraphicsConfiguration;
import java.awt.GraphicsEnvironment;
import java.awt.Image;
-import java.awt.Polygon;
import java.awt.Rectangle;
import java.awt.RenderingHints;
-import java.awt.Shape;
import java.awt.Transparency;
import java.awt.geom.AffineTransform;
-import java.awt.geom.Ellipse2D;
import java.awt.geom.GeneralPath;
-import java.awt.geom.Rectangle2D;
import java.awt.image.BufferedImage;
import java.io.IOException;
import java.io.InputStream;
-import java.util.ArrayList;
import javax.imageio.ImageIO;
@@ -64,7 +56,7 @@ public Field(Simulation simulation) {
InputStream compassStream = classLoader.getResourceAsStream(
"background/misc/compass-rose-white-text.png");
InputStream fieldStream = classLoader.getResourceAsStream(
- "background/season-2024-intothedeep/field-2024-juice-dark.png");
+ "background/season-2025-decode/field-2025-juice-dark.png");
try {
if (compassStream != null) {
@@ -133,38 +125,9 @@ private void initializeRobotImage() {
round(ROBOT_IMAGE_HEIGHT * DIRECTION_LINE_HEIGHT));
}
- // Render the outline of the robot's true position:
- void renderRobot(Graphics2D g) {
- Pose2d pose = simulation.getPose(0, true);
- AffineTransform oldTransform = g.getTransform();
- g.translate(pose.position.x, pose.position.y);
- g.rotate(pose.heading.log());
- g.setColor(Color.RED);
- setAlpha(g, 0.5);
-
- g.draw(new Rectangle2D.Double(
- -WilyCore.config.robotWidth / 2.0, -WilyCore.config.robotLength / 2.0,
- WilyCore.config.robotWidth, WilyCore.config.robotLength));
-
- // Restore the graphics state:
- setAlpha(g, 1.0);
- g.setTransform(oldTransform);
-
-// Pose2d simulationPose = simulation.getPose(0, true);
-// AffineTransform imageTransform = new AffineTransform();
-// imageTransform.translate(simulationPose.position.x, simulationPose.position.y);
-// imageTransform.scale(1.0 / ROBOT_IMAGE_WIDTH,1.0 / ROBOT_IMAGE_HEIGHT);
-// imageTransform.rotate(simulationPose.heading.log() + Math.toRadians(90));
-// imageTransform.scale(WilyCore.config.robotWidth, WilyCore.config.robotLength);
-// imageTransform.translate(-ROBOT_IMAGE_HEIGHT / 2.0, -ROBOT_IMAGE_HEIGHT / 2.0);
-// setAlpha(g, 0.5);
-// g.drawImage(robotImage, imageTransform, null);
-// setAlpha(g, 1.0);
- }
-
// Set the transform to use inches and have the origin at the center of field. This
// returns the current transform to restore via Graphics2D.setTransform() once done:
- public static AffineTransform setFieldTransform(Graphics2D g) {
+ public static AffineTransform setFieldViewportAndClip(Graphics2D g) {
// Prime the viewport/transform and the clipping for field and overlay rendering:
AffineTransform oldTransform = g.getTransform();
g.setClip(FIELD_VIEW.x, FIELD_VIEW.y, FIELD_VIEW.width, FIELD_VIEW.height);
@@ -187,55 +150,6 @@ public static AffineTransform setPageFrameTransform(Graphics2D g) {
return oldTransform;
}
- // Render the LED for REV Digital LEDs:
- void renderLeds(Graphics2D g) {
- HardwareMap hardwareMap = WilyCore.hardwareMap;
- if (hardwareMap == null)
- return; // Might not have been created yet
-
- final int[] colors = { 0, 0xff0000, 0x00ff00, 0xffbf00 }; // black, red, green, amber
- final double radius = 2.0; // Circle radius, in inches
- Pose2d pose = simulation.getPose(0, true);
-
- ArrayList ledArray = new ArrayList<>();
- for (LED led: hardwareMap.led) {
- ledArray.add((WilyLED) led);
- }
- for (int i = 0; i < ledArray.size(); i++) {
- WilyLED led = ledArray.get(i);
- int colorIndex = 0;
- colorIndex |= (led.isRed && led.enable) ? 1 : 0;
- colorIndex |= (!led.isRed && led.enable) ? 2 : 0;
-
- // The LED actually needs two digital channels to describe all 4 possible colors.
- // Assume that consecutively registered channels make a pair:
- if (i + 1 < ledArray.size()) {
- WilyLED nextLed = ledArray.get(i + 1);
- if ((nextLed.x == led.x) &&
- (nextLed.y == led.y) &&
- (nextLed.isRed == !led.isRed)) {
-
- colorIndex |= (nextLed.isRed && nextLed.enable) ? 1 : 0;
- colorIndex |= (!nextLed.isRed && nextLed.enable) ? 2 : 0;
- i++;
- }
- }
-
- // Draw the circle at the location of the sensor on the robot, accounting for its
- // current heading:
- Point point = new Point(led.x, led.y)
- .rotate(pose.heading.log())
- .add(new Point(pose.position));
- g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
-
- g.setColor(new Color(0xffffff));
- g.fill(new Ellipse2D.Double(point.x - radius - 0.5, point.y - radius - 0.5,2 * radius + 1, 2 * radius + 1));
- g.setColor(new Color(colors[colorIndex]));
- g.fill(new Ellipse2D.Double(point.x - radius, point.y - radius,2 * radius, 2 * radius));
- g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_OFF);
- }
- }
-
// Render the field, the robot, and the field overlay:
@SuppressLint("DefaultLocale")
public void render(Graphics2D g, String caption) {
@@ -246,13 +160,15 @@ public void render(Graphics2D g, String caption) {
// Lay down the background image without needing a transform:
g.drawImage(backgroundImage, FIELD_VIEW.x + FIELD_INSET, FIELD_VIEW.y + FIELD_INSET, null);
- defaultTransform = g.getTransform();
- AffineTransform oldTransform = setFieldTransform(g);
- if (FtcDashboard.fieldOverlay != null)
+ defaultTransform = setFieldViewportAndClip(g);
+ if (FtcDashboard.fieldOverlay != null) // Can't remember why this could be null
FtcDashboard.fieldOverlay.render(g);
- renderRobot(g);
- renderLeds(g);
- g.setTransform(oldTransform);
+
+ // Render the robot via the appropriate MechSim:
+ if (WilyCore.mechSim != null)
+ WilyCore.mechSim.update(g, simulation.getPose(0, true));
+
+ g.setTransform(defaultTransform);
}
// Set the global alpha in the range [0.0, 1.0]:
@@ -282,7 +198,7 @@ void renderFieldOfView(Graphics2D g, Point origin, double orientation, double fo
public void renderStartScreenOverlay(Graphics2D g) {
g.drawImage(compassImage, 20, 20, null);
- AffineTransform oldTransform = setFieldTransform(g);
+ AffineTransform oldTransform = setFieldViewportAndClip(g);
for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) {
g.setColor(new Color(0xffffff));
renderFieldOfView(g, new Point(camera.x, camera.y), camera.orientation, camera.fieldOfView);
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java
index c3d6d9fac007..ab675e0734cb 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/InputManager.java
@@ -195,12 +195,17 @@ class GamepadInput {
GLFWGamepadState gamepadState; // Gamepad input object, null if no controller is plugged in
GamepadInput() {
- if (!GLFW.glfwInit()) {
- System.out.println("Failed to initialize GLFW!");
- return;
- }
- if (GLFW.glfwJoystickIsGamepad(GLFW.GLFW_JOYSTICK_1)) {
- gamepadState = GLFWGamepadState.create();
+ // Don't attempt to do gamepads on Macs because of -XstartOnFirstThread is required
+ // on Macs and that magical incantation isn't working:
+ String osName = System.getProperty("os.name").toLowerCase();
+ if (!osName.contains("mac")) {
+ if (!GLFW.glfwInit()) {
+ System.out.println("Failed to initialize GLFW!");
+ return;
+ }
+ if (GLFW.glfwJoystickIsGamepad(GLFW.GLFW_JOYSTICK_1)) {
+ gamepadState = GLFWGamepadState.create();
+ }
}
}
@@ -348,6 +353,7 @@ void update(Gamepad gamepad, int gamepadId) {
gamepad.right_trigger = getAxis(gamepadId, SDL.SDL_CONTROLLER_AXIS_TRIGGERRIGHT);
gamepad.updateButtonAliases();
+ gamepad.updateEdgeDetection();
}
// Get a string describing which gamepads the inputs correspond to.
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java
new file mode 100644
index 000000000000..f5a9aea48b89
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyAnalogInput.java
@@ -0,0 +1,18 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.AnalogInput;
+
+/**
+ * Wily Works AnalogInput implementation.
+ */
+public class WilyAnalogInput extends AnalogInput {
+ @Override
+ public double getVoltage() {
+ return 0;
+ }
+
+ @Override
+ public double getMaxVoltage() {
+ return 0;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java
new file mode 100644
index 000000000000..c53e7c3b55db
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyCRServo.java
@@ -0,0 +1,42 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.ServoController;
+
+/**
+ * Wily Works CRServo implementation.
+ */
+public class WilyCRServo extends WilyHardwareDevice implements CRServo {
+ double power;
+ Direction direction;
+
+ @Override
+ public ServoController getController() {
+ return new WilyServoController();
+ }
+
+ @Override
+ public int getPortNumber() {
+ return 0;
+ }
+
+ @Override
+ public void setDirection(Direction direction) {
+ this.direction = direction;
+ }
+
+ @Override
+ public Direction getDirection() {
+ return direction;
+ }
+
+ @Override
+ public void setPower(double power) {
+ this.power = power;
+ }
+
+ @Override
+ public double getPower() {
+ return power;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java
new file mode 100644
index 000000000000..8faab7df9acc
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyColorSensor.java
@@ -0,0 +1,47 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.ColorSensor;
+import com.qualcomm.robotcore.hardware.I2cAddr;
+
+/**
+ * Wily Works color sensor implementation.
+ */
+public class WilyColorSensor extends WilyHardwareDevice implements ColorSensor {
+ @Override
+ public int red() {
+ return 0;
+ }
+
+ @Override
+ public int green() {
+ return 0;
+ }
+
+ @Override
+ public int blue() {
+ return 0;
+ }
+
+ @Override
+ public int alpha() {
+ return 0;
+ }
+
+ @Override
+ public int argb() {
+ return 0;
+ }
+
+ @Override
+ public void enableLed(boolean enable) {
+ }
+
+ @Override
+ public void setI2cAddress(I2cAddr newAddress) {
+ }
+
+ @Override
+ public I2cAddr getI2cAddress() {
+ return null;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java
index 2597af4657ba..8000e43ffa1e 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDcMotorEx.java
@@ -11,6 +11,10 @@
* Wily Works DcMotorEx implementation.
*/
public class WilyDcMotorEx extends WilyHardwareDevice implements DcMotorEx {
+ RunMode mode;
+ double velocity;
+ double power;
+ Direction direction;
@Override
public DcMotorController getController() {
@@ -40,6 +44,7 @@ public boolean getPowerFloat() {
@Override
public void setTargetPosition(int position) {
+
}
@Override
@@ -59,12 +64,12 @@ public int getCurrentPosition() {
@Override
public void setMode(RunMode mode) {
-
+ this.mode = mode;
}
@Override
public RunMode getMode() {
- return null;
+ return mode;
}
@Override
@@ -84,7 +89,7 @@ public boolean isMotorEnabled() {
@Override
public void setVelocity(double angularRate) {
-
+ velocity = angularRate;
}
@Override
@@ -94,7 +99,7 @@ public void setVelocity(double angularRate, AngleUnit unit) {
@Override
public double getVelocity() {
- return 0;
+ return velocity;
}
@Override
@@ -143,18 +148,20 @@ public boolean isOverCurrent() {
}
@Override
- public void setDirection(Direction direction) { }
+ public void setDirection(Direction direction) {
+ this.direction = direction;
+ }
@Override
public Direction getDirection() {
- return null;
+ return direction;
}
@Override
- public void setPower(double power) { }
+ public void setPower(double power) { this.power = power; }
@Override
public double getPower() {
- return 0;
+ return power;
}
}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java
new file mode 100644
index 000000000000..13c4752bb0a4
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDigitalChannel.java
@@ -0,0 +1,29 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+
+/**
+ * Wily Works DigitalChannel implementation.
+ */
+public class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel {
+ boolean state;
+
+ @Override
+ public Mode getMode() {
+ return null;
+ }
+
+ @Override
+ public void setMode(Mode mode) {
+ }
+
+ @Override
+ public boolean getState() {
+ return state;
+ }
+
+ @Override
+ public void setState(boolean state) {
+ this.state = state;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java
new file mode 100644
index 000000000000..05471c831737
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyDistanceSensor.java
@@ -0,0 +1,15 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * Wily Works distance sensor implementation.
+ */
+public class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor {
+ @Override
+ public double getDistance(DistanceUnit unit) {
+ return unit.fromMm(65535);
+ } // Distance when not responding
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java
index e8776118bb18..429961f96acb 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyGamepad.java
@@ -1,15 +1,146 @@
package com.wilyworks.simulator.framework;
-import com.qualcomm.robotcore.util.Range;
+import com.qualcomm.robotcore.hardware.Gamepad;
+
+import org.firstinspires.ftc.robotcore.internal.ui.GamepadUser;
import java.util.ArrayList;
+/**
+ * Monitors changes to buttons on a Gamepad.
+ *
+ * wasPressed() will return true if the button was pressed since the last call
+ * of that method.
+ *
+ * wasReleased() will return ture if the button was released since the last call
+ * of that method.
+ *
+ * @see Gamepad
+ */
+class GamepadStateChanges {
+ protected ButtonStateMonitor dpadDown = new ButtonStateMonitor();
+ protected ButtonStateMonitor dpadLeft = new ButtonStateMonitor();
+ protected ButtonStateMonitor dpadRight = new ButtonStateMonitor();
+ protected ButtonStateMonitor a = new ButtonStateMonitor();
+ protected ButtonStateMonitor dpadUp = new ButtonStateMonitor();
+ protected ButtonStateMonitor b = new ButtonStateMonitor();
+ protected ButtonStateMonitor x = new ButtonStateMonitor();
+ protected ButtonStateMonitor y = new ButtonStateMonitor();
+ protected ButtonStateMonitor guide = new ButtonStateMonitor();
+ protected ButtonStateMonitor start = new ButtonStateMonitor();
+ protected ButtonStateMonitor back = new ButtonStateMonitor();
+ protected ButtonStateMonitor leftBumper = new ButtonStateMonitor();
+ protected ButtonStateMonitor rightBumper = new ButtonStateMonitor();
+ protected ButtonStateMonitor leftStickButton = new ButtonStateMonitor();
+ protected ButtonStateMonitor rightStickButton = new ButtonStateMonitor();
+ protected ButtonStateMonitor circle = new ButtonStateMonitor();
+ protected ButtonStateMonitor cross = new ButtonStateMonitor();
+ protected ButtonStateMonitor triangle = new ButtonStateMonitor();
+ protected ButtonStateMonitor square = new ButtonStateMonitor();
+ protected ButtonStateMonitor share = new ButtonStateMonitor();
+ protected ButtonStateMonitor options = new ButtonStateMonitor();
+ protected ButtonStateMonitor touchpad = new ButtonStateMonitor();
+ protected ButtonStateMonitor ps = new ButtonStateMonitor();
+
+ protected class ButtonStateMonitor {
+ private boolean lastPressed = false;
+ private boolean pressNotify = false;
+ private boolean releaseNotify = false;
+
+ private void update(boolean nowPressed) {
+ if (!lastPressed && nowPressed) {
+ pressNotify = true;
+ }
+ if (lastPressed && !nowPressed) {
+ releaseNotify = true;
+ }
+ lastPressed = nowPressed;
+ }
+
+ protected boolean wasPressed() {
+ boolean pressed = pressNotify;
+ pressNotify = false;
+ return pressed;
+ }
+
+ protected boolean wasReleased() {
+ boolean released = releaseNotify;
+ releaseNotify = false;
+ return released;
+ }
+ }
+
+ protected void updateAllButtons(WilyGamepad gamepad) {
+ dpadUp.update(gamepad.dpad_up);
+ dpadDown.update(gamepad.dpad_down);
+ dpadLeft.update(gamepad.dpad_left);
+ dpadRight.update(gamepad.dpad_right);
+ a.update(gamepad.a);
+ b.update(gamepad.b);
+ x.update(gamepad.x);
+ y.update(gamepad.y);
+ guide.update(gamepad.guide);
+ start.update(gamepad.start);
+ back.update(gamepad.back);
+ leftBumper.update(gamepad.left_bumper);
+ rightBumper.update(gamepad.right_bumper);
+ leftStickButton.update(gamepad.left_stick_button);
+ rightStickButton.update(gamepad.right_stick_button);
+ circle.update(gamepad.circle);
+ cross.update(gamepad.cross);
+ triangle.update(gamepad.triangle);
+ square.update(gamepad.square);
+ share.update(gamepad.share);
+ options.update(gamepad.options);
+ touchpad.update(gamepad.touchpad);
+ ps.update(gamepad.ps);
+ }
+}
+
/**
* Wily Works Gamepad implementation that takes input either from a connected gamepad or
* from the keyboard.
*/
public class WilyGamepad {
+
+ public enum Type {
+ // Do NOT change the order/names of existing entries,
+ // you will break backwards compatibility!!
+ UNKNOWN(LegacyType.UNKNOWN),
+ LOGITECH_F310(LegacyType.LOGITECH_F310),
+ XBOX_360(LegacyType.XBOX_360),
+ SONY_PS4(LegacyType.SONY_PS4), // This indicates a PS4-compatible controller that is being used through our compatibility mode
+ SONY_PS4_SUPPORTED_BY_KERNEL(LegacyType.SONY_PS4); // This indicates a PS4-compatible controller that is being used through the DualShock 4 Linux kernel driver.
+
+ private final LegacyType correspondingLegacyType;
+ Type(LegacyType correspondingLegacyType) {
+ this.correspondingLegacyType = correspondingLegacyType;
+ }
+ }
+
+ // LegacyType is necessary because robocol gamepad version 3 was written in a way that was not
+ // forwards-compatible, so we have to keep sending V3-compatible values.
+ public enum LegacyType {
+ // Do NOT change the order or names of existing entries, or add new entries.
+ // You will break backwards compatibility!!
+ UNKNOWN,
+ LOGITECH_F310,
+ XBOX_360,
+ SONY_PS4;
+ }
+
+ /**
+ * Get the type of gamepad as a {@link Type}. This method defaults to "UNKNOWN".
+ * @return gamepad type
+ */
+ public Type type() {
+ return type;
+ }
+
+ @SuppressWarnings("UnusedAssignment")
+ public volatile Type type = Type.XBOX_360; // IntelliJ thinks this is redundant, but it is NOT. Must be a bug in the analyzer?
+
public volatile float left_stick_x = 0f;
public volatile float left_stick_y = 0f;
public volatile float right_stick_x = 0f;
@@ -50,6 +181,11 @@ public class WilyGamepad {
public WilyGamepad() {
}
+ /**
+ * Edge detection for gamepads
+ */
+ private volatile GamepadStateChanges changes = new GamepadStateChanges();
+
public void updateButtonAliases(){
// There is no assignment for touchpad because there is no equivalent on XBOX controllers.
circle = b;
@@ -61,6 +197,10 @@ public void updateButtonAliases(){
ps = guide;
}
+ public void updateEdgeDetection(){
+ changes.updateAllButtons(this);
+ }
+
public void runRumbleEffect(RumbleEffect effect) { }
public void rumble(int durationMs) { }
public void rumble(double rumble1, double rumble2, int durationMs) { }
@@ -93,4 +233,377 @@ public RumbleEffect build() {
}
}
+ public GamepadUser getUser() {
+ return GamepadUser.ONE;
+ }
+
+ /**
+ * Checks if dpad_up was pressed since the last call of this method
+ * @return true if dpad_up was pressed since the last call of this method; otherwise false
+ */
+ public boolean dpadUpWasPressed() {
+ return changes.dpadUp.wasPressed();
+ }
+
+ /**
+ * Checks if dpad_up was released since the last call of this method
+ * @return true if dpad_up was released since the last call of this method; otherwise false
+ */
+ public boolean dpadUpWasReleased() {
+ return changes.dpadUp.wasReleased();
+ }
+
+ /**
+ * Checks if dpad_down was pressed since the last call of this method
+ * @return true if dpad_down was pressed since the last call of this method; otherwise false
+ */
+ public boolean dpadDownWasPressed() {
+ return changes.dpadDown.wasPressed();
+ }
+
+ /**
+ * Checks if dpad_down was released since the last call of this method
+ * @return true if dpad_down was released since the last call of this method; otherwise false
+ */
+ public boolean dpadDownWasReleased() {
+ return changes.dpadDown.wasReleased();
+ }
+
+ /**
+ * Checks if dpad_left was pressed since the last call of this method
+ * @return true if dpad_left was pressed since the last call of this method; otherwise false
+ */
+ public boolean dpadLeftWasPressed() {
+ return changes.dpadLeft.wasPressed();
+ }
+
+ /**
+ * Checks if dpad_left was released since the last call of this method
+ * @return true if dpad_left was released since the last call of this method; otherwise false
+ */
+ public boolean dpadLeftWasReleased() {
+ return changes.dpadLeft.wasReleased();
+ }
+
+ /**
+ * Checks if dpad_right was pressed since the last call of this method
+ * @return true if dpad_right was pressed since the last call of this method; otherwise false
+ */
+ public boolean dpadRightWasPressed() {
+ return changes.dpadRight.wasPressed();
+ }
+
+ /**
+ * Checks if dpad_right was released since the last call of this methmethodod
+ * @return true if dpad_right was released since the last call of this ; otherwise false
+ */
+ public boolean dpadRightWasReleased() {
+ return changes.dpadRight.wasReleased();
+ }
+
+ /**
+ * Checks if a was pressed since the last call of this method
+ * @return true if a was pressed since the last call of this method; otherwise false
+ */
+ public boolean aWasPressed() {
+ return changes.a.wasPressed();
+ }
+
+ /**
+ * Checks if a was released since the last call of this method
+ * @return true if a was released since the last call of this method; otherwise false
+ */
+ public boolean aWasReleased() {
+ return changes.a.wasReleased();
+ }
+
+ /**
+ * Checks if b was pressed since the last call of this method
+ * @return true if b was pressed since the last call of this method; otherwise false
+ */
+ public boolean bWasPressed() {
+ return changes.b.wasPressed();
+ }
+
+ /**
+ * Checks if b was released since the last call of this method
+ * @return true if b was released since the last call of this method; otherwise false
+ */
+ public boolean bWasReleased() {
+ return changes.b.wasReleased();
+ }
+
+ /**
+ * Checks if x was pressed since the last call of this method
+ * @return true if x was pressed since the last call of this method; otherwise false
+ */
+ public boolean xWasPressed() {
+ return changes.x.wasPressed();
+ }
+
+ /**
+ * Checks if x was released since the last call of this method
+ * @return true if x was released since the last call of this method; otherwise false
+ */
+ public boolean xWasReleased() {
+ return changes.x.wasReleased();
+ }
+
+ /**
+ * Checks if y was pressed since the last call of this method
+ * @return true if y was pressed since the last call of this method; otherwise false
+ */
+ public boolean yWasPressed() {
+ return changes.y.wasPressed();
+ }
+
+ /**
+ * Checks if y was released since the last call of this method
+ * @return true if y was released since the last call of this method; otherwise false
+ */
+ public boolean yWasReleased() {
+ return changes.y.wasReleased();
+ }
+
+ /**
+ * Checks if guide was pressed since the last call of this method
+ * @return true if guide was pressed since the last call of this method; otherwise false
+ */
+ public boolean guideWasPressed() {
+ return changes.guide.wasPressed();
+ }
+
+ /**
+ * Checks if guide was released since the last call of this method
+ * @return true if guide was released since the last call of this method; otherwise false
+ */
+ public boolean guideWasReleased() {
+ return changes.guide.wasReleased();
+ }
+
+ /**
+ * Checks if start was pressed since the last call of this method
+ * @return true if start was pressed since the last call of this method; otherwise false
+ */
+ public boolean startWasPressed() {
+ return changes.start.wasPressed();
+ }
+
+ /**
+ * Checks if start was released since the last call of this method
+ * @return true if start was released since the last call of this method; otherwise false
+ */
+ public boolean startWasReleased() {
+ return changes.start.wasReleased();
+ }
+
+ /**
+ * Checks if back was pressed since the last call of this method
+ * @return true if back was pressed since the last call of this method; otherwise false
+ */
+ public boolean backWasPressed() {
+ return changes.back.wasPressed();
+ }
+
+ /**
+ * Checks if back was released since the last call of this method
+ * @return true if back was released since the last call of this method; otherwise false
+ */
+ public boolean backWasReleased() {
+ return changes.back.wasReleased();
+ }
+
+ /**
+ * Checks if left_bumper was pressed since the last call of this method
+ * @return true if left_bumper was pressed since the last call of this method; otherwise false
+ */
+ public boolean leftBumperWasPressed() {
+ return changes.leftBumper.wasPressed();
+ }
+
+ /**
+ * Checks if left_bumper was released since the last call of this method
+ * @return true if left_bumper was released since the last call of this method; otherwise false
+ */
+ public boolean leftBumperWasReleased() {
+ return changes.leftBumper.wasReleased();
+ }
+
+ /**
+ * Checks if right_bumper was pressed since the last call of this method
+ * @return true if right_bumper was pressed since the last call of this method; otherwise false
+ */
+ public boolean rightBumperWasPressed() {
+ return changes.rightBumper.wasPressed();
+ }
+
+ /**
+ * Checks if right_bumper was released since the last call of this method
+ * @return true if right_bumper was released since the last call of this method; otherwise false
+ */
+ public boolean rightBumperWasReleased() {
+ return changes.rightBumper.wasReleased();
+ }
+
+ /**
+ * Checks if left_stick_button was pressed since the last call of this method
+ * @return true if left_stick_button was pressed since the last call of this method; otherwise false
+ */
+ public boolean leftStickButtonWasPressed() {
+ return changes.leftStickButton.wasPressed();
+ }
+
+ /**
+ * Checks if left_stick_button was released since the last call of this method
+ * @return true if left_stick_button was released since the last call of this method; otherwise false
+ */
+ public boolean leftStickButtonWasReleased() {
+ return changes.leftStickButton.wasReleased();
+ }
+
+ /**
+ * Checks if right_stick_button was pressed since the last call of this method
+ * @return true if right_stick_button was pressed since the last call of this method; otherwise false
+ */
+ public boolean rightStickButtonWasPressed() {
+ return changes.rightStickButton.wasPressed();
+ }
+
+ /**
+ * Checks if right_stick_button was released since the last call of this method
+ * @return true if right_stick_button was released since the last call of this method; otherwise false
+ */
+ public boolean rightStickButtonWasReleased() {
+ return changes.rightStickButton.wasReleased();
+ }
+
+ /**
+ * Checks if circle was pressed since the last call of this method
+ * @return true if circle was pressed since the last call of this method; otherwise false
+ */
+ public boolean circleWasPressed() {
+ return changes.circle.wasPressed();
+ }
+
+ /**
+ * Checks if circle was released since the last call of this method
+ * @return true if circle was released since the last call of this method; otherwise false
+ */
+ public boolean circleWasReleased() {
+ return changes.circle.wasReleased();
+ }
+
+ /**
+ * Checks if cross was pressed since the last call of this method
+ * @return true if cross was pressed since the last call of this method; otherwise false
+ */
+ public boolean crossWasPressed() {
+ return changes.cross.wasPressed();
+ }
+
+ /**
+ * Checks if cross was released since the last call of this method
+ * @return true if cross was released since the last call of this method; otherwise false
+ */
+ public boolean crossWasReleased() {
+ return changes.cross.wasReleased();
+ }
+
+ /**
+ * Checks if triangle was pressed since the last call of this method
+ * @return true if triangle was pressed since the last call of this method; otherwise false
+ */
+ public boolean triangleWasPressed() {
+ return changes.triangle.wasPressed();
+ }
+
+ /**
+ * Checks if triangle was released since the last call of this method
+ * @return true if triangle was released since the last call of this method; otherwise false
+ */
+ public boolean triangleWasReleased() {
+ return changes.triangle.wasReleased();
+ }
+
+ /**
+ * Checks if square was pressed since the last call of this method
+ * @return true if square was pressed since the last call of this method; otherwise false
+ */
+ public boolean squareWasPressed() {
+ return changes.square.wasPressed();
+ }
+
+ /**
+ * Checks if square was released since the last call of this method
+ * @return true if square was released since the last call of this method; otherwise false
+ */
+ public boolean squareWasReleased() {
+ return changes.square.wasReleased();
+ }
+
+ /**
+ * Checks if share was pressed since the last call of this method
+ * @return true if share was pressed since the last call of this method; otherwise false
+ */
+ public boolean shareWasPressed() {
+ return changes.share.wasPressed();
+ }
+
+ /**
+ * Checks if share was released since the last call of this method
+ * @return true if share was released since the last call of this method; otherwise false
+ */
+ public boolean shareWasReleased() {
+ return changes.share.wasReleased();
+ }
+
+ /**
+ * Checks if options was pressed since the last call of this method
+ * @return true if options was pressed since the last call of this method; otherwise false
+ */
+ public boolean optionsWasPressed() {
+ return changes.options.wasPressed();
+ }
+
+ /**
+ * Checks if options was released since the last call of this method
+ * @return true if options was released since the last call of this method; otherwise false
+ */
+ public boolean optionsWasReleased() {
+ return changes.options.wasReleased();
+ }
+
+ /**
+ * Checks if touchpad was pressed since the last call of this method
+ * @return true if touchpad was pressed since the last call of this method; otherwise false
+ */
+ public boolean touchpadWasPressed() {
+ return changes.touchpad.wasPressed();
+ }
+
+ /**
+ * Checks if touchpad was released since the last call of this method
+ * @return true if touchpad was released since the last call of this method; otherwise false
+ */
+ public boolean touchpadWasReleased() {
+ return changes.touchpad.wasReleased();
+ }
+
+ /**
+ * Checks if ps was pressed since the last call of this method
+ * @return true if ps was pressed since the last call of this method; otherwise false
+ */
+ public boolean psWasPressed() {
+ return changes.ps.wasPressed();
+ }
+
+ /**
+ * Checks if ps was released since the last call of this method
+ * @return true if ps was released since the last call of this method; otherwise false
+ */
+ public boolean psWasReleased() {
+ return changes.ps.wasReleased();
+ }
+
+
}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java
index e53f8fa883dd..42aab53a9722 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareDevice.java
@@ -13,9 +13,7 @@ public Manufacturer getManufacturer() {
}
@Override
- public String getDeviceName() {
- return "";
- }
+ public String getDeviceName() { return ""; }
@Override
public String getConnectionInfo() {
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java
index 053526501945..64bc601d1bf5 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyHardwareMap.java
@@ -6,37 +6,27 @@
import androidx.annotation.NonNull;
import androidx.annotation.Nullable;
+import com.qualcomm.hardware.gobilda.GoBildaPinpointDriver;
+import com.qualcomm.hardware.limelightvision.Limelight3A;
+import com.qualcomm.hardware.sparkfun.SparkFunLEDStick;
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
+import com.qualcomm.robotcore.eventloop.opmode.OpModeManagerNotifier;
+import com.qualcomm.robotcore.hardware.AnalogInput;
import com.qualcomm.robotcore.hardware.CRServo;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.DistanceSensor;
import com.qualcomm.robotcore.hardware.HardwareDevice;
-import com.qualcomm.robotcore.hardware.I2cAddr;
import com.qualcomm.robotcore.hardware.IMU;
import com.qualcomm.robotcore.hardware.LED;
import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
-import com.qualcomm.robotcore.hardware.NormalizedRGBA;
import com.qualcomm.robotcore.hardware.Servo;
-import com.qualcomm.robotcore.hardware.ServoController;
import com.qualcomm.robotcore.hardware.VoltageSensor;
import com.qualcomm.robotcore.util.SerialNumber;
-import com.wilyworks.common.WilyWorks;
import com.wilyworks.simulator.WilyCore;
-import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics;
import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
-import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
-import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
-import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
-import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
-import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
-import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
-import org.firstinspires.ftc.robotcore.internal.system.Deadline;
-import org.swerverobotics.ftc.GoBildaPinpointDriver;
import org.swerverobotics.ftc.UltrasonicDistanceSensor;
import java.util.ArrayList;
@@ -49,345 +39,6 @@
import java.util.Set;
import java.util.SortedSet;
import java.util.TreeSet;
-import java.util.function.Consumer;
-
-import kotlin.coroutines.Continuation;
-
-/**
- * Wily Works simulated IMU implementation.
- */
-class WilyIMU extends WilyHardwareDevice implements IMU {
- double startYaw;
- @Override
- public boolean initialize(Parameters parameters) {
- resetYaw();
- return true;
- }
-
- @Override
- public void resetYaw() {
- startYaw = WilyWorks.getPose().heading.log();
- }
-
- @Override
- public YawPitchRollAngles getRobotYawPitchRollAngles() {
- return new YawPitchRollAngles(
- AngleUnit.RADIANS,
- WilyWorks.getPose().heading.log() - startYaw,
- 0,
- 0,
- 0);
- }
-
- @Override
- public Orientation getRobotOrientation(AxesReference reference, AxesOrder order, AngleUnit angleUnit) {
- return new Orientation();
- }
-
- @Override
- public Quaternion getRobotOrientationAsQuaternion() {
- return new Quaternion();
- }
-
- @Override
- public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) {
- return new AngularVelocity(
- angleUnit,
- (float) WilyWorks.getPoseVelocity().angVel, // ### transformedAngularVelocityVector.get(0),
- 0, // ### transformedAngularVelocityVector.get(1),
- 0, // ### transformedAngularVelocityVector.get(2),
- 0); // ### rawAngularVelocity.acquisitionTime);
- }
-}
-
-/**
- * Wily Works voltage sensor implementation.
- */
-class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor {
- @Override
- public double getVoltage() {
- return 13.0;
- }
-
- @Override
- public String getDeviceName() {
- return "Voltage Sensor";
- }
-}
-
-/**
- * Wily Works distance sensor implementation.
- */
-class WilyDistanceSensor extends WilyHardwareDevice implements DistanceSensor {
- @Override
- public double getDistance(DistanceUnit unit) { return unit.fromMm(65535); } // Distance when not responding
-}
-
-/**
- * Wily Works normalized color sensor implementation.
- */
-class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor {
- @Override
- public NormalizedRGBA getNormalizedColors() { return new NormalizedRGBA(); }
-
- @Override
- public float getGain() { return 0; }
-
- @Override
- public void setGain(float newGain) { }
-}
-
-/**
- * Wily Works color sensor implementation.
- */
-class WilyColorSensor extends WilyHardwareDevice implements ColorSensor {
-
- @Override
- public int red() { return 0; }
-
- @Override
- public int green() { return 0; }
-
- @Override
- public int blue() { return 0; }
-
- @Override
- public int alpha() { return 0; }
-
- @Override
- public int argb() { return 0; }
-
- @Override
- public void enableLed(boolean enable) { }
-
- @Override
- public void setI2cAddress(I2cAddr newAddress) { }
-
- @Override
- public I2cAddr getI2cAddress() { return null; }
-}
-
-/**
- * Wily Works named webcam implementation.
- */
-class WilyWebcam extends WilyHardwareDevice implements WebcamName {
- WilyWorks.Config.Camera wilyCamera;
-
- WilyWebcam(String deviceName) {
- for (WilyWorks.Config.Camera camera: WilyCore.config.cameras) {
- if (camera.name.equals(deviceName)) {
- wilyCamera = camera;
- }
- }
- if (wilyCamera == null) {
- System.out.printf("WilyWorks: Couldn't find configuration data for camera '%s'", deviceName);
- }
- }
-
- @Override
- public boolean isWebcam() {
- return true;
- }
-
- @Override
- public boolean isCameraDirection() {
- return false;
- }
-
- @Override
- public boolean isSwitchable() {
- return false;
- }
-
- @Override
- public boolean isUnknown() {
- return false;
- }
-
- @Override
- public void asyncRequestCameraPermission(Context context, Deadline deadline, Continuation> continuation) {
-
- }
-
- @Override
- public boolean requestCameraPermission(Deadline deadline) {
- return false;
- }
-
- @Override
- public CameraCharacteristics getCameraCharacteristics() {
- return null;
- }
-
- @Override
- public SerialNumber getSerialNumber() {
- return null;
- }
-
- @Nullable
- @Override
- public String getUsbDeviceNameIfAttached() {
- return null;
- }
-
- @Override
- public boolean isAttached() {
- return false;
- }
-}
-
-/**
- * Wily Works ServoController implementation.
- */
-class WilyServoController extends WilyHardwareDevice implements ServoController {
- @Override
- public void pwmEnable() { }
-
- @Override
- public void pwmDisable() { }
-
- @Override
- public PwmStatus getPwmStatus() { return PwmStatus.DISABLED; }
-
- @Override
- public void setServoPosition(int servo, double position) { }
-
- @Override
- public double getServoPosition(int servo) { return 0; }
-
- @Override
- public void close() { }
-}
-
-/**
- * Wily Works Servo implementation.
- */
-class WilyServo extends WilyHardwareDevice implements Servo {
- double position;
-
- @Override
- public ServoController getController() {
- return new WilyServoController();
- }
-
- @Override
- public int getPortNumber() {
- return 0;
- }
-
- @Override
- public void setDirection(Direction direction) {
-
- }
-
- @Override
- public Direction getDirection() {
- return null;
- }
-
- @Override
- public void setPosition(double position) { this.position = Math.max(0, Math.min(1, position)); }
-
- @Override
- public double getPosition() {return position; }
-
- @Override
- public void scaleRange(double min, double max) {
-
- }
-}
-
-/**
- * Wily Works CRServo implementation.
- */
-class WilyCRServo extends WilyHardwareDevice implements CRServo {
-
- @Override
- public ServoController getController() {
- return new WilyServoController();
- }
-
- @Override
- public int getPortNumber() {
- return 0;
- }
-
- @Override
- public void setDirection(Direction direction) {
-
- }
-
- @Override
- public Direction getDirection() {
- return null;
- }
-
- @Override
- public void setPower(double power) {
-
- }
-
- @Override
- public double getPower() {
- return 0;
- }
-}
-
-/**
- * Wily Works DigitalChannel implementation.
- */
-class WilyDigitalChannel extends WilyHardwareDevice implements DigitalChannel {
- boolean state;
-
- @Override
- public Mode getMode() { return null; }
-
- @Override
- public void setMode(Mode mode) {}
-
- @Override
- public boolean getState() { return state; }
-
- @Override
- public void setState(boolean state) { this.state = state; }
-}
-
-/**
- * Wily Works LED implementation.
- */
-class WilyLED extends LED {
- // Assume that every digital channels is a REV LED indicator. Doesn't hurt if that's not
- // the case:
- boolean enable = true; // They're always on by default
- double x;
- double y;
- boolean isRed;
- WilyLED(String deviceName) {
- WilyWorks.Config.LEDIndicator wilyLed = null;
- for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) {
- if (led.name.equals(deviceName)) {
- wilyLed = led;
- }
- }
- if (wilyLed != null) {
- x = wilyLed.x;
- y = wilyLed.y;
- isRed = wilyLed.isRed;
- } else {
- isRed = !(deviceName.toLowerCase().contains("green"));
- }
- }
-
- @Override
- public void enableLight(boolean enable) {
- this.enable = enable;
- }
-
- @Override
- public boolean isLightOn() {
- return enable;
- }
-}
/**
* Wily Works hardware map.
@@ -407,10 +58,18 @@ public class WilyHardwareMap implements Iterable {
public DeviceMapping sparkFunOTOS = new DeviceMapping<>(SparkFunOTOS.class);
public DeviceMapping goBildaPinpointDrivers = new DeviceMapping<>(GoBildaPinpointDriver.class);
public DeviceMapping ultrasonicDistanceSensor = new DeviceMapping<>(UltrasonicDistanceSensor.class);
+ public DeviceMapping analogInput = new DeviceMapping<>(AnalogInput.class);
+ public DeviceMapping imu = new DeviceMapping<>(IMU.class);
+ public DeviceMapping limelight = new DeviceMapping<>(Limelight3A.class);
+ public DeviceMapping sparkFunLEDStick = new DeviceMapping<>(SparkFunLEDStick.class);
protected Map> allDevicesMap = new HashMap<>();
protected List allDevicesList = new ArrayList<>();
- public WilyHardwareMap() {
+ public final List> allDeviceMappings = new ArrayList<>();
+
+ public WilyHardwareMap(Context appContext, OpModeManagerNotifier notifier) {
+ // Road Runner expects this object to be already created because it references
+ // hardwareMap.voltageSensor.iterator().next() directly:
put("voltage_sensor", VoltageSensor.class);
}
@@ -436,9 +95,6 @@ public T tryGet(Class classOrInterface, String deviceName) {
return get(classOrInterface, deviceName);
}
public T get(Class classOrInterface, String deviceName) {
- if (classOrInterface.equals(IMU.class)) {
- return (T) new WilyIMU();
- }
T result = wilyTryGet(classOrInterface, deviceName);
if (result == null) {
// Wily Works behavior is that we automatically add the device if it's not found:
@@ -448,13 +104,15 @@ public T get(Class classOrInterface, String deviceName) {
return result;
}
- @Deprecated
public HardwareDevice get(String deviceName) {
- List list = allDevicesMap.get(deviceName.trim());
+ deviceName = deviceName.trim();
+ List list = allDevicesMap.get(deviceName);
if (list != null) {
- return list.get(0);
+ for (HardwareDevice device : list) {
+ initializeDeviceIfNecessary(device);
+ return device;
+ }
}
-
throw new IllegalArgumentException("Use the typed version of get(), e.g. get(DcMotorEx.class, \"leftMotor\")");
}
@@ -497,18 +155,36 @@ public synchronized void put(String deviceName, Class klass) {
} else if (LED.class.isAssignableFrom(klass)) {
device = new WilyLED(deviceName);
led.put(deviceName, (LED) device);
+ } else if (AnalogInput.class.isAssignableFrom(klass)) {
+ device = new WilyAnalogInput();
+ analogInput.put(deviceName, (WilyAnalogInput) device);
+ } else if (IMU.class.isAssignableFrom(klass)) {
+ device = new WilyIMU();
+ imu.put(deviceName, (WilyIMU) device);
+
+ // Not actually built into HardwareMap.java:
+ } else if (UltrasonicDistanceSensor.class.isAssignableFrom(klass)) {
+ device = new WilyUltrasonicDistanceSensor(deviceName);
+ ultrasonicDistanceSensor.put(deviceName, (WilyUltrasonicDistanceSensor) device);
} else if (SparkFunOTOS.class.isAssignableFrom(klass)) {
device = new SparkFunOTOS(null);
sparkFunOTOS.put(deviceName, (SparkFunOTOS) device);
} else if (GoBildaPinpointDriver.class.isAssignableFrom(klass)) {
device = new GoBildaPinpointDriver(null, false);
goBildaPinpointDrivers.put(deviceName, (GoBildaPinpointDriver) device);
- } else if (UltrasonicDistanceSensor.class.isAssignableFrom(klass)) {
- device = new WilyUltrasonicDistanceSensor(deviceName);
- ultrasonicDistanceSensor.put(deviceName, (WilyUltrasonicDistanceSensor) device);
+ } else if (Limelight3A.class.isAssignableFrom(klass)) {
+ device = new Limelight3A(null, "", null);
+ limelight.put(deviceName, (Limelight3A) device);
+ } else if (SparkFunLEDStick.class.isAssignableFrom(klass)) {
+ device = new SparkFunLEDStick(null, false);
+ sparkFunLEDStick.put(deviceName, (SparkFunLEDStick) device);
} else {
throw new IllegalArgumentException("Unexpected device type for HardwareMap");
}
+
+ // Let the game simulation change the behavior of this device:
+ device = WilyCore.mechSim.hookDevice(deviceName, device);
+
list.add(device);
allDevicesList.add(device);
}
@@ -524,7 +200,12 @@ private void initializeMultipleDevicesIfNecessary(Iterable getAllNames(Class classOrInterface) {
SortedSet result = new TreeSet<>();
- result.add("voltage_sensor");
+ for (String userName: allDevicesMap.keySet()) {
+ HardwareDevice device = allDevicesMap.get(userName).get(0);
+ if (classOrInterface.isInstance(device)) {
+ result.add(userName);
+ }
+ }
return result;
}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java
new file mode 100644
index 000000000000..b26da584b708
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyIMU.java
@@ -0,0 +1,60 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.IMU;
+import com.wilyworks.common.WilyWorks;
+
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AngularVelocity;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Quaternion;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+/**
+ * Wily Works simulated IMU implementation.
+ */
+public class WilyIMU extends WilyHardwareDevice implements IMU {
+ double startYaw;
+
+ @Override
+ public boolean initialize(Parameters parameters) {
+ resetYaw();
+ return true;
+ }
+
+ @Override
+ public void resetYaw() {
+ startYaw = WilyWorks.getPose().heading.log();
+ }
+
+ @Override
+ public YawPitchRollAngles getRobotYawPitchRollAngles() {
+ return new YawPitchRollAngles(
+ AngleUnit.RADIANS,
+ WilyWorks.getPose().heading.log() - startYaw,
+ 0,
+ 0,
+ 0);
+ }
+
+ @Override
+ public Orientation getRobotOrientation(AxesReference reference, AxesOrder order, AngleUnit angleUnit) {
+ return new Orientation();
+ }
+
+ @Override
+ public Quaternion getRobotOrientationAsQuaternion() {
+ return new Quaternion();
+ }
+
+ @Override
+ public AngularVelocity getRobotAngularVelocity(AngleUnit angleUnit) {
+ return new AngularVelocity(
+ angleUnit,
+ (float) WilyWorks.getPoseVelocity().angVel, // ### transformedAngularVelocityVector.get(0),
+ 0, // ### transformedAngularVelocityVector.get(1),
+ 0, // ### transformedAngularVelocityVector.get(2),
+ 0); // ### rawAngularVelocity.acquisitionTime);
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java
new file mode 100644
index 000000000000..1907266f4471
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyLED.java
@@ -0,0 +1,44 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.LED;
+import com.wilyworks.common.WilyWorks;
+import com.wilyworks.simulator.WilyCore;
+
+/**
+ * Wily Works LED implementation.
+ */
+public class WilyLED extends LED {
+ // Assume that every digital channels is a REV LED indicator. Doesn't hurt if that's not
+ // the case:
+ public boolean enable = true; // They're always on by default
+ public double x;
+ public double y;
+ public boolean isRed;
+ public WilyLED(String deviceName) {
+ WilyWorks.Config.LEDIndicator wilyLed = null;
+ for (WilyWorks.Config.LEDIndicator led: WilyCore.config.ledIndicators) {
+ if (led.name.equals(deviceName)) {
+ wilyLed = led;
+ }
+ }
+ if (wilyLed != null) {
+ x = wilyLed.x;
+ y = wilyLed.y;
+ isRed = wilyLed.isRed;
+ } else {
+ isRed = !(deviceName.toLowerCase().contains("green"));
+ }
+ }
+
+ @Override
+ public void enable(boolean enableLed) { this.enable = enableLed; }
+
+ @Override
+ public void enableLight(boolean enable) { enable(enable); }
+
+ @Override
+ public boolean isLightOn() {
+ return enable;
+ }
+}
+
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java
new file mode 100644
index 000000000000..f37d08b78294
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyNormalizedColorSensor.java
@@ -0,0 +1,31 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.DistanceSensor;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+/**
+ * Wily Works normalized color sensor implementation.
+ */
+public class WilyNormalizedColorSensor extends WilyHardwareDevice implements NormalizedColorSensor, DistanceSensor {
+ @Override
+ public NormalizedRGBA getNormalizedColors() {
+ return new NormalizedRGBA();
+ }
+
+ @Override
+ public float getGain() {
+ return 0;
+ }
+
+ @Override
+ public void setGain(float newGain) {
+ }
+
+ @Override
+ public double getDistance(DistanceUnit unit) {
+ return 0;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java
index f6fc6abe1994..650fd54f8440 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyOpMode.java
@@ -19,6 +19,10 @@ public final void idle() {
Thread.yield();
}
+ public final boolean opModeInInit() {
+ return !isStarted() && !isStopRequested();
+ }
+
public void waitForStart() {
WilyCore.render();
while (!isStarted())
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java
new file mode 100644
index 000000000000..44022174de2c
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServo.java
@@ -0,0 +1,46 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.Servo;
+import com.qualcomm.robotcore.hardware.ServoController;
+
+/**
+ * Wily Works Servo implementation.
+ */
+public class WilyServo extends WilyHardwareDevice implements Servo {
+ double position;
+
+ @Override
+ public ServoController getController() {
+ return new WilyServoController();
+ }
+
+ @Override
+ public int getPortNumber() {
+ return 0;
+ }
+
+ @Override
+ public void setDirection(Direction direction) {
+
+ }
+
+ @Override
+ public Direction getDirection() {
+ return null;
+ }
+
+ @Override
+ public void setPosition(double position) {
+ this.position = Math.max(0, Math.min(1, position));
+ }
+
+ @Override
+ public double getPosition() {
+ return position;
+ }
+
+ @Override
+ public void scaleRange(double min, double max) {
+
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java
new file mode 100644
index 000000000000..b6ee70013097
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyServoController.java
@@ -0,0 +1,34 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.ServoController;
+
+/**
+ * Wily Works ServoController implementation.
+ */
+public class WilyServoController extends WilyHardwareDevice implements ServoController {
+ @Override
+ public void pwmEnable() {
+ }
+
+ @Override
+ public void pwmDisable() {
+ }
+
+ @Override
+ public PwmStatus getPwmStatus() {
+ return PwmStatus.DISABLED;
+ }
+
+ @Override
+ public void setServoPosition(int servo, double position) {
+ }
+
+ @Override
+ public double getServoPosition(int servo) {
+ return 0;
+ }
+
+ @Override
+ public void close() {
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java
index 6ef7048ef057..b5aac6ef48f8 100644
--- a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyTelemetry.java
@@ -1,18 +1,17 @@
package com.wilyworks.simulator.framework;
+import static org.firstinspires.ftc.robotcore.external.Telemetry.DisplayFormat;
import static java.awt.Font.MONOSPACED;
import static java.awt.font.TextAttribute.POSTURE_REGULAR;
import static java.awt.font.TextAttribute.WEIGHT_REGULAR;
-import org.firstinspires.ftc.robotcore.external.Func;
-import org.firstinspires.ftc.robotcore.external.Telemetry;
-
-import static org.firstinspires.ftc.robotcore.external.Telemetry.DisplayFormat;
-
import androidx.annotation.Nullable;
import com.qualcomm.robotcore.robocol.TelemetryMessage;
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+
import java.awt.Canvas;
import java.awt.Color;
import java.awt.Graphics2D;
@@ -517,12 +516,66 @@ public Telemetry.Item addData(String caption, String format, Func valuePr
}
}
+class WilyLog implements Telemetry.Log {
+ LinkedList lines = new LinkedList<>();
+ DisplayOrder displayOrder = DisplayOrder.OLDEST_FIRST;
+ int capacity = 9;
+
+ @Override
+ public int getCapacity() {
+ return capacity;
+ }
+
+ @Override
+ public void setCapacity(int capacity) {
+ this.capacity = capacity;
+ }
+
+ @Override
+ public DisplayOrder getDisplayOrder() {
+ return displayOrder;
+ }
+
+ @Override
+ public void setDisplayOrder(DisplayOrder displayOrder) {
+ this.displayOrder = displayOrder;
+ }
+
+ @Override
+ public void add(String entry) {
+ lines.add(entry);
+ if (lines.size() > capacity)
+ lines.removeFirst();
+ }
+
+ @Override
+ public void add(String format, Object... args) {
+ add(String.format(format, args));
+ }
+
+ @Override
+ public void clear() {
+ lines.clear();
+ }
+
+ // This is a private function called by Telemetry.update() to add all of the lines
+ // of the log to the telemetry:
+ public void get(List telemetry) {
+ if (displayOrder == DisplayOrder.OLDEST_FIRST) {
+ telemetry.addAll(lines);
+ } else {
+ Iterator iterator = lines.descendingIterator();
+ while (iterator.hasNext()) {
+ telemetry.add(iterator.next());
+ }
+ }
+ }
+}
+
/**
* This class implements a lightweight emulation of FTC Telemetry that can run on the PC.
*/
public class WilyTelemetry implements Telemetry {
- final int MAX_LINES = 36; // It's 18 with a regular sized font
-
// Global state:
public static WilyTelemetry instance;
@@ -532,6 +585,7 @@ public class WilyTelemetry implements Telemetry {
ArrayList lineList = new ArrayList<>();
DisplayFormat displayFormat = DisplayFormat.CLASSIC; // HTML vs. monospace modes
Layout layout = new Layout();
+ WilyLog log = new WilyLog();
// Wily Works constructor for a Telemetry object:
public WilyTelemetry(java.awt.Image icon) {
@@ -544,15 +598,13 @@ public WilyTelemetry(java.awt.Image icon) {
}
public Line addLine(String string) {
- if (lineList.size() <= MAX_LINES) {
- int newLineIndex;
- while ((newLineIndex = string.indexOf("\n")) != -1) {
- String line = string.substring(0, newLineIndex);
- lineList.add(line);
- string = string.substring(newLineIndex + 1);
- }
- lineList.add(string);
+ int newLineIndex;
+ while ((newLineIndex = string.indexOf("\n")) != -1) {
+ String line = string.substring(0, newLineIndex);
+ lineList.add(line);
+ string = string.substring(newLineIndex + 1);
}
+ lineList.add(string);
return new WilyLine(this);
}
@@ -600,11 +652,12 @@ public void setDisplayFormat(DisplayFormat displayFormat) {
@Override
public Log log() {
- return null;
+ return log;
}
public Item addData(String caption, Object value) {
- addLine(String.format("%s : %s", caption, value.toString()));
+ String string = (value == null) ? "null" : value.toString();
+ addLine(String.format("%s : %s", caption, string));
return null; // ###
}
@@ -651,6 +704,7 @@ public void speak(String text, String languageCode, String countryCode) { }
public boolean update() {
Graphics2D g = (Graphics2D) canvas.getBufferStrategy().getDrawGraphics();
g.clearRect(0, 0, canvas.getWidth(), canvas.getHeight());
+ log.get(lineList); // Append the log to the line list
layout.parseAndRender(g, displayFormat, lineList);
g.dispose();
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java
new file mode 100644
index 000000000000..9d846f19b514
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyVoltageSensor.java
@@ -0,0 +1,18 @@
+package com.wilyworks.simulator.framework;
+
+import com.qualcomm.robotcore.hardware.VoltageSensor;
+
+/**
+ * Wily Works voltage sensor implementation.
+ */
+public class WilyVoltageSensor extends WilyHardwareDevice implements VoltageSensor {
+ @Override
+ public double getVoltage() {
+ return 13.0;
+ }
+
+ @Override
+ public String getDeviceName() {
+ return "Voltage Sensor";
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java
new file mode 100644
index 000000000000..8aa242fa397b
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/WilyWebcam.java
@@ -0,0 +1,86 @@
+package com.wilyworks.simulator.framework;
+
+import android.content.Context;
+
+import androidx.annotation.Nullable;
+
+import com.qualcomm.robotcore.util.SerialNumber;
+import com.wilyworks.common.WilyWorks;
+import com.wilyworks.simulator.WilyCore;
+
+import org.firstinspires.ftc.robotcore.external.hardware.camera.CameraCharacteristics;
+import org.firstinspires.ftc.robotcore.external.hardware.camera.WebcamName;
+import org.firstinspires.ftc.robotcore.internal.system.Deadline;
+
+import java.util.function.Consumer;
+
+import kotlin.coroutines.Continuation;
+
+/**
+ * Wily Works named webcam implementation.
+ */
+public class WilyWebcam extends WilyHardwareDevice implements WebcamName {
+ WilyWorks.Config.Camera wilyCamera;
+
+ WilyWebcam(String deviceName) {
+ for (WilyWorks.Config.Camera camera : WilyCore.config.cameras) {
+ if (camera.name.equals(deviceName)) {
+ wilyCamera = camera;
+ }
+ }
+ if (wilyCamera == null) {
+ System.out.printf("WilyWorks: Couldn't find configuration data for camera '%s'", deviceName);
+ }
+ }
+
+ @Override
+ public boolean isWebcam() {
+ return true;
+ }
+
+ @Override
+ public boolean isCameraDirection() {
+ return false;
+ }
+
+ @Override
+ public boolean isSwitchable() {
+ return false;
+ }
+
+ @Override
+ public boolean isUnknown() {
+ return false;
+ }
+
+ @Override
+ public void asyncRequestCameraPermission(Context context, Deadline deadline, Continuation> continuation) {
+
+ }
+
+ @Override
+ public boolean requestCameraPermission(Deadline deadline) {
+ return false;
+ }
+
+ @Override
+ public CameraCharacteristics getCameraCharacteristics() {
+ return null;
+ }
+
+ @Override
+ public SerialNumber getSerialNumber() {
+ return null;
+ }
+
+ @Nullable
+ @Override
+ public String getUsbDeviceNameIfAttached() {
+ return null;
+ }
+
+ @Override
+ public boolean isAttached() {
+ return false;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java
new file mode 100644
index 000000000000..6947273e92da
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumAnalogInput.java
@@ -0,0 +1,19 @@
+package com.wilyworks.simulator.framework.mechsim;
+
+import com.wilyworks.simulator.framework.WilyAnalogInput;
+
+// Hooked class for measuring the position of the drum:
+public class DrumAnalogInput extends WilyAnalogInput {
+ DecodeSlowBotMechSim mechSim;
+
+ DrumAnalogInput(DecodeSlowBotMechSim mechSim) {
+ this.mechSim = mechSim;
+ }
+
+ // Return a voltage that is proportional to the drum location, with some variation:
+ @Override
+ public double getVoltage() {
+ double variation = -0.1 + Math.random() * 0.2; // random() generates numbers between 0 and 1
+ return 3.5 * mechSim.actualDrumPosition + variation;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java
new file mode 100644
index 000000000000..ec1a42bc95d9
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumColorSensor.java
@@ -0,0 +1,63 @@
+package com.wilyworks.simulator.framework.mechsim;
+
+import com.qualcomm.robotcore.hardware.NormalizedRGBA;
+import com.wilyworks.simulator.framework.WilyNormalizedColorSensor;
+
+import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
+
+import java.awt.Color;
+
+// Hooked class for determining the color of the ball once it's in the drum:
+public class DrumColorSensor extends WilyNormalizedColorSensor {
+ DecodeSlowBotMechSim mechSim;
+ int idMask; // Sensor 0 or 1
+
+ DrumColorSensor(DecodeSlowBotMechSim mechSim, int index) {
+ this.mechSim = mechSim;
+ this.idMask = 1 << index;
+ }
+
+ // Returns true if this sensor can read a ball; false if a hole in the ball is positioned
+ // over the sensor.
+ boolean sensorCanReadBall() {
+ // Every time we get a new ball, reset our variations:
+ if (mechSim.colorSensorMask == -1) {
+ mechSim.colorSensorMask = 1 + (int) (Math.random() * 3.0); // Mask = 1, 2 or 3
+ }
+ return ((mechSim.colorSensorMask & idMask) != 0);
+ }
+
+ @Override
+ public NormalizedRGBA getNormalizedColors() {
+ NormalizedRGBA normalizedColor = new NormalizedRGBA();
+
+ // Simulate the ball holes for some reads:
+ int rgbColor = 0;
+ // Figure out what slot is being input into, if any:
+ int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS);
+ if (slot != -1) {
+ DecodeSlowBotMechSim.Ball ball = mechSim.slotBalls.get(slot);
+ if (ball != null) {
+ // There's ball over the sensors. See if they can be read:
+ if (sensorCanReadBall()) {
+ if (ball.color == DecodeSlowBotMechSim.BallColor.GREEN) {
+ rgbColor = android.graphics.Color.HSVToColor(new float[]{175, 1, 1});
+ } else {
+ rgbColor = android.graphics.Color.HSVToColor(new float[]{210, 1, 1});
+ }
+ }
+ }
+ }
+ normalizedColor.red = new Color(rgbColor).getRed() / 255.0f;
+ normalizedColor.green = new Color(rgbColor).getGreen() / 255.0f;
+ normalizedColor.blue = new Color(rgbColor).getBlue() / 255.0f;
+ return normalizedColor;
+ }
+
+ @Override
+ public double getDistance(DistanceUnit unit) {
+ int slot = mechSim.findDrumSlot(mechSim.INTAKE_POSITIONS);
+ boolean ballPositionedForRead = (slot != -1) && mechSim.slotBalls.get(slot) != null;
+ return ballPositionedForRead && sensorCanReadBall() ? unit.fromMm(18) : unit.fromMm(70);
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java
new file mode 100644
index 000000000000..daabafd7594d
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/DrumDigitalChannel.java
@@ -0,0 +1,18 @@
+package com.wilyworks.simulator.framework.mechsim;
+
+import com.wilyworks.simulator.framework.WilyDigitalChannel;
+
+public class DrumDigitalChannel extends WilyDigitalChannel {
+ DecodeSlowBotMechSim mechSim;
+ int index;
+
+ DrumDigitalChannel(DecodeSlowBotMechSim mechSim, int index) {
+ this.mechSim = mechSim;
+ this.index = index;
+ }
+
+ @Override
+ public boolean getState() {
+ return super.getState();
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java
new file mode 100644
index 000000000000..c1fc627bb3cc
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/LaunchMotor.java
@@ -0,0 +1,19 @@
+package com.wilyworks.simulator.framework.mechsim;
+
+import com.wilyworks.simulator.framework.WilyDcMotorEx;
+
+// Let us ramp up the launcher motor velocity.
+public class LaunchMotor extends WilyDcMotorEx {
+ double targetVelocity;
+ double actualVelocity;
+
+ @Override
+ public void setVelocity(double angularRate) {
+ targetVelocity = angularRate;
+ }
+
+ @Override
+ public double getVelocity() {
+ return actualVelocity;
+ }
+}
diff --git a/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java
new file mode 100644
index 000000000000..fd9568b61f80
--- /dev/null
+++ b/WilyCore/src/main/java/com/wilyworks/simulator/framework/mechsim/MechSim.java
@@ -0,0 +1,588 @@
+package com.wilyworks.simulator.framework.mechsim;
+
+import static com.wilyworks.simulator.WilyCore.time;
+
+import com.acmerobotics.roadrunner.Pose2d;
+import com.qualcomm.robotcore.hardware.AnalogInput;
+import com.qualcomm.robotcore.hardware.CRServo;
+import com.qualcomm.robotcore.hardware.DcMotorEx;
+import com.qualcomm.robotcore.hardware.DigitalChannel;
+import com.qualcomm.robotcore.hardware.HardwareDevice;
+import com.qualcomm.robotcore.hardware.HardwareMap;
+import com.qualcomm.robotcore.hardware.LED;
+import com.qualcomm.robotcore.hardware.NormalizedColorSensor;
+import com.qualcomm.robotcore.hardware.Servo;
+import com.wilyworks.simulator.WilyCore;
+import com.wilyworks.simulator.framework.WilyLED;
+import com.wilyworks.simulator.helpers.Point;
+
+import java.awt.AlphaComposite;
+import java.awt.BasicStroke;
+import java.awt.Color;
+import java.awt.Graphics2D;
+import java.awt.RenderingHints;
+import java.awt.geom.AffineTransform;
+import java.awt.geom.Arc2D;
+import java.awt.geom.Ellipse2D;
+import java.awt.geom.Rectangle2D;
+import java.util.ArrayList;
+import java.util.Arrays;
+import java.util.Collections;
+import java.util.Iterator;
+import java.util.LinkedList;
+import java.util.List;
+import java.util.stream.Collectors;
+import java.util.stream.IntStream;
+
+// This is the base class for game simulations for particular years and robots.
+public class MechSim {
+ // Create the game simulation appropriate to this year.
+ static public MechSim create() {
+ return new DecodeSlowBotMechSim();
+ }
+
+ // Hook the creation of a particular device.
+ public HardwareDevice hookDevice(String deviceName, HardwareDevice original) {
+ return original;
+ }
+
+ // Run the simulation and render the mechanisms.
+ public void update(Graphics2D g, Pose2d pose) {
+ renderRobotBox(g, pose);
+ renderLeds(g, pose);
+ }
+
+ // Advance the simulation time.
+ public void advance(double deltaT) {}
+
+ // Render the robot box.
+ protected void renderRobotBox(Graphics2D g, Pose2d pose) {
+ AffineTransform originalTransform = g.getTransform();
+ g.translate(pose.position.x, pose.position.y);
+ g.rotate(pose.heading.log());
+ g.setColor(Color.RED);
+ g.setComposite(AlphaComposite.getInstance(AlphaComposite.SRC_OVER, (float) 0.5));
+ // Draw a rectangle for the true position:
+ g.draw(new Rectangle2D.Double(
+ -WilyCore.config.robotWidth / 2.0, -WilyCore.config.robotLength / 2.0,
+ WilyCore.config.robotWidth, WilyCore.config.robotLength));
+ g.setComposite(AlphaComposite.getInstance(AlphaComposite.SRC_OVER, (float) 1.0));
+ g.setTransform(originalTransform);
+ }
+
+ // Render the LED for REV Digital LEDs.
+ protected void renderLeds(Graphics2D g, Pose2d pose) {
+ HardwareMap hardwareMap = WilyCore.hardwareMap;
+ if (hardwareMap == null)
+ return; // Might not have been created yet
+
+ final int[] colors = { 0, 0xff0000, 0x00ff00, 0xffbf00 }; // black, red, green, amber
+ final double radius = 2.0; // Circle radius, in inches
+
+ ArrayList ledArray = new ArrayList<>();
+ for (LED led: hardwareMap.led) {
+ ledArray.add((WilyLED) led);
+ }
+ for (int i = 0; i < ledArray.size(); i++) {
+ WilyLED led = ledArray.get(i);
+ int colorIndex = 0;
+ colorIndex |= (led.isRed && led.enable) ? 1 : 0;
+ colorIndex |= (!led.isRed && led.enable) ? 2 : 0;
+
+ // The LED actually needs two digital channels to describe all 4 possible colors.
+ // Assume that consecutively registered channels make a pair:
+ if (i + 1 < ledArray.size()) {
+ WilyLED nextLed = ledArray.get(i + 1);
+ if ((nextLed.x == led.x) &&
+ (nextLed.y == led.y) &&
+ (nextLed.isRed == !led.isRed)) {
+
+ colorIndex |= (nextLed.isRed && nextLed.enable) ? 1 : 0;
+ colorIndex |= (!nextLed.isRed && nextLed.enable) ? 2 : 0;
+ i++;
+ }
+ }
+
+ // Draw the circle at the location of the sensor on the robot, accounting for its
+ // current heading:
+ Point point = new Point(led.x, led.y)
+ .rotate(pose.heading.log())
+ .add(new Point(pose.position));
+ g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_ON);
+
+ g.setColor(new Color(0xffffff));
+ g.fill(new Ellipse2D.Double(point.x - radius - 0.5, point.y - radius - 0.5,2 * radius + 1, 2 * radius + 1));
+ g.setColor(new Color(colors[colorIndex]));
+ g.fill(new Ellipse2D.Double(point.x - radius, point.y - radius,2 * radius, 2 * radius));
+ g.setRenderingHint(RenderingHints.KEY_ANTIALIASING, RenderingHints.VALUE_ANTIALIAS_OFF);
+ }
+ }
+
+}
+
+// Simulation for the SlowBot in the 2025/2026 Decode game.
+class DecodeSlowBotMechSim extends MechSim {
+ enum BallColor {PURPLE, GREEN, GOLD}
+
+ final double WIDTH = 18; // Robot width
+ final double HEIGHT = 18; // Robot height
+ final double BALL_SIZE = 5; // 5 inches in diameter
+ final double DRUM_SERVO_SPEED = 1.0 / 0.9; // Speed of the drum servo, position/s
+ final Point[] INTAKE_OFFSETS = { new Point(8, -1), new Point(8, +1) };
+ final double INTAKE_EPSILON = 2.5; // Epsilon for intake distance
+ final double INTAKE_POWER = 0.1; // Minimum power for intake
+ final double[] INTAKE_POSITIONS = { 0.0/6, 2.0/6, 4.0/6 }; // AKA 'launch' positions
+ final double[] TRANSFER_POSITIONS = { 3.0/6, 5.0/6, 1.0/6 }; // Servo positions for intaking
+ final double SLOT_EPSILON = 0.02; // Epsilon for determining a slot relative to a [0, 1] range
+ final double MIN_TRANSFER_TIME = 0.020; // Second it takes for a transfer
+ final double MIN_TRANSFER_POSITION = 0.6; // Minimum position to start a transfer
+ final double TRANSFER_SERVO_SPEED = (60.0 / 360) / 0.25; // Speed of a goBilda torque servo, position/s
+ final double LAUNCH_SPEED = 144; // Ball launch speed, inches per second
+ final Point LAUNCH_OFFSET = new Point(-4, 0);
+ final double FEEDER_POWER = 0.1; // Minimum power for the feeder servo
+ final double GOAL_EPSILON = 12; // Distance from goal center to consider a goal
+ final Point[] GOAL_CENTERS = { new Point(-72, 72), new Point(-72, -72) };
+ final Point[] CLASSIFIER_STARTS = { new Point(-BALL_SIZE/2, 72-BALL_SIZE/2),
+ new Point(-BALL_SIZE/2, -72-BALL_SIZE/2) }; // Start locations, corresponding to goals
+ final double LAUNCH_ACCELERATION = 1000; // Increase flywheel speed by this many ticks per second
+ final double LAUNCH_DROP = 500; // Drop flywheel speed by this many ticks on launch
+ final double LAUNCH_EPSILON = 50; // Target and actual flywheel velocities must be within this amount
+ final double INTAKE_ERROR_PROBABILITY = 0.2; // Probability that a ball is missed on intake
+
+ // Struct for tracking ball locations:
+ static class Ball {
+ BallColor color; // GREEN or PURPLE
+ Point point; // Current location; not relevant if in robot mechanisms
+ Point velocity; // Velocity vector; null for balls lying on the field
+ public Ball(BallColor color, double x, double y) {
+ this.color = color;
+ this.point = new Point(x, y);
+ }
+ }
+
+ // These preset balls are mirrored in y the constructor:
+ Ball[] ballPresets = {
+ new Ball(BallColor.GREEN, -12, 53),
+ new Ball(BallColor.PURPLE, -12, 48),
+ new Ball(BallColor.PURPLE, -12, 43),
+ new Ball(BallColor.PURPLE, 12, 53),
+ new Ball(BallColor.GREEN, 12, 48),
+ new Ball(BallColor.PURPLE, 12, 43),
+ new Ball(BallColor.PURPLE, 36, 53),
+ new Ball(BallColor.PURPLE, 36, 48),
+ new Ball(BallColor.GREEN, 36, 43),
+ new Ball(BallColor.PURPLE, 69.5, 69.5),
+ new Ball(BallColor.GREEN, 64.5, 69.5),
+ new Ball(BallColor.PURPLE, 59.5, 69.5)
+ };
+ Ball[] ballPreloads = {
+ new Ball(BallColor.GOLD, 0, 0),
+ new Ball(BallColor.GOLD, 0, 0),
+ new Ball(BallColor.GOLD, 0, 0)
+ };
+
+ // Hooked devices:
+ LaunchMotor upperLaunchMotor;
+ LaunchMotor lowerLaunchMotor;
+ AnalogInput analogDrum;
+ NormalizedColorSensor sensorColor1;
+ NormalizedColorSensor sensorColor2;
+ DigitalChannel digitalChannel1;
+ DigitalChannel digitalChannel2;
+ DcMotorEx intakeMotor;
+ Servo drumServo;
+ Servo transferServo;
+ CRServo forwardFeederServo;
+ CRServo backwardFeederServo;
+
+ // State:
+ double accumulatedDeltaT;
+ Ball intakeBall; // Ball in the intake, may be null
+ List slotBalls = new ArrayList<>(Collections.nCopies(3, null));
+ List airBalls = new LinkedList<>(); // Balls flying through the air
+ List fieldBalls = new LinkedList<>(); // Pickable balls on the field
+ List> classifierBalls = IntStream.range(0, 2)
+ .mapToObj(i -> new LinkedList()).collect(Collectors.toList());
+ double actualDrumPosition; // Current location of the drum, [0, 1]
+ double actualTransferPosition; // Current transfer servo position, [0, 1]
+ double transferStartTime; // Time that a transfer started, zero when not transferring
+ int colorSensorMask = -1; // Random 2-bit mask indicating which sensors return true data; -1 if reset
+ boolean hasIntaken = false; // True once any ball has been taken into the drum
+ boolean hasLaunched = false; // True once any ball has been launched
+
+ // Initialize the beast.
+ DecodeSlowBotMechSim() {
+ // Add the presets to the field, along with the mirrored-in y versions:
+ for (Ball ball: ballPresets) {
+ fieldBalls.add(ball);
+ fieldBalls.add(new Ball(ball.color, ball.point.x, -ball.point.y));
+ }
+ }
+
+ // Compute the draw color from the ball object.
+ private Color ballColor(Ball ball) {
+ if (ball == null)
+ return Color.BLACK;
+ else if (ball.color == BallColor.PURPLE)
+ return new Color(128, 0, 128);
+ else if (ball.color == BallColor.GREEN)
+ return Color.GREEN;
+ else
+ return new Color(255, 215, 0); // Gold
+ }
+
+ // WilyHardwareMap calls this method when it creates a device, allowing us to substitute
+ // with a different device object.
+ @Override
+ public HardwareDevice hookDevice(String name, HardwareDevice device) {
+ // These are input-only devices:
+ if (name.equals("motIntake")) {
+ intakeMotor = (DcMotorEx) device;
+ }
+ if (name.equals("servoDrum")) {
+ drumServo = (Servo) device;
+ }
+ if (name.equals("servoTransfer")) {
+ transferServo = (Servo) device;
+ }
+ if (name.equals("servoFLaunchFeeder")) {
+ forwardFeederServo = (CRServo) device;
+ }
+ if (name.equals("servoBLaunchFeeder")) {
+ backwardFeederServo = (CRServo) device;
+ }
+
+ // There have outputs:
+ if (name.equals("motULauncher")) {
+ device = upperLaunchMotor = new LaunchMotor();
+ }
+ if (name.equals("motLLauncher")) {
+ device = lowerLaunchMotor = new LaunchMotor();
+ }
+ if (name.equals("analogDrum")) {
+ device = analogDrum = new DrumAnalogInput(this);
+ }
+ if (name.equals("sensorColor1")) {
+ device = sensorColor1 = new DrumColorSensor(this, 0);
+ }
+ if (name.equals("sensorColor2")) {
+ device = sensorColor2 = new DrumColorSensor(this, 1);
+ }
+ if (name.equals("breakBeam1")) {
+ device = digitalChannel1 = new DrumDigitalChannel(this, 1);
+ }
+ if (name.equals("breakBeam2")) {
+ device = digitalChannel2 = new DrumDigitalChannel(this, 2);
+ }
+ return device;
+ }
+
+ // Check to see if the caller created all of the expected state.
+ void verifyState() {
+ if (upperLaunchMotor == null)
+ throw new RuntimeException("Missing upper launch motor");
+ if (lowerLaunchMotor == null)
+ throw new RuntimeException("Missing lower launch motor");
+ if (intakeMotor == null)
+ throw new RuntimeException("Missing intake motor");
+ if (drumServo == null)
+ throw new RuntimeException("Missing drum servo");
+ if (transferServo == null)
+ throw new RuntimeException("Missing transfer servo");
+ if (forwardFeederServo == null)
+ throw new RuntimeException("Missing forward feeder servo");
+ if (backwardFeederServo == null)
+ throw new RuntimeException("Missing backward feeder servo");
+ if (analogDrum == null)
+ throw new RuntimeException("Missing analog drum");
+ if (sensorColor1 == null)
+ throw new RuntimeException("Missing color sensor 1");
+ if (sensorColor2 == null)
+ throw new RuntimeException("Missing color sensor 2");
+ }
+
+ void render(Graphics2D g, Pose2d pose) {
+ // Draw the balls on the field:
+ for (Ball ball : fieldBalls) {
+ g.setColor(ballColor(ball));
+ g.fill(ballEllipse(ball.point.x, ball.point.y));
+ }
+
+ // Draw the classifier balls:
+ for (int i = 0; i < 2; i++) {
+ double x = CLASSIFIER_STARTS[i].x;
+ double y = CLASSIFIER_STARTS[i].y;
+ int count = 0;
+ for (Ball ball: classifierBalls.get(i)) {
+ g.setColor(ballColor(ball));
+ g.fill(ballEllipse(x, y));
+ x -= BALL_SIZE;
+ count++;
+ if (count >= 9)
+ break; // Classifier maxes out at 9 balls, more causes an overflow
+ }
+ }
+
+ AffineTransform fieldTransform = g.getTransform();
+
+ // Set the transform to draw the robots in the canonical space, from (-1, -1) to (1, 1):
+ g.translate(pose.position.x, pose.position.y);
+ g.rotate(pose.heading.log());
+ g.scale(WIDTH / 2, HEIGHT / 2);
+
+ // Draw the robot outline:
+ g.setStroke(new BasicStroke(0.1f));
+ g.setColor(Color.DARK_GRAY);
+ g.fill(new Rectangle2D.Double(-1, -1, 2, 2));
+
+ // Draw the intake wheels:
+ Color intakeColor = Color.GRAY;
+ if (intakeMotor.getPower() > 0) {
+ intakeColor = Color.GREEN; // Green when intaking
+ } else if (intakeMotor.getPower() < 0) {
+ intakeColor = Color.RED; // Red when spitting out
+ }
+ g.setColor(intakeColor);
+ final int INTAKE_WHEEL_COUNT = 4;
+ final double INTAKE_HEIGHT = 0.2;
+ final double INTAKE_SPACE = (1.6 - INTAKE_WHEEL_COUNT * INTAKE_HEIGHT) / (INTAKE_WHEEL_COUNT - 1);
+ for (int i = 1; i < 3; i++) { // Just draw the middle two wheels
+ double y = -0.8 + i * (INTAKE_HEIGHT + INTAKE_SPACE);
+ g.fill(new Rectangle2D.Double(0.6, y, 0.4, INTAKE_HEIGHT));
+ }
+
+ // Draw the drum:
+ g.setColor(Color.GRAY);
+ g.translate(-0.33, 0);
+ g.rotate(-2 * Math.PI * actualDrumPosition); // Note the negative
+ g.fill(new Ellipse2D.Double(-0.8, -0.8, 1.6, 1.6));
+
+ // Draw the dead wedge area:
+ g.setColor(Color.DARK_GRAY);
+ g.fill(new Arc2D.Double(-0.8, -0.8, 1.6, 1.6, 0.0f, -60, Arc2D.PIE));
+
+ // Draw each of the drum slot circles:
+ for (Ball ball: slotBalls) {
+ g.setColor(ballColor(ball));
+ g.fill(new Ellipse2D.Double(0.1, -0.25, 0.6, 0.6));
+ g.rotate(Math.toRadians(120));
+ }
+
+ // Draw the air balls:
+ g.setTransform(fieldTransform);
+ for (Ball ball : airBalls) {
+ g.setColor(ballColor(ball));
+ g.fill(ballEllipse(ball.point.x, ball.point.y));
+ }
+
+ // Draw the intake ball, if there is one:
+ if (intakeBall != null) {
+ g.translate(pose.position.x, pose.position.y);
+ g.rotate(pose.heading.log());
+ g.setColor(ballColor(intakeBall));
+ g.fill(ballEllipse(7, 0));
+ }
+ }
+
+ // Shortcut to create an ellipse representing a ball, centered around the specified point.
+ Ellipse2D ballEllipse(double x, double y) {
+ return new Ellipse2D.Double(x - BALL_SIZE/2, y - BALL_SIZE/2, BALL_SIZE, BALL_SIZE);
+ }
+
+
+ // Find the slot that corresponds to the current drum angle. Returns -1 if not found.
+ int findDrumSlot(double[] slotPositions) {
+ for (int i = 0; i < 3; i++) {
+ if (Math.abs(actualDrumPosition - slotPositions[i]) < SLOT_EPSILON) {
+ return i;
+ }
+ }
+ return -1;
+ }
+
+ // Advance the simulation:
+ void simulate(Pose2d pose, double deltaT) {
+ verifyState();
+
+ double heading = pose.heading.log();
+ // Advance the balls flying through the air:
+ Iterator ballIterator = airBalls.iterator();
+ while (ballIterator.hasNext()) {
+ Ball ball = ballIterator.next();
+ // Move the ball:
+ ball.point = ball.point.add(ball.velocity.multiply(deltaT));
+
+ // See if it's scored a goal:
+ for (int i = 0; i < 2; i++) { // Goal index
+ if (Math.hypot(GOAL_CENTERS[i].x - ball.point.x,
+ GOAL_CENTERS[i].y - ball.point.y) < GOAL_EPSILON) {
+ ballIterator.remove();
+ classifierBalls.get(i).add(ball);
+ }
+ }
+ }
+
+ // Ramp up the launcher motors velocities:
+ double upperDiff = upperLaunchMotor.targetVelocity - upperLaunchMotor.actualVelocity;
+ double lowerDiff = lowerLaunchMotor.targetVelocity - lowerLaunchMotor.actualVelocity;
+ upperLaunchMotor.actualVelocity += Math.signum(upperDiff) * Math.min(Math.abs(upperDiff), deltaT * LAUNCH_ACCELERATION);
+ lowerLaunchMotor.actualVelocity += Math.signum(lowerDiff) * Math.min(Math.abs(lowerDiff), deltaT * LAUNCH_ACCELERATION);
+
+ // Move the transfer servo towards the target angle:
+ double targetTransferPosition = transferServo.getPosition();
+ double transferDiff = targetTransferPosition - actualTransferPosition;
+ actualTransferPosition += Math.signum(transferDiff) * Math.min(Math.abs(transferDiff), deltaT * TRANSFER_SERVO_SPEED);
+
+ // Move the drum towards the target angle:
+ double targetDrumPosition = drumServo.getPosition();
+ double drumDiff = targetDrumPosition - actualDrumPosition;
+ actualDrumPosition += Math.signum(drumDiff) * Math.min(Math.abs(drumDiff), deltaT * DRUM_SERVO_SPEED);
+
+ // Reset the color sensor mask whenever the drum moves:
+ if (targetDrumPosition != actualDrumPosition) {
+ colorSensorMask = -1;
+ }
+
+ // Find the slot if in position to transfer:
+ int transferSlot = findDrumSlot(TRANSFER_POSITIONS);
+
+ // Handle load requests for the launch calibration app, signaled by running the launchers
+ // backwards:
+ if (upperLaunchMotor.getVelocity() < 0) {
+ if (forwardFeederServo.getPower() >= 0) {
+ throw new RuntimeException("When running launch motors backwards, forward feeder servo must too.");
+ }
+ if (backwardFeederServo.getPower() >= 0) {
+ throw new RuntimeException("When running launch motors backwards, backward feeder servo must too.");
+ }
+ // If the slot is empty, fill it up with a ball!
+ if ((transferSlot != -1) && (slotBalls.get(transferSlot) == null)) {
+ slotBalls.set(transferSlot, new Ball(BallColor.PURPLE, 0, 0));
+ }
+ }
+
+ // Handle transfer requests:
+ if (actualTransferPosition <= MIN_TRANSFER_POSITION) {
+ if (transferStartTime != 0) {
+ throw new RuntimeException("Didn't transfer for sufficient time.");
+ }
+ } else {
+ if (transferSlot == -1) {
+ throw new RuntimeException("A transfer is requested when drum isn't in the right spot. That will break things!");
+ }
+ if (targetDrumPosition != actualDrumPosition) {
+ throw new RuntimeException("The drum is moving during a transfer. That will break things!");
+ }
+ if (upperLaunchMotor.getVelocity() <= 0) {
+ throw new RuntimeException("A transfer is requested when upper launcher motor isn't running forward. That won't work!");
+ }
+ if (lowerLaunchMotor.getVelocity() <= 0) {
+ throw new RuntimeException("A transfer is requested when lower launcher motor isn't running forward. That won't work!");
+ }
+ if (forwardFeederServo.getPower() < FEEDER_POWER) {
+ throw new RuntimeException("A transfer is requested when forward feeder servo isn't running. That won't work!");
+ }
+ if (Math.abs(backwardFeederServo.getPower()) < FEEDER_POWER) {
+ throw new RuntimeException("A transfer is requested when backward feeder servo isn't running. That won't work!");
+ }
+ if (slotBalls.get(transferSlot) == null) {
+ if (!hasIntaken && !hasLaunched) {
+ // A transfer has been requested there was no intake done. Assume that this
+ // is Auto with preloads and populate the drum with three golden balls. We
+ // make them gold balls so that we don't have to worry about getting the
+ // ordering correct.
+ slotBalls = new ArrayList<>(Arrays.asList(ballPreloads));
+ }
+ }
+ if (slotBalls.get(transferSlot) != null) {
+ if (transferStartTime == 0) {
+ transferStartTime = time();
+
+ // Check these only at the start of the transfer because we're going to
+ // immediately drop the speeds:
+ if (Math.abs(upperLaunchMotor.targetVelocity - upperLaunchMotor.actualVelocity) > LAUNCH_EPSILON) {
+ throw new RuntimeException("A transfer is requested when upper launcher motor isn't running. That won't work!");
+ }
+ if (Math.abs(lowerLaunchMotor.targetVelocity - lowerLaunchMotor.actualVelocity) > LAUNCH_EPSILON) {
+ throw new RuntimeException("A transfer is requested when lower launcher motor isn't running. That won't work!");
+ }
+
+ upperLaunchMotor.actualVelocity -= LAUNCH_DROP;
+ lowerLaunchMotor.actualVelocity -= LAUNCH_DROP;
+ } else if (time() - transferStartTime > MIN_TRANSFER_TIME) {
+ // Transfer the ball from the drum to the air-balls list:
+ Ball ball = slotBalls.get(transferSlot);
+ ball.velocity = new Point(LAUNCH_SPEED, 0).rotate(heading);
+ ball.point = new Point(pose.position).add(LAUNCH_OFFSET.rotate(heading));
+ airBalls.add(ball);
+ slotBalls.set(transferSlot, null);
+ transferStartTime = 0;
+ hasLaunched = true;
+ }
+ }
+ }
+
+ // Finally, check for intake:
+ if (intakeMotor.getPower() > INTAKE_POWER) {
+ if (intakeBall == null) {
+ // We're intaking from the field into the intake:
+ for (Point intakeOffset : INTAKE_OFFSETS) {
+ Point intakePoint = new Point(pose.position).add(intakeOffset.rotate(pose.heading.log()));
+ for (Ball ball : fieldBalls) {
+ double distance = Math.hypot(ball.point.x - intakePoint.x, ball.point.y - intakePoint.y);
+ if (distance < INTAKE_EPSILON) {
+ // Remove the ball from the field. I think this is okay so long as we
+ // terminate the loop...
+ fieldBalls.remove(ball);
+
+ // Move the ball into an intake position, unless random error says
+ // to toss it:
+ if ((!WilyCore.enableSensorError) || (Math.random() > INTAKE_ERROR_PROBABILITY)) {
+ intakeBall = ball;
+ }
+ break;
+ }
+ }
+ }
+ } else {
+ // We're intaking from the intake into a drum slot:
+ int slot = findDrumSlot(INTAKE_POSITIONS);
+ if (slot != -1) {
+ // Accumulate the empty slots:
+ LinkedList emptySlots = new LinkedList<>();
+ for (int i = 0; i < 3; i++) {
+ if (slotBalls.get(i) == null) {
+ emptySlots.add(i);
+ }
+ }
+ // Assign the ball to an empty slot:
+ if (!emptySlots.isEmpty()) {
+ int emptySlot = emptySlots.get((int) (Math.random() * emptySlots.size()));
+ slotBalls.set(emptySlot, intakeBall);
+ intakeBall = null;
+ hasIntaken = true;
+ }
+ }
+ }
+ }
+ }
+
+ // Advance the Mech simulation.
+ @Override
+ public void advance(double deltaT) {
+ // Note that we don't update our simulation until update() time, when we also know the
+ // new robot pose.
+ this.accumulatedDeltaT += deltaT;
+ }
+
+ // Render the robot and the balls.
+ @Override
+ public void update(Graphics2D g, Pose2d pose) {
+ // Don't call simulate() or render() for things like Configuration Tester.
+ if (upperLaunchMotor != null) {
+ simulate(pose, accumulatedDeltaT);
+ render(g, pose);
+ }
+ accumulatedDeltaT = 0;
+ }
+}
diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java
index 0e1d55ab76ee..3a266370f70c 100644
--- a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java
+++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/AngleUnit.java
@@ -223,4 +223,14 @@ public static float normalizeRadians(float radians)
{
return (float)normalizeRadians((double)radians);
}
+
+ public UnnormalizedAngleUnit getUnnormalized()
+ {
+ switch (this)
+ {
+ default:
+ case RADIANS: return UnnormalizedAngleUnit.RADIANS;
+ case DEGREES: return UnnormalizedAngleUnit.DEGREES;
+ }
+ }
}
diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java
new file mode 100644
index 000000000000..ad88ccbabc06
--- /dev/null
+++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Pose3D.java
@@ -0,0 +1,89 @@
+package org.firstinspires.ftc.robotcore.external.navigation;
+
+/*
+ * Copyright (c) 2024 Dryw Wade
+ *
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to
+ * endorse or promote products derived from this software without specific prior
+ * written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+ * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+ * THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+import androidx.annotation.NonNull;
+
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.YawPitchRollAngles;
+
+import java.util.Locale;
+
+/**
+ * Pose3D represents the position and orientation of an object in 3D space.
+ */
+public class Pose3D
+{
+ protected final Position position;
+ protected final YawPitchRollAngles orientation;
+
+ public Pose3D(Position position, YawPitchRollAngles orientation)
+ {
+ this.position = position;
+ this.orientation = orientation;
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ @NonNull
+ @Override public String toString()
+ {
+ return String.format(Locale.getDefault(),
+ "position=%s, orientation=%s",
+ position.toString(),
+ orientation.toString());
+ }
+
+ /**
+ * A 3D orientation.
+ *
+ * The axis mapping is defined by the code that creates objects from this class. One should not assume that
+ * pitch, for example, is along the x axis. Consult the documentation of the API that returns
+ * a Pose3D for its axis mapping.
+ */
+ public YawPitchRollAngles getOrientation()
+ {
+ return orientation;
+ }
+
+ /**
+ * A 3D position.
+ */
+ public Position getPosition()
+ {
+ return position;
+ }
+}
diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java
new file mode 100644
index 000000000000..feb87ba7c310
--- /dev/null
+++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/Position.java
@@ -0,0 +1,100 @@
+package org.firstinspires.ftc.robotcore.external.navigation;
+
+/*
+Copyright (c) 2016 Robert Atkinson
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Robert Atkinson nor the names of his contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+import java.util.Locale;
+
+/**
+ * Instances of {@link Position} represent a three-dimensional distance in a particular distance unit.
+ *
+ * @see Position
+ */
+public class Position
+{
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ public DistanceUnit unit;
+
+ public double x;
+ public double y;
+ public double z;
+
+ /**
+ * the time on the System.nanoTime() clock at which the data was acquired. If no
+ * timestamp is associated with this particular set of data, this value is zero.
+ */
+ public long acquisitionTime;
+
+ //----------------------------------------------------------------------------------------------
+ // Construction
+ //----------------------------------------------------------------------------------------------
+
+ public Position()
+ {
+ this(DistanceUnit.MM, 0, 0, 0, 0);
+ }
+
+ public Position(DistanceUnit unit, double x, double y, double z, long acquisitionTime)
+ {
+ this.unit = unit;
+ this.x = x;
+ this.y = y;
+ this.z = z;
+ this.acquisitionTime = acquisitionTime;
+ }
+
+ public Position toUnit(DistanceUnit distanceUnit)
+ {
+ if (distanceUnit != this.unit)
+ {
+ return new Position(distanceUnit,
+ distanceUnit.fromUnit(this.unit, x),
+ distanceUnit.fromUnit(this.unit, y),
+ distanceUnit.fromUnit(this.unit, z),
+ this.acquisitionTime);
+ }
+ else
+ return this;
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ @Override public String toString()
+ {
+ return String.format(Locale.getDefault(), "(%.3f %.3f %.3f)%s", x, y, z, unit.toString());
+ }
+}
diff --git a/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java
new file mode 100644
index 000000000000..724c144de3c7
--- /dev/null
+++ b/WilyCore/src/main/java/org/firstinspires/ftc/robotcore/external/navigation/UnnormalizedAngleUnit.java
@@ -0,0 +1,198 @@
+package org.firstinspires.ftc.robotcore.external.navigation;
+
+/*
+Copyright (c) 2016 Robert Atkinson
+
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without modification,
+are permitted (subject to the limitations in the disclaimer below) provided that
+the following conditions are met:
+
+Redistributions of source code must retain the above copyright notice, this list
+of conditions and the following disclaimer.
+
+Redistributions in binary form must reproduce the above copyright notice, this
+list of conditions and the following disclaimer in the documentation and/or
+other materials provided with the distribution.
+
+Neither the name of Robert Atkinson nor the names of his contributors may be used to
+endorse or promote products derived from this software without specific prior
+written permission.
+
+NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR
+TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF
+THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+*/
+
+/**
+ * An {@link UnnormalizedAngleUnit} represents angles in different units of measure and
+ * provides utility methods to convert across units. {@link UnnormalizedAngleUnit} does not
+ * maintain angle information internally, but only helps organize
+ * and use angle measures that may be maintained separately across various contexts.
+ *
+ * Angles can be distinguished along (at least) two axes:
+ * Normalized angles are of most utility when dealing with physical angles, as normalization
+ * removes ambiguity of representation. In particular, two angles can be compared for equality
+ * by subtracting them, normalizing, and testing whether the absolute value of the result is
+ * smaller than some tolerance threshold. This approach neatly handles all cases of cyclical
+ * wrapping without unexpected discontinuities.
+ *
+ *
+ * Unnormalized angles can be handy when the angular quantity is not a physical angle but some
+ * related quantity such as an angular velocity or acceleration, where the
+ * quantity in question lacks the 360-degree cyclical equivalence of a physical angle.
+ *