diff --git a/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit-members.html b/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit-members.html index 38f4de9..4bd209a 100644 --- a/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit-members.html +++ b/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit-members.html @@ -86,18 +86,69 @@
-
com.lasarobotics.library.util.DistanceUnit Member List
+
com.lasarobotics.library.util.Units Member List
+
-

This is the complete list of members for com.lasarobotics.library.util.DistanceUnit, including all inherited members.

+

This is the complete list of members for com.lasarobotics.library.util.Units, + including all inherited members.

- - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + +
CENTIMETERS (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
ENCODER_COUNTS (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
FEET (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
INCHES (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
METERS (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
REVOLUTIONS (defined in com.lasarobotics.library.util.DistanceUnit)com.lasarobotics.library.util.DistanceUnit
CENTIMETERS (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
ENCODER_COUNTS (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
FEET (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
INCHES (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
METERS (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
REVOLUTIONS (defined in com.lasarobotics.library.util.Units) + com.lasarobotics.library.util.Units +
diff --git a/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit.html b/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit.html index 04e7cb2..9f845b7 100644 --- a/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit.html +++ b/docs/doxygen/html/enumcom_1_1lasarobotics_1_1library_1_1util_1_1_distance_unit.html @@ -4,7 +4,7 @@ -FTCAndroidLibrary: com.lasarobotics.library.util.DistanceUnit Enum Reference + FTCAndroidLibrary: com.lasarobotics.library.util.Units Enum Reference @@ -89,7 +89,8 @@ Public Attributes | List of all members
-
com.lasarobotics.library.util.DistanceUnit Enum Reference
+
com.lasarobotics.library.util.Units Enum Reference
+
diff --git a/docs/doxygen/html/hierarchy.html b/docs/doxygen/html/hierarchy.html index 498fec8..45c8540 100644 --- a/docs/doxygen/html/hierarchy.html +++ b/docs/doxygen/html/hierarchy.html @@ -95,7 +95,14 @@ - + + + + diff --git a/docs/javadoc/allclasses-frame.html b/docs/javadoc/allclasses-frame.html index cc07ec6..4bb568e 100644 --- a/docs/javadoc/allclasses-frame.html +++ b/docs/javadoc/allclasses-frame.html @@ -25,7 +25,8 @@

All Classes

  • DoodleWrite
  • Gyroscope
  • IR
    • -
  • Kalman
    • +
  • Kalman
  • LiDAR
  • LinearAcceleration
  • LookupTable
    • diff --git a/docs/javadoc/allclasses-noframe.html b/docs/javadoc/allclasses-noframe.html index ff68180..638f12f 100644 --- a/docs/javadoc/allclasses-noframe.html +++ b/docs/javadoc/allclasses-noframe.html @@ -25,7 +25,8 @@

    All Classes

  • DoodleWrite
  • Gyroscope
  • IR
    • -
  • Kalman
    • +
  • Kalman
  • LiDAR
  • LinearAcceleration
  • LookupTable
    • diff --git a/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html b/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html index 2168b95..d46d513 100644 --- a/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html +++ b/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html @@ -78,7 +78,7 @@
    -
    com.lasarobotics.library.skynet
    +
    com.lasarobotics.library.nav

    Class Kalman

    @@ -86,7 +86,7 @@

    Class Kalman

  • java.lang.Object
    • -
    • com.lasarobotics.library.skynet.Kalman
      • +
    • com.lasarobotics.library.nav.KalmanFilter
    • diff --git a/docs/javadoc/com/lasarobotics/library/skynet/package-frame.html b/docs/javadoc/com/lasarobotics/library/skynet/package-frame.html index 376ecd5..ed35770 100644 --- a/docs/javadoc/com/lasarobotics/library/skynet/package-frame.html +++ b/docs/javadoc/com/lasarobotics/library/skynet/package-frame.html @@ -3,16 +3,18 @@ -com.lasarobotics.library.skynet + com.lasarobotics.library.nav -

    com.lasarobotics.library.skynet

    +

    com.lasarobotics.library.nav

    Classes

    diff --git a/docs/javadoc/com/lasarobotics/library/skynet/package-summary.html b/docs/javadoc/com/lasarobotics/library/skynet/package-summary.html index d65e9e2..b230b86 100644 --- a/docs/javadoc/com/lasarobotics/library/skynet/package-summary.html +++ b/docs/javadoc/com/lasarobotics/library/skynet/package-summary.html @@ -3,7 +3,7 @@ -com.lasarobotics.library.skynet + com.lasarobotics.library.nav @@ -62,7 +62,7 @@
    -

    Package com.lasarobotics.library.skynet

    +

    Package com.lasarobotics.library.nav

      @@ -75,7 +75,8 @@

      Package com.lasarobotics.library.skynet

    - + - + diff --git a/docs/javadoc/overview-tree.html b/docs/javadoc/overview-tree.html index e1e7b15..d8e80da 100644 --- a/docs/javadoc/overview-tree.html +++ b/docs/javadoc/overview-tree.html @@ -79,7 +79,8 @@

    Hierarchy For All Packages

  • com.lasarobotics.library.sensor.legacy.hitechnic,
  • com.lasarobotics.library.sensor.legacy.lego,
  • com.lasarobotics.library.sensor.modernrobotics,
    • -
  • com.lasarobotics.library.skynet,
    • +
  • com.lasarobotics.library.nav, +
  • com.lasarobotics.library.util
    • @@ -106,7 +107,10 @@

    Class Hierarchy

  • com.lasarobotics.library.doodle.DoodleWrite
  • com.lasarobotics.library.sensor.legacy.hitechnic.Gyroscope
  • com.lasarobotics.library.sensor.generic.IR
    • -
  • com.lasarobotics.library.skynet.Kalman
    • +
  • com.lasarobotics.library.skynet.Kalman +
  • com.lasarobotics.library.sensor.generic.LiDAR
  • com.lasarobotics.library.sensor.android.LinearAcceleration
  • com.lasarobotics.library.util.LookupTable<T>
    • diff --git a/ftc-library/build.gradle b/ftc-library/build.gradle index 7b37499..a98e826 100644 --- a/ftc-library/build.gradle +++ b/ftc-library/build.gradle @@ -23,12 +23,15 @@ android { repositories { flatDir { - dirs = ['libs'] + dirs = ['../libs'] } } + dependencies { compile files('libs/android-support-v4.jar') + compile project(':lib-navx') + compile 'com.google.code.gson:gson:2.4' compile(name: 'RobotCore-release', ext: 'aar') diff --git a/ftc-library/src/main/java/com/lasarobotics/library/android/Sensors.java b/ftc-library/src/main/java/com/lasarobotics/library/android/Sensors.java new file mode 100644 index 0000000..02fc365 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/android/Sensors.java @@ -0,0 +1,193 @@ +package com.lasarobotics.library.android; + +import android.content.Context; +import android.hardware.Sensor; +import android.hardware.SensorEvent; +import android.hardware.SensorEventListener; +import android.hardware.SensorManager; +import android.util.Log; +import android.view.WindowManager; + +import com.lasarobotics.library.util.ScreenOrientation; +import com.lasarobotics.library.util.Vector3; + +/** + * Contains methods for reading Android native sensors, other than the camera + */ +public final class Sensors implements SensorEventListener { + + private static final float PITCH_TOLERANCE = 20.0f; + private static final float PITCH_TOLERANCE_HIGH = 45.0f; + private static final float ROLL_MINIMUM = 10.0f; + private static final int READ_SPEED = SensorManager.SENSOR_DELAY_FASTEST; + + static float[] gravity = new float[3]; + static float[] linearAcceleration = new float[3]; + static float[] geomagnetic = new float[3]; + + static float[] integratedVelocity = new float[3]; + static float[] integratedPosition = new float[3]; + static long lastTime = 0; //in nanoseconds + + private static boolean activated = false; + private final SensorManager mSensorManager; + private final Sensor mAccelerometer; + private final Sensor mMagneticField; + private ScreenOrientation screenOrientation = null; + + public Sensors() { + mSensorManager = (SensorManager) Util.getContext().getSystemService(Context.SENSOR_SERVICE); + mAccelerometer = mSensorManager.getDefaultSensor(Sensor.TYPE_ACCELEROMETER); + mMagneticField = mSensorManager.getDefaultSensor(Sensor.TYPE_MAGNETIC_FIELD); + activated = false; + resume(); + resetIntegration(); + } + + public void resume() { + if (activated) + return; + mSensorManager.registerListener(this, mAccelerometer, READ_SPEED); + mSensorManager.registerListener(this, mMagneticField, READ_SPEED); + activated = true; + resetIntegration(); + } + + public void stop() { + if (!activated) + return; + activated = false; + mSensorManager.unregisterListener(this); + } + + /** + * Get the current linear acceleration in m/s^2 + * + * @return The instantaneous linear acceleration in m/s^2 + */ + public Vector3 getLinearAcceleration() { + return new Vector3<>(linearAcceleration[0], linearAcceleration[1], linearAcceleration[2]); + } + + public Vector3 getIntegratedVelocity() { + return new Vector3<>(integratedVelocity[0], integratedVelocity[1], integratedVelocity[2]); + } + + public Vector3 getIntegratedPosition() { + return new Vector3<>(integratedPosition[0], integratedPosition[1], integratedPosition[2]); + } + + public void resetIntegration() { + integratedPosition = new float[]{0, 0, 0}; + integratedVelocity = new float[]{0, 0, 0}; + lastTime = 0; + } + + public boolean hasOrientation() { + return screenOrientation != null; + } + + public ScreenOrientation getScreenOrientation() { + return screenOrientation != null ? screenOrientation : ScreenOrientation.LANDSCAPE; + } + + public ScreenOrientation getActivityScreenOrientation() { + WindowManager windowManager = (WindowManager) Util.getContext().getSystemService(Context.WINDOW_SERVICE); + int rotation = windowManager.getDefaultDisplay().getRotation(); + return ScreenOrientation.getFromSurface(rotation); + } + + public double getScreenOrientationCompensation() { + return getScreenOrientation().getAngle() - getActivityScreenOrientation().getAngle() + ScreenOrientation.PORTRAIT.getAngle(); + } + + private void updateScreenOrientation() { + float[] R = new float[9]; + float[] I = new float[9]; + SensorManager.getRotationMatrix(R, I, gravity, geomagnetic); + float[] orientation = new float[3]; + SensorManager.getOrientation(R, orientation); + + //device rotation angle = pitch (first value) [clockwise from horizontal] + + double pitch = orientation[1] / 2 / Math.PI * 360.0; + double roll = orientation[2] / 2 / Math.PI * 360.0; + double azimuth = orientation[0] / 2 / Math.PI * 360.0; + + Log.w("Rotation", pitch + ", " + roll + ", " + azimuth); + + //If the phone is too close to the ground, don't update + if (Math.abs(roll) <= ROLL_MINIMUM) + return; + + ScreenOrientation current = screenOrientation; + + if (Math.abs(pitch) <= PITCH_TOLERANCE) + if (roll > 0.0f) + current = ScreenOrientation.LANDSCAPE_WEST; + else + current = ScreenOrientation.LANDSCAPE; + else if (Math.abs(pitch) >= PITCH_TOLERANCE_HIGH) + if (pitch > 0.0f) + current = ScreenOrientation.PORTRAIT; + else + current = ScreenOrientation.PORTRAIT_REVERSE; + + screenOrientation = current; + } + + private void integrateSensors() { + long currentTime = System.nanoTime(); + long timeDelta = currentTime - lastTime; + + if (lastTime == 0) { + lastTime = currentTime; + return; + } + + lastTime = currentTime; + + final float NANO_TO_SEC = 1000000000.0f; + + //dv = a * dt + integratedVelocity[0] += linearAcceleration[0] * timeDelta / NANO_TO_SEC; + integratedVelocity[1] += linearAcceleration[1] * timeDelta / NANO_TO_SEC; + integratedVelocity[2] += linearAcceleration[2] * timeDelta / NANO_TO_SEC; + + //dx = v * dt + integratedPosition[0] += integratedVelocity[0] * timeDelta / NANO_TO_SEC; + integratedPosition[1] += integratedVelocity[1] * timeDelta / NANO_TO_SEC; + integratedPosition[2] += integratedVelocity[2] * timeDelta / NANO_TO_SEC; + } + + public void onSensorChanged(SensorEvent event) { + switch (event.sensor.getType()) { + case Sensor.TYPE_ACCELEROMETER: + // alpha is calculated as t / (t + dT) + // with t, the low-pass filter's time-constant + // and dT, the event delivery rate + final float alpha = 0.8f; + + gravity[0] = alpha * gravity[0] + (1 - alpha) * event.values[0]; + gravity[1] = alpha * gravity[1] + (1 - alpha) * event.values[1]; + gravity[2] = alpha * gravity[2] + (1 - alpha) * event.values[2]; + + linearAcceleration[0] = event.values[0] - gravity[0]; + linearAcceleration[1] = event.values[1] - gravity[1]; + linearAcceleration[2] = event.values[2] - gravity[2]; + updateScreenOrientation(); + integrateSensors(); + break; + case Sensor.TYPE_MAGNETIC_FIELD: + geomagnetic = event.values; + updateScreenOrientation(); + integrateSensors(); + break; + } + } + + @Override + public void onAccuracyChanged(Sensor sensor, int i) { + + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/android/Util.java b/ftc-library/src/main/java/com/lasarobotics/library/android/Util.java index 94ad135..d7c078c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/android/Util.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/android/Util.java @@ -27,7 +27,7 @@ public static Context getContext() { return (Application) method.invoke(null, (Object[]) null); } catch (final java.lang.Throwable e) { // handle exception - return null; + throw new RuntimeException("Unable to get an application context."); } } @@ -41,7 +41,7 @@ public static String getDataDirectory(Context ctx) { try { return ctx.getPackageManager().getPackageInfo(ctx.getPackageName(), 0).applicationInfo.dataDir; } catch (PackageManager.NameNotFoundException e) { - return null; + throw new RuntimeException("Unable to find the current data directory.."); } } diff --git a/ftc-library/src/main/java/com/lasarobotics/library/controller/Controller.java b/ftc-library/src/main/java/com/lasarobotics/library/controller/Controller.java index 0c288b7..f5b3ebb 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/controller/Controller.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/controller/Controller.java @@ -55,11 +55,14 @@ public class Controller { @SerializedName("ry") public float right_stick_y; //right joystick Y axis + private boolean initialized = false; //true if the controller exists + /** * Initialize a blank controller */ public Controller() { reset(); + initialized = false; } /** @@ -89,35 +92,43 @@ public Controller(Controller another) { this.left_stick_y = another.left_stick_y; this.right_stick_x = another.right_stick_x; this.right_stick_y = another.right_stick_y; + initialized = true; } /** * Initialize a controller from a Gamepad (FIRST library underlayer) */ public Controller(Gamepad g) { - dpad_up = g.dpad_up ? ButtonState.HELD : ButtonState.NOT_PRESSED; - dpad_down = g.dpad_down ? ButtonState.HELD : ButtonState.NOT_PRESSED; - dpad_left = g.dpad_left ? ButtonState.HELD : ButtonState.NOT_PRESSED; - dpad_right = g.dpad_right ? ButtonState.HELD : ButtonState.NOT_PRESSED; - a = g.a ? ButtonState.HELD : ButtonState.NOT_PRESSED; - b = g.b ? ButtonState.HELD : ButtonState.NOT_PRESSED; - x = g.x ? ButtonState.HELD : ButtonState.NOT_PRESSED; - y = g.y ? ButtonState.HELD : ButtonState.NOT_PRESSED; - guide = g.guide ? ButtonState.HELD : ButtonState.NOT_PRESSED; - start = g.start ? ButtonState.HELD : ButtonState.NOT_PRESSED; - back = g.back ? ButtonState.HELD : ButtonState.NOT_PRESSED; - left_bumper = g.left_bumper ? ButtonState.HELD : ButtonState.NOT_PRESSED; - right_bumper = g.right_bumper ? ButtonState.HELD : ButtonState.NOT_PRESSED; - right_trigger = g.right_trigger; - left_stick_x = g.left_stick_x; - left_stick_y = g.left_stick_y; - right_stick_x = g.right_stick_x; - right_stick_y = g.right_stick_y; + try { + dpad_up = g.dpad_up ? ButtonState.HELD : ButtonState.NOT_PRESSED; + dpad_down = g.dpad_down ? ButtonState.HELD : ButtonState.NOT_PRESSED; + dpad_left = g.dpad_left ? ButtonState.HELD : ButtonState.NOT_PRESSED; + dpad_right = g.dpad_right ? ButtonState.HELD : ButtonState.NOT_PRESSED; + a = g.a ? ButtonState.HELD : ButtonState.NOT_PRESSED; + b = g.b ? ButtonState.HELD : ButtonState.NOT_PRESSED; + x = g.x ? ButtonState.HELD : ButtonState.NOT_PRESSED; + y = g.y ? ButtonState.HELD : ButtonState.NOT_PRESSED; + guide = g.guide ? ButtonState.HELD : ButtonState.NOT_PRESSED; + start = g.start ? ButtonState.HELD : ButtonState.NOT_PRESSED; + back = g.back ? ButtonState.HELD : ButtonState.NOT_PRESSED; + left_bumper = g.left_bumper ? ButtonState.HELD : ButtonState.NOT_PRESSED; + right_bumper = g.right_bumper ? ButtonState.HELD : ButtonState.NOT_PRESSED; + right_trigger = g.right_trigger; + left_stick_x = g.left_stick_x; + left_stick_y = g.left_stick_y; + right_stick_x = g.right_stick_x; + right_stick_y = g.right_stick_y; + initialized = true; + } catch (NullPointerException e) { + initialized = false; + reset(); + } } public static Controller getZeroController() { Controller a = new Controller(); a.reset(); + a.initialized = true; return a; } @@ -144,6 +155,7 @@ public static Controller getPressedController() { a.left_stick_y = 1.0f; a.right_stick_x = 1.0f; a.right_stick_y = 1.0f; + a.initialized = true; return a; } @@ -219,6 +231,15 @@ private int handleUpdate(Integer b, boolean updatedstatus) { } } + /** + * Tests whether the controller is connected to the driver station + * + * @return True if connected and no errors exist, false otherwise + */ + public boolean isConnected() { + return initialized; + } + @Override public String toString() { return "Controller{" + diff --git a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java new file mode 100644 index 0000000..358883c --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java @@ -0,0 +1,30 @@ +package com.lasarobotics.library.drive; + +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.util.Units; +import com.lasarobotics.library.util.Vector2; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * [NOT IMPLEMENTED] + * Drivetrain configuration information - tells where motors are placed in addition to + */ +public class DriveConfiguration { + + public static class LocatedMotor { + DcMotor motorStandard = null; + EncodedMotor motorEncoded = null; + + public LocatedMotor(DcMotor motor, Vector2 location, Units.Distance locationUnit) { + //TODO make sure motor is not null + } + + public LocatedMotor(EncodedMotor encodedMotor, Vector2 location, Units.Distance locationUnit) { + + } + + public boolean hasEncoder() { + return (motorEncoded != null); + } + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/drive/Drivetrain.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/Drivetrain.java new file mode 100644 index 0000000..b30b1d7 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/Drivetrain.java @@ -0,0 +1,30 @@ +package com.lasarobotics.library.drive; + +/** + * Drive train types and useful information about these drivetrains required by various classes + */ +public enum Drivetrain { + TANK(false), + MECANUM(true), + OTHER_OMNIDIRECTIONAL(true); + + boolean isOmnidirectional; + + Drivetrain(boolean isOmnidirectional) { + this.isOmnidirectional = isOmnidirectional; + } + + /** + * Test whether the selected drive train can strafe + * If true, the drive train can, in addition to basic tank capabilities: + * 1. Strafe (move sideways), and therefore can move in virtually any direction + * 2. Supports field oriented drive - generally true if 1. is true + * If this is NOT true, then it is assumed that the drive train also must + * rotate in order to move to a target not directly in front or behind it + * + * @return True if omnidirectional, false otherwise + */ + public boolean isOmnidirectional() { + return isOmnidirectional; + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java new file mode 100644 index 0000000..508f7d4 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -0,0 +1,207 @@ +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.util.Units; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorController; + +/** + * Drive encoder support + */ +public class EncodedMotor extends DcMotor { + protected double wheelRadius; //wheel radius in meters + private boolean isEncoderEnabled = true; + private boolean encodersResetting = false; + private boolean requestReset = false; + private int encoderOffset = 0; + private MotorInfo motorInfo; + + /** + * Initialize the encoded motor. You will need to run this during the init() method and run + * the update() method every loop(). + * + * @param motor DcMotor instance. + */ + public EncodedMotor(DcMotor motor, MotorInfo info) { + super(motor.getController(), motor.getPortNumber(), motor.getDirection()); + enableEncoder(); + requestReset(); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; + } + + protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info) { + super(controller, portNumber); + enableEncoder(); + requestReset(); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; + } + + protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info, Direction direction) { + super(controller, portNumber, direction); + enableEncoder(); + requestReset(); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; + } + + /** + * Move a certain distance, in encoder counts + * If the distance is negative, the wheel will move in reverse - forward otherwise + * + * It is assumed that there are 1440 encoder counts in a full rotation. + * + * @param distance The distance to move, in encoder counts + */ + public void moveDistance(double distance) { + if (distance == 0) + return; + + requestReset(); + + if (distance < 0) + super.setDirection(Direction.REVERSE); + else + super.setDirection(Direction.FORWARD); + + setTargetPosition(Math.abs((int) distance)); + } + + public void moveDistance(double distance, Units.Distance unit) { + moveDistance(convertDistanceToEncoderCounts(distance, unit)); + } + + /** + * Checks if the robot has reached the expected encoder position. + * After reaching the position, it is recommended to requestReset the encoders. + * + * @param position Expected position, in encoder counts + * @return True if the robot has reached the position + */ + public boolean hasReachedPosition(double position) { + return Math.abs(getCurrentPosition()) >= Math.abs(position); + } + + /** + * Test if the encoder has reached a specific position in a unit + * After reaching the position, it is recommended to requestReset the encoders. + * + * @param position Position, in units + * @param unit Unit of position + * @return True if reached the expected position, false otherwise + */ + public boolean hasReachedPosition(double position, Units.Distance unit) { + return Math.abs(getCurrentPosition(unit)) >= Math.abs(position); + } + + public void enableEncoder() { + //This command requires (at least ?) one execution loop to requestReset the encoders + setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + isEncoderEnabled = true; + } + + public void disableEncoder() { + //This command requires (at least ?) one execution loop to requestReset the encoders + setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS); + + isEncoderEnabled = false; + } + + public boolean isEncoderEnabled() { + return isEncoderEnabled; + } + + public void update() { + //Check for encoder requestReset + if (requestReset && !encodersResetting) + resetEncoder(); + if (encodersResetting && super.getCurrentPosition() == 0) { + encodersResetting = false; + encoderOffset = 0; + enableEncoder(); + } + } + + private void resetEncoder() { + //This command requires (at least ?) one execution loop to requestReset the encoders + setMode(DcMotorController.RunMode.RESET_ENCODERS); + + encodersResetting = true; + requestReset = false; + encoderOffset = -super.getCurrentPosition(); + } + + /** + * Reset encoder asynchronously + * + * (May take two frames for full effect) + */ + public void requestReset() { + if (encodersResetting) + return; + + requestReset = true; + } + + /** + * Reset encoder immediately + * (May take one frame for full effect) + */ + public void resetNow() { + resetEncoder(); + } + + public void setTargetPosition(double position, Units.Distance unit) { + setTargetPosition((int) convertDistanceToEncoderCounts(position, unit)); + } + + public MotorInfo getMotorInfo() { + return motorInfo; + } + + @Override + public int getTargetPosition() { + return super.getTargetPosition() - encoderOffset; + } + + @Override + public void setTargetPosition(int position) { + super.setTargetPosition(position + encoderOffset); + } + + public double getTargetPosition(Units.Distance unit) { + return convertEncoderCountsToDistance(getTargetPosition(), unit); + } + + public double convertDistanceToEncoderCounts(double distance, Units.Distance unit) { + return Units.Distance.convertToAngle(distance, wheelRadius, Units.Distance.METERS, + unit, Units.Angle.ENCODER_COUNTS); + } + + public double convertEncoderCountsToDistance(double encoderCounts, Units.Distance unit) { + return Units.Angle.convertToDistance(encoderCounts, wheelRadius, Units.Distance.METERS, + Units.Angle.ENCODER_COUNTS, unit); + } + + /** + * Tests if the encoder has requestReset. + * Note that encoders take at least one execution loop to fully requestReset. + * + * @return True for requestReset, false for not requestReset. + */ + public boolean hasEncoderReset() { + return getCurrentPosition() == 0; + } + + @Override + public int getCurrentPosition() { + if (encodersResetting) + return 0; + return super.getCurrentPosition() + encoderOffset; + } + + public double getCurrentPosition(Units.Distance unit) { + return convertEncoderCountsToDistance(getCurrentPosition(), unit); + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java new file mode 100644 index 0000000..60cf452 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java @@ -0,0 +1,227 @@ +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.util.Matrix; + +/** + * Kalman + *

    + * predict: + * X = F*X + H*U + * P = F*P*F^T + Q. + *

    + * Update: + * Y = M – H*X Called the innovation = measurement – state transformed by H. + * S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R + * K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance. + * X = X + K*Y Update with gain the new measurement + * P = (I – K * H) * P Update covariance to this time. + *

    + * Note: + * Derived classes need to perhaps hide certain matrixes to simplify the + * interface. Also perhaps override: setupFMatrix, predict, Reset or Update. + */ +public class KalmanFilter { + /** + * State matrix (X) + */ + protected Matrix m_x = new Matrix(); + /** + * Covariance matrix (P) + */ + protected Matrix m_p = new Matrix(); + /* + * Minimal covariance matrix (Q) + */ + protected Matrix m_q = new Matrix(); + /** + * Minimal innovative covariance, keeps filter from locking in to a solution (R) + */ + protected Matrix m_r = new Matrix(); + /** + * Converts m_x forward to a new time interval. + * Depending on what the states mean in m_x this could be anything. + * Often for 2D it is: 1, dt + * 0, 1 + * Because that would convert a 2 dimentional motion, velocity vector + * forward by time dt. + *

    + * For exampe, if acceleration was used instead of velocity + * we could instead have dt*dt instead of dt and thus have a 2nd order + * Kalman that estimated position with smooth accelerations. + */ + protected Matrix m_f = new Matrix(); + /** + * Converts measurement vector into state vector space. + */ + protected Matrix m_h = new Matrix(); + /** + * Apriori per update information. For example if we knew something + * moved at a specific velocity always we could just use U to add + * that in and decouple that from the other statistics addressed + * by the Kalman filter. + */ + protected Matrix m_u = new Matrix(); + private double lastGain; + + /** + * Last gain determinant, useful for debug + * @return Last gain determinant + */ + public double getLastGain() { + return lastGain; + } + + /** + * Get the state value at an index + */ + public double value(int index) { + return m_x.data[index]; + } + + /** + * Last updated value[0] variance. + * + * @return Variance at the first data point + */ + public double variance() { + return m_p.data[0]; + } + + /** + * Last updated variance at a specificindex + * + * @param index Index ro aearch for variance + * @return The variance at a specific index + */ + public double variance(int index) { + return m_p.get(index, index); + } + + /** + * Setup matrix F based on dt, the time between last update and this update. + * Default is for a rectangular time based: + * 1, dt, dt^2, dt^3, ... + * 0, 1, dt, dt^2, ... + * 0, 0, 1, dt, ... + * 0, 0, 0, 1, ... + * ... + * + * @param dt Time delta between samples + */ + public void setupFMatrix(double dt) { + m_f.zero(); + + for (int i = 0; i < m_f.getRows(); i++) { + double m = 1; + m_f.set(i, i, m); + for (int j = i + 1; j < m_f.getColumns(); j++) { + m *= dt; + m_f.set(i, i, m); + } + } + } + + /** + * Predict the most significant value forward from + * last measurement time by dt. + * X = F*X + H*U + * + * @param dt Time delta between measurements + * @return The predicted value at a time delta + */ + public double predict(double dt) { + setupFMatrix(dt); + Matrix tmp = Matrix.multiply(m_f, m_x); + Matrix tmp2 = Matrix.multiply(m_h, m_u); + tmp.add(tmp2); + return tmp.data[0]; + } + + /** + * Get the estimated covariance of position predicted + * forward from last measurement time by dt. + * P = F*P*F^T + Q. + * + * @param dt Time delta between measurements + * @return Approximate covariance at position predicted + */ + public double variance(double dt) { + setupFMatrix(dt); + Matrix tmp = Matrix.multiplyABAT(m_f, m_p); + tmp.add(m_q); + return tmp.data[0]; + } + + public void reset(Matrix q, Matrix r, Matrix p, Matrix x) { + m_q = q; + m_r = r; + m_p = p; + m_x = x; + } + + /** + * Update the state by measurement m at dt time from last measurement + * + * @param measurement Previous measurement data + * @param dt Time delta since last measuremment + * @return Latest estimate + */ + public double update(Matrix measurement, double dt) { + // Predict to now, then update. + // Predict: + // X = F*X + H*U + // P = F*P*F^T + Q. + // Update: + // Y = M – H*X Called the innovation = measurement – state transformed by H. + // S = H*P*H^T + R S= Residual covariance = covariane transformed by H + R + // K = P * H^T *S^-1 K = Kalman gain = variance / residual covariance. + // X = X + K*Y Update with gain the new measurement + // P = (I – K * H) * P Update covariance to this time. + // + // Same as 1D but mv is used instead of delta m_x[0], and H = [1,1]. + + // X = F*X + H*U + setupFMatrix(dt); + Matrix t1 = Matrix.multiply(m_f, m_x); + Matrix t2 = Matrix.multiply(m_h, m_u); + t1.add(t2); + m_x.set(t1); + + // P = F*P*F^T + Q + m_p = Matrix.multiplyABAT(m_f, m_p); + m_p.add(m_q); + + // Y = M – H*X + t1 = Matrix.multiply(m_h, m_x); + Matrix y = Matrix.subtract(measurement, t1); + + // S = H*P*H^T + R + Matrix s = Matrix.multiplyABAT(m_h, m_p); + s.add(m_r); + + // K = P * H^T *S^-1 + Matrix ht = Matrix.transpose(m_h); + Matrix tmp = Matrix.multiply(m_p, ht); + Matrix sinv = Matrix.invert(s); + Matrix k = new Matrix(y.getRows(), m_x.getRows()); + if (sinv != null) { + k = Matrix.multiply(tmp, sinv); + } + + lastGain = k.getDeterminant(); + + // X = X + K*Y + m_x.add(Matrix.multiply(k, y)); + + // P = (I – K * H) * P + Matrix kh = Matrix.multiply(k, m_h); + Matrix id = new Matrix(kh.getColumns(), kh.getRows()); + id.makeIdentity(); + id.subtract(kh); + id.multiply(m_p); + m_p.set(id); + + // Return latest estimate. + return m_x.data[0]; + } +} \ No newline at end of file diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java new file mode 100644 index 0000000..046e39f --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java @@ -0,0 +1,55 @@ +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.util.Units; + +/** + * Motor and axle info + */ +public class MotorInfo { + private double wheelRadius; //wheel radius in meters + private double mechanicalAdvantage; //mechanical advantage (2x = 2x effective radius) + + public MotorInfo(double encoderCountsPerUnit, Units.Distance distanceUnit) { + //encoderCountsPerUnit should be converted to encoder counts per meter + //(encoder counts / feet) * (feet / real encoder counts) + //(1000 / 1 foot) * (1 foot / 720 real counts) = radius of 1000/720 + + this.wheelRadius = distanceUnit.convertTo(Units.Distance.METERS, encoderCountsPerUnit) / Units.Angle.ENCODER_COUNTS.getConversionFactor(); + this.mechanicalAdvantage = 1; + } + + public MotorInfo(double wheelRadius, Units.Distance radiusUnits, double mechanicalAdvantage) { + this.wheelRadius = radiusUnits.convertTo(Units.Distance.METERS, wheelRadius); + this.mechanicalAdvantage = mechanicalAdvantage; + } + + public static MotorInfo mean(EncodedMotor[] motors) { + double effectiveRadius = 0.0; + for (EncodedMotor m : motors) + effectiveRadius += m.getMotorInfo().getEffectiveWheelRadius(Units.Distance.METERS); + effectiveRadius /= motors.length; + return new MotorInfo(effectiveRadius, Units.Distance.METERS); + } + + public static MotorInfo mean(EncodedMotor[] motors1, EncodedMotor[] motors2) { + double effectiveRadius = 0.0; + for (EncodedMotor m : motors1) + effectiveRadius += m.getMotorInfo().getEffectiveWheelRadius(Units.Distance.METERS); + for (EncodedMotor m : motors2) + effectiveRadius += m.getMotorInfo().getEffectiveWheelRadius(Units.Distance.METERS); + effectiveRadius /= motors1.length + motors2.length; + return new MotorInfo(effectiveRadius, Units.Distance.METERS); + } + + public double getWheelRadius(Units.Distance unit) { + return Units.Distance.METERS.convertTo(unit, wheelRadius); + } + + public double getMechanicalAdvantage() { + return mechanicalAdvantage; + } + + public double getEffectiveWheelRadius(Units.Distance unit) { + return getWheelRadius(unit) / mechanicalAdvantage; + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java new file mode 100644 index 0000000..b04ffef --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -0,0 +1,217 @@ +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; + +/** + * PID Targeting + */ +public class PID { + protected PIDCoefficients coefficients = new PIDCoefficients(0.005, 0, 0); // PID constant multipliers + protected double setpoint = 0; + protected double processValue = 0; // actual position (Process Value) + protected double error = 0; // how much SP and PV are diff (SP - PV) + protected double integral = 0; // curIntegral + (error * Delta Time) + protected double derivative = 0; //(error - prev error) / Delta time + protected double previousError = 0; // error from last time (previous Error) + protected double dt = 100.0; // delta time + protected double output = 0; // the drive amount that effects the PV. + protected double outputFlattened = 0; // drive power flattened by the accel and decel + protected double outputLast = 0; // last output power + protected double maxChangePositive = 0; // maximum change in output per second + protected double maxChangeNegative = 0; // maximum change in output per second + protected double minOutput = -1; // maximum change in output per second + protected double maxOutput = 1; // maximum change in output per second + protected double minPower = 0; // minimum power for anti-stall protection + + public PID() { + this.coefficients = new PIDCoefficients(0.05, 0, 0); + } + + public PID(double p, double i, double d) { + this.coefficients = new PIDCoefficients(p, i, d); + } + + public PID(PIDCoefficients coefficients) { + this.coefficients = coefficients; + } + + public void setCoefficients(double p, double i, double d) { + coefficients.p = p; + coefficients.i = i; + coefficients.d = d; + } + + public void addMeasurement(double measuredValue, double timeDelta) { + this.processValue = measuredValue; + this.dt = timeDelta; + update(); + } + + public double getSetpoint() { + return this.setpoint; + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + public void setSetpointDistance(MotorInfo motorInfo, double distance, Units.Distance distanceUnit) { + setSetpoint(Units.Distance.convertToAngle(distance, + motorInfo.getEffectiveWheelRadius(distanceUnit), + distanceUnit, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); + } + + /** + * Gets the most recent error - the difference between the setpoint and the measured value + * + * @return setpoint - measured value + */ + public double getError() { + return error; + } + + /** + * Get the integral value, or the sum of the recent error values + * + * @return Integral value I + */ + public double getIntegralValue() { + return integral; + } + + /** + * Get the derivative value, or how much the error is changing + * + * @return The slope of the error values + */ + public double getDerivativeValue() { + return derivative; + } + + /** + * Gets the error value from the previous measurement + * + * @return The previous error value + */ + public double getPreviousError() { + return previousError; + } + + public PIDCoefficients getCoefficients() { + return coefficients; + } + + public void setCoefficients(PIDCoefficients coefficients) { + coefficients.ff = 0.0; + this.coefficients = coefficients; + } + + public double getLastTimeDelta() { + return dt; + } + + public double getOutputValue() { + return outputFlattened; + } + + public double getOutputValueRaw() { + return output; + } + + public void setOutputRange(double min, double max) { + this.minOutput = min; + this.maxOutput = max; + } + + public double getMaxAcceleration() { + return maxChangePositive; + } + + public void setMaxAcceleration(double maxChange) { + this.maxChangePositive = maxChange; + } + + public double getMaxDeceleration() { + return maxChangeNegative; + } + + public void setMaxDeceleration(double maxChange) { + this.maxChangeNegative = Math.abs(maxChange); + } + + public double getAntistallMinimumPower() { + return minPower; + } + + public void setAntistallMinimumPower(double minPower) { + this.minPower = minPower; + } + + private void update() { + // calculate the difference between the desired value and the actual value + error = setpoint - processValue; + // track error over time, scaled to the timer interval + integral = integral + (error * dt); + // determin the amount of change from the last time checked + derivative = (error - previousError) / dt; + + // calculate how much drive the output in order to get to the + // desired setpoint. + output = (coefficients.p * error) + (coefficients.i * integral) + (coefficients.d * derivative); + + //Clamp output + output = MathUtil.coerce(minOutput, maxOutput, output); + outputFlattened = output; + + //Make sure the system does not move too fast + if ((outputFlattened - outputLast) > maxChangePositive * dt && maxChangePositive > 0) + outputFlattened = outputLast + (maxChangePositive * dt); + else if ((outputLast - outputFlattened) > maxChangeNegative * dt && maxChangeNegative > 0) + outputFlattened = outputLast - (maxChangeNegative * dt); + + //Clamp and deadband the flattened output + outputFlattened = MathUtil.deadband(minPower, MathUtil.coerce(minOutput, maxOutput, outputFlattened)); + + // remember the error and output for the next time around. + previousError = error; + outputLast = outputFlattened; + } + + public static class PIDCoefficients { + double p; + double i; + double d; + double ff; + + public PIDCoefficients(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + this.ff = 0.0; + } + + public PIDCoefficients(double p, double i, double d, double ff) { + this.p = p; + this.i = i; + this.d = d; + this.ff = ff; + } + + public double p() { + return p; + } + + public double i() { + return i; + } + + public double d() { + return d; + } + + public double ff() { + return ff; + } + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/Navigator.java new file mode 100644 index 0000000..72cd849 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/Navigator.java @@ -0,0 +1,41 @@ +package com.lasarobotics.library.nav.navigator; + +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.util.MathUtil; + +/** + * Smart navigation class that performs two operations: + * 1. Locates the robot on the field at any given time + * 2. Moves the robot to another point on the field + *

    + * Location is given through integration of the sensors. + *

    + * Movement can be set by: + * 1. A single point. The robot will move to this point immediately. + * 2. A path containing several points. The robot will move through the path + * as if it is a Bezier curve. + *

    + * Movement supports by 2D and 3D movement, as well as rotation. + *

    + * The Navigator currently supports only navX-based controllers. + *

    + *

    + * The current idea is to use: + * 1. the yaw-/z-axis PID controller as a precise rotation mechanism and self-correcting drive. + * 2. motor encoders to calculate distance traveled in the direction - more encoders give better positioning data + * Possibly even: + * 3. x and y accelorometers to measure (approximate) position + */ +abstract class Navigator implements NavXDataReceiver { + + protected NavXDevice navx; + + protected Navigator(NavXDevice navx) { + this.navx = navx; + } + + public static double coerceMotorValue(double value, double minValue) { + return MathUtil.deadband(minValue, MathUtil.coerce(-1, 1, value)); + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/TankNav.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/TankNav.java new file mode 100644 index 0000000..798dc7b --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/TankNav.java @@ -0,0 +1,328 @@ +package com.lasarobotics.library.nav.navigator; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.MotorInfo; +import com.lasarobotics.library.nav.PID; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; +import com.qualcomm.robotcore.robocol.Telemetry; + +/** + * TankDrive navigation + */ +public class TankNav extends Navigator { + EncodedMotor[] motorsLeft, motorsRight; + NavigationParams params; + NavXPIDController rotPID; + PID transPIDLeft; + PID transPIDRight; + NavXPIDController.PIDState rotState; + AsyncMotorResult lastResult; + long lastTime = System.nanoTime(); + Telemetry telemetry; + + public TankNav(NavXDevice navx, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight, Telemetry telemetry) { + super(navx); + this.motorsLeft = motorsLeft; + this.motorsRight = motorsRight; + this.telemetry = telemetry; + this.params = new NavigationParams(); + } + + public TankNav(NavXDevice navx, NavigationParams params, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight, Telemetry telemetry) { + super(navx); + this.motorsLeft = motorsLeft; + this.motorsRight = motorsRight; + this.telemetry = telemetry; + this.params = params; + } + + public void coast() { + for (EncodedMotor m : motorsLeft) + m.setPowerFloat(); + for (EncodedMotor m : motorsRight) + m.setPowerFloat(); + } + + public void stop() { + for (EncodedMotor m : motorsLeft) + m.setPower(0.0); + for (EncodedMotor m : motorsRight) + m.setPower(0.0); + } + + private double updateTime() { + long time = System.nanoTime(); + double dt = (time - lastTime) / 1000000000.0; + lastTime = time; + return dt; + } + + private void updateMotors() { + for (EncodedMotor m : motorsLeft) + m.update(); + for (EncodedMotor m : motorsRight) + m.update(); + } + + private double meanPos(EncodedMotor[] motors) { + double mean = 0.0; + for (EncodedMotor m : motors) + mean += m.getCurrentPosition(); + mean /= motors.length; + return mean; + } + + public void rotateInPlaceAsyncStart(double targetDegrees, double angleTolerance) { + lastResult = new AsyncMotorResult(); + navx.reset(); + try { + navx.waitForReset(); + } catch (InterruptedException e) { + e.printStackTrace(); + return; + } + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + rotPID = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + rotPID.setSetpoint(targetDegrees); + //Allow crossing over the bounds (see setContinuous() documentation) + rotPID.setContinuous(true); + //Set P,I,D coefficients + rotPID.setCoefficients(params.kRotMoving); + //Disable antistall (more accurate, and since this is only used for compensation, we can ignore the stalls) + rotPID.disableAntistall(); + //Making the tolerance very small makes the robot work hard to get to get to a very close estimate + if (angleTolerance > 0) + rotPID.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, angleTolerance); + else + rotPID.setTolerance(navXPIDController.ToleranceType.NONE, 0); + //Start data collection + rotPID.start(); + + //Initiate blank PID state + rotState = new NavXPIDController.PIDState(); + + updateTime(); + } + + public AsyncMotorResult rotateInPlaceAsyncRun(double powerFactor) { + double dt = updateTime(); + //updateMotors(); + + double power = 0.0; + power = rotPID.getOutputValue(); + lastResult = new AsyncMotorResult(-power * powerFactor, power * powerFactor, rotPID.isOnTarget()); + displayDebug(telemetry); + telemetry.addData("Motor Power", power * powerFactor); + return lastResult; + } + + public void rotateInPlace(double targetDegrees, double power) { + rotateInPlace(targetDegrees, power, 0, 0); + } + + public void rotateInPlace(double targetDegrees, double power, double distTolerance, double secTimeout) { + AsyncMotorResult result; + + rotateInPlaceAsyncStart(targetDegrees, distTolerance); + long t = System.nanoTime(); + while (true) { + result = rotateInPlaceAsyncRun(power); + if (result.isAtTarget() || + ((secTimeout > 0 && ((System.nanoTime() - t) / 1000000000.0 > secTimeout)))) + return; + t = System.nanoTime(); + for (EncodedMotor l : motorsLeft) + l.setPower(result.getLeftPower()); + for (EncodedMotor r : motorsRight) + r.setPower(result.getRightPower()); + try { + Thread.sleep(5); + } catch (InterruptedException e) { + e.printStackTrace(); + return; + } + } + } + + public void moveStabilizedAsyncStart(double distTarget, Units.Distance distUnit) { + lastResult = new AsyncMotorResult(); + navx.reset(); + try { + navx.waitForReset(); + } catch (InterruptedException e) { + e.printStackTrace(); + return; + } + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + rotPID = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + rotPID.setSetpoint(0.0); + //Allow crossing over the bounds (see setContinuous() documentation) + rotPID.setContinuous(true); + //Set P,I,D coefficients + rotPID.setCoefficients(params.kRotMoving); + //Disable antistall (more accurate, and since this is only used for compensation, we can ignore the stalls) + rotPID.disableAntistall(); + //Making the tolerance very small makes the robot work hard to get to get to a very close estimate + rotPID.setTolerance(navXPIDController.ToleranceType.NONE, 0); + //Start data collection + rotPID.start(); + + //Initiate blank PID state + rotState = new NavXPIDController.PIDState(); + + //Get mean (average) motor info + MotorInfo mean = MotorInfo.mean(motorsLeft, motorsRight); + + //Create translation PID + transPIDLeft = new PID(); + transPIDLeft.setSetpointDistance(mean, distTarget, distUnit); + transPIDLeft.setCoefficients(params.kTrans); + + transPIDRight = new PID(); + transPIDRight.setSetpointDistance(mean, distTarget, distUnit); + transPIDRight.setCoefficients(params.kTrans); + + updateTime(); + } + + public AsyncMotorResult moveStabilizedAsyncRun(double powerFactor, double distTolerance) { + double dt = updateTime(); + updateMotors(); + + transPIDLeft.addMeasurement(meanPos(motorsLeft), dt); + transPIDRight.addMeasurement(meanPos(motorsRight), dt); + + double power = 0.0; + double powerComp = 0.0; + + power = (transPIDLeft.getOutputValue() + transPIDRight.getOutputValue()) / 2.0; + powerComp = rotPID.getOutputValue(); + + double left, right; + left = MathUtil.coerce(-1, 1, power) - + Math.sin(MathUtil.coerce(-1, 1, powerComp * params.kRotMovingFerocity) * Math.PI / 2); + right = MathUtil.coerce(-1, 1, power) + + Math.sin(MathUtil.coerce(-1, 1, powerComp * params.kRotMovingFerocity) * Math.PI / 2); + + left = coerceMotorValue(left, params.kMinMotorPower); + right = coerceMotorValue(right, params.kMinMotorPower); + + lastResult = new AsyncMotorResult(left * powerFactor, right * powerFactor, + rotState.isOnTarget() + && ((distTolerance > 0) || (Math.abs(transPIDLeft.getError()) <= distTolerance) + && (Math.abs(transPIDRight.getError()) <= distTolerance))); + navx.displayTelemetry(telemetry); + return lastResult; + } + + public void moveStabilized(double distTarget, Units.Distance distUnit, double power) throws InterruptedException { + moveStabilized(distTarget, distUnit, power, 0, 0); + } + + public void moveStabilized(double distTarget, Units.Distance distUnit, double power, double distTolerance, double secTimeout) throws InterruptedException { + AsyncMotorResult result = new AsyncMotorResult(); + + moveStabilizedAsyncStart(distTarget, distUnit); + long t = System.nanoTime(); + while (true) { + result = moveStabilizedAsyncRun(power, distTolerance); + if (result.isAtTarget() || + ((secTimeout > 0 && ((System.nanoTime() - t) / 1000000000.0 > secTimeout)))) + break; + t = System.nanoTime(); + for (EncodedMotor l : motorsLeft) + l.setPower(result.getLeftPower()); + for (EncodedMotor r : motorsRight) + r.setPower(result.getRightPower()); + Thread.sleep(5); + } + } + + public void displayDebug(Telemetry telemetry) { + navx.displayTelemetry(telemetry); + telemetry.addData("RotPID Coefficients", rotPID.getCoefficientDebugString()); + } + + @Override + public void dataReceived(long timeDelta) { + + } + + public static class NavigationParams { + PID.PIDCoefficients kTrans = new PID.PIDCoefficients(0.0009, 0.0, 0.0); + PID.PIDCoefficients kRotStatic = new PID.PIDCoefficients(0.01, 0.0, 0.0005); + PID.PIDCoefficients kRotMoving = new PID.PIDCoefficients(0.005, 0.0, 0.0); + double kRotMovingFerocity = 4; + double kTransMaxAccel = 0.0; + double kTransMaxDecel = 0.0; + double kMinMotorPower = 0.00; + + NavigationParams() { + + } + + public void setTranslationPID(PID.PIDCoefficients c) { + kTrans = c; + } + + public void setStaticRotationPID(PID.PIDCoefficients c) { + kRotStatic = c; + } + + public void setMovingRotationPID(PID.PIDCoefficients c) { + kRotMoving = c; + } + + public void setRotationFerocity(double k) { + kRotMovingFerocity = k; + } + + public void setTranslationMaxAccel(double maxAccel, double maxDecel) { + kTransMaxAccel = maxAccel; + kTransMaxDecel = maxDecel; + } + + public void setMinimumMotorPower(double minPower) { + this.kMinMotorPower = minPower; + } + } + + public static class AsyncMotorResult { + double left, right; + boolean atTarget; + + AsyncMotorResult() { + this.left = 0; + this.right = 0; + this.atTarget = false; + } + + AsyncMotorResult(double left, double right, boolean atTarget) { + this.left = left; + this.right = right; + this.atTarget = atTarget; + } + + public boolean isAtTarget() { + return atTarget; + } + + public double getLeftPower() { + return left; + } + + public double getRightPower() { + return right; + } + } +} \ No newline at end of file diff --git a/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDataReceiver.java b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDataReceiver.java new file mode 100644 index 0000000..2b660d9 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDataReceiver.java @@ -0,0 +1,8 @@ +package com.lasarobotics.library.sensor.kauailabs.navx; + +/** + * Data receiver for the navX, allowing customized actions to be performed upon receiving data + */ +public interface NavXDataReceiver { + void dataReceived(long timeDelta); +} \ No newline at end of file diff --git a/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDevice.java b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDevice.java new file mode 100644 index 0000000..9f03a5e --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDevice.java @@ -0,0 +1,356 @@ +package com.lasarobotics.library.sensor.kauailabs.navx; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.IDataArrivalSubscriber; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Vector3; +import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.robocol.Telemetry; + +import java.security.InvalidParameterException; +import java.text.DecimalFormat; + +/** + * NavX MXP controller + */ +public class NavXDevice implements IDataArrivalSubscriber { + + static final DataType dataType = DataType.RAW_AND_PROCESSED; + static final int MAX_NUM_CALLBACKS = 3; + AHRS ahrs; + double last_world_linear_accel_x = 0.0; + double last_world_linear_accel_y = 0.0; + private double maxJerk = 0.0; + private long last_system_timestamp = 0; + private long last_sensor_timestamp = 0; + private double collisionThreshold = 0.5; + private NavXDataReceiver[] callbacks; + + /** + * Initialize a NavX MXP or NavX micro device + * + * @param map HardwareMap instance + * @param deviceInterfaceModuleName String name of the device interface module the sensor is on + * @param i2cPort The i2C port the sensor is currently on + */ + public NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort) { + initialize(map, deviceInterfaceModuleName, i2cPort, SensorSpeed.NORMAL_FAST, dataType); + } + + /** + * Initialize a NavX MXP or NavX micro device, and also set the sensor's speed + * + * @param map HardwareMap instance + * @param deviceInterfaceModuleName String name of the device interface module the sensor is on + * @param i2cPort The i2C port the sensor is currently on + * @param speed Sensor read speed (recommended anything from VERY_SLOW to VERY_FAST - other values are experimental) + */ + public NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort, SensorSpeed speed) { + initialize(map, deviceInterfaceModuleName, i2cPort, speed, DataType.PROCESSED_DATA); + } + + private void initialize(HardwareMap map, String deviceInterfaceModuleName, int i2cPort, SensorSpeed speed, DataType type) { + ahrs = AHRS.getInstance(map.deviceInterfaceModule.get(deviceInterfaceModuleName), + i2cPort, DataType.PROCESSED_DATA.getValue(), speed.getSpeedHertzByte()); + this.callbacks = new NavXDataReceiver[MAX_NUM_CALLBACKS]; + ahrs.registerCallback(this); + } + + public boolean isCalibrating() { + return ahrs.isCalibrating(); + } + + public boolean isCalibrated() { + return !ahrs.isCalibrating() && ahrs.isMagnetometerCalibrated(); + } + + public boolean isConnected() { + return ahrs.isConnected(); + } + + public boolean isMoving() { + return ahrs.isMoving(); + } + + public boolean isMagneticDisturbance() { + return ahrs.isMagneticDisturbance(); + } + + public boolean isRotating() { + return ahrs.isRotating() && !ahrs.isMagneticDisturbance(); + } + + public void reset() { + ahrs.zeroYaw(); + } + + public void waitForReset() throws InterruptedException { + //noinspection StatementWithEmptyBody + while (MathUtil.inBounds(-1, 1, this.getRotation().x)) { + + Thread.sleep(20); + } + } + + public void displayTelemetry(Telemetry telemetry) { + boolean connected = ahrs.isConnected(); + telemetry.addData("navX Status", connected ? + "Connected" : "Disconnected"); + String gyrocal, magcal, yaw, pitch, roll, compass_heading; + String fused_heading, ypr, cf, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + if (connected) { + gyrocal = (ahrs.isCalibrating() ? + "CALIBRATING" : "Calibration Complete"); + magcal = (ahrs.isMagnetometerCalibrated() ? + "Calibrated" : "UNCALIBRATED"); + yaw = df.format(ahrs.getYaw()); + pitch = df.format(ahrs.getPitch()); + roll = df.format(ahrs.getRoll()); + ypr = yaw + ", " + pitch + ", " + roll; + compass_heading = df.format(ahrs.getCompassHeading()); + fused_heading = df.format(ahrs.getFusedHeading()); + if (!ahrs.isMagnetometerCalibrated()) { + compass_heading = "-------"; + } + cf = compass_heading + ", " + fused_heading; + if (ahrs.isMagneticDisturbance()) { + cf += " (Mag. Disturbance)"; + } + motion = (ahrs.isMoving() ? "Moving" : "Not Moving"); + if (ahrs.isRotating()) { + motion += ", Rotating"; + } + + telemetry.addData("navX GyroAccel", gyrocal); + telemetry.addData("navX Y,P,R", ypr); + telemetry.addData("navX Magnetometer", magcal); + telemetry.addData("navX Compass,9Axis", cf); + telemetry.addData("navX Motion", motion); + } + } + + /** + * Get update frequency in Hertz + * + * @return Update rate in Hertzs + */ + public float getUpdateRate() { + return (float) ahrs.getActualUpdateRate(); + } + + /** + * Get the number of the current update + * + * @return The update count + */ + public int getUpdateCount() { + return (int) ahrs.getUpdateCount(); + } + + /** + * Get the ambient temperature, in degrees Celsius + * + * @return Ambient temperature, in degrees Celsius + */ + public float getTemperature() { + return ahrs.getTempC(); + } + + public Vector3 getLinearAcceleration() { + return new Vector3<>(ahrs.getWorldLinearAccelX(), ahrs.getWorldLinearAccelY(), ahrs.getWorldLinearAccelZ()); + } + + /** + * Gets the rotation vector from the processed gyroscope measurements + * + * @return YAW (x-axis), PITCH (y-axis), ROLL (z-axis) rotation in meters/second + */ + public Vector3 getRotation() { + return new Vector3<>(ahrs.getYaw(), ahrs.getPitch(), ahrs.getRoll()); + } + + /** + * Get the heading in degrees, from 0 to 360. + * + * @return The heading in degrees from 0 to 360. + */ + public float getHeading() { + return ahrs.getFusedHeading(); + } + + public void stop() { + ahrs.close(); + for (int i = 0; i < callbacks.length; i++) { + callbacks[i] = null; + } + } + + public double getCollisionThreshold() { + return collisionThreshold; + } + + public void setCollisionThreshold(double thresholdGs) { + if (thresholdGs <= 0.0) + throw new InvalidParameterException("Threshold must be greater than 0 Gs!"); + this.collisionThreshold = thresholdGs; + } + + /** + * Test whether the robot has collided - using the set or default threshold + * + * @return True if collided, false otherwise + */ + public boolean hasCollided() { + if (collisionThreshold <= 0.0) + throw new InvalidParameterException("Threshold must be greater than 0 Gs!"); + return maxJerk > collisionThreshold; + } + + /** + * Test whether the robot has collided + * + * @param thresholdGs Threshold for collision, in Gs. Default is 0.5 Gs. + * @return True if collided, false otherwise + */ + public boolean hasCollided(double thresholdGs) { + if (thresholdGs <= 0.0) + throw new InvalidParameterException("Threshold must be greater than 0 Gs!"); + return maxJerk > thresholdGs; + } + + /** + * Get the instantaneous jerk, in m/s^3. + * + * @return Jerk (derivative of acceleration, i.e. change in acceleration) in m/s^3. + */ + public double getJerk() { + return maxJerk; + } + + /** + * Registers a callback interface. This interface + * will be called back when new data is available, + * based upon a change in the sensor timestamp. + *

    + * Note that this callback will occur within the context of the + * device IO thread, which is not the same thread context the + * caller typically executes in. + */ + public boolean registerCallback(NavXDataReceiver callback) { + boolean registered = false; + for (int i = 0; i < this.callbacks.length; i++) { + if (this.callbacks[i] == null) { + this.callbacks[i] = callback; + registered = true; + break; + } + } + return registered; + } + + /** + * Deregisters a previously registered callback interface. + *

    + * Be sure to deregister any callback which have been + * previously registered, to ensure that the object + * implementing the callback interface does not continue + * to be accessed when no longer necessary. + */ + public boolean deregisterCallback(NavXDataReceiver callback) { + boolean deregistered = false; + for (int i = 0; i < this.callbacks.length; i++) { + if (this.callbacks[i] == callback) { + this.callbacks[i] = null; + deregistered = true; + break; + } + } + return deregistered; + } + + @Override + public void untimestampedDataReceived(long curr_system_timestamp, Object kind) { + long system_timestamp_delta = curr_system_timestamp - last_system_timestamp; + for (NavXDataReceiver callback : callbacks) { + if (callback != null) { + callback.dataReceived(system_timestamp_delta); + } + } + } + + @Override + public void timestampedDataReceived(long curr_system_timestamp, long curr_sensor_timestamp, Object kind) { + //Test for collisions + long sensor_timestamp_delta = curr_sensor_timestamp - last_sensor_timestamp; + long system_timestamp_delta = curr_system_timestamp - last_system_timestamp; + double curr_world_linear_accel_x = ahrs.getWorldLinearAccelX(); + double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x; + last_world_linear_accel_x = curr_world_linear_accel_x; + double curr_world_linear_accel_y = ahrs.getWorldLinearAccelY(); + double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y; + last_world_linear_accel_y = curr_world_linear_accel_y; + + maxJerk = Math.max(Math.abs(currentJerkX), Math.abs(currentJerkY)); + + last_sensor_timestamp = curr_sensor_timestamp; + last_system_timestamp = curr_system_timestamp; + + for (NavXDataReceiver callback : callbacks) { + if (callback != null) { + callback.dataReceived(sensor_timestamp_delta); + } + } + } + + + public enum DataType { + PROCESSED_DATA(AHRS.DeviceDataType.kProcessedData), + RAW_DATA(AHRS.DeviceDataType.kQuatAndRawData), + RAW_AND_PROCESSED(AHRS.DeviceDataType.kAll); + + private AHRS.DeviceDataType value; + + DataType(AHRS.DeviceDataType value) { + this.value = value; + } + + AHRS.DeviceDataType getValue() { + return value; + } + } + + /** + * NavX response speed + *

    + * Anything with "EXTREMELY" or "SLOWEST"/"FASTEST" is experimental. + */ + public enum SensorSpeed { + + SLOWEST(1.0), + EXTREMELY_SLOW(5.0), + VERY_SLOW(15.38), + SLOW(25.0), + NORMAL_SLOW(33.3), + NORMAL(40.0), + NORMAL_FAST(50.0), + VERY_FAST(66.6), + EXTREMELY_FAST(100.0), + FASTEST(200.0); + + double hertz; + + SensorSpeed(double hz) { + this.hertz = hz; + } + + public double getSpeedHertz() { + return hertz; + } + + public byte getSpeedHertzByte() { + return (byte) ((int) hertz); + } + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java new file mode 100644 index 0000000..085f78b --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java @@ -0,0 +1,192 @@ +package com.lasarobotics.library.sensor.kauailabs.navx; + +import android.util.Log; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.nav.PID; +import com.lasarobotics.library.util.MathUtil; + +import java.util.Locale; + +/** + * PID Controller designed for the navX MXP or micro + */ +public class NavXPIDController extends navXPIDController { + + private final static int waitTimeout = 50; //wait timeout in ms + private double antistallDeadband = 0.03; //minimum motor power - prevents stalls + private boolean enableAntistall = false; //true to enable antistall deadband + + public NavXPIDController(NavXDevice navX, DataSource dataSource) { + super(navX.ahrs, dataSource.val); + setPID(0.5, 0, 0, 0); + setOutputRange(-1.0, 1.0); + } + + public PID.PIDCoefficients getCoefficients() { + return new PID.PIDCoefficients(p, i, d, ff); + } + + public void setCoefficients(PID.PIDCoefficients c) { + this.p = c.p(); + this.i = c.i(); + this.d = c.d(); + this.ff = c.ff(); + } + + public String getCoefficientString() { + return "p: " + p + ", i: " + i + ", d: " + d + ", ff: " + ff; + } + + public String getCoefficientDebugString() { + return String.format(Locale.US, + "p: %3.4f i: %3.4f d: %3.4f", p * error_current, i * error_total, d * error_d); + } + + public void reset() { + super.reset(); + } + + public boolean isEnabled() { + return super.isEnabled(); + } + + public void start() { + super.enable(true); + } + + public void stop() { + super.enable(false); + } + + public void waitForUpdate(PIDState state) { + try { + if (!super.waitForNewUpdate(state, waitTimeout)) + throw new InterruptedException(); + } catch (InterruptedException e) { + Log.e("navX PID", "waitForUpdate() failed!"); + } + } + + public boolean isUpdateAvailable(PIDState state) { + boolean isAvailable = isNewUpdateAvailable(state); + if (isAvailable) { + state.output = MathUtil.deadband(antistallDeadband, state.output); + } + return isAvailable; + } + + /** + * Returns the latest output value, selected by the DataSource when creating the controller + * + * @return Latest output value as a double + */ + public double getOutputValue() { + return enableAntistall ? MathUtil.deadband(antistallDeadband, super.get()) : super.get(); + } + + public double getError() { + return super.getError(); + } + + public double getAntistallDeadband() { + return antistallDeadband; + } + + public void setAntistallDeadband(double minMotorPower) { + antistallDeadband = minMotorPower; + } + + public void setAntistall(boolean enableAntistall) { + this.enableAntistall = enableAntistall; + } + + public void enableAntistall() { + enableAntistall = true; + } + + public void disableAntistall() { + enableAntistall = false; + } + + public void setTolerance(ToleranceType toleranceType, double tolerance) { + super.setTolerance(toleranceType.val, tolerance); + } + + /** + * The data source specifies the + * sensor data source type used by the controller as it's input data source. + * These data sources are timestamped by the navX-Model device and thus are delivered + * with sufficient data (a highly-accurate "sensor timestamp") to allow the + * PID controller to compensate for any jitter in the transmission from the + * navX-Model device to the controller. + */ + public enum DataSource { + YAW(navXPIDController.navXTimestampedDataSource.YAW), + PITCH(navXPIDController.navXTimestampedDataSource.PITCH), + ROLL(navXPIDController.navXTimestampedDataSource.ROLL), + HEADING(navXPIDController.navXTimestampedDataSource.FUSED_HEADING), + LINEAR_ACCEL_X(navXPIDController.navXTimestampedDataSource.LINEAR_ACCEL_X), + LINEAR_ACCEL_Y(navXPIDController.navXTimestampedDataSource.LINEAR_ACCEL_Y), + LINEAR_ACCEL_Z(navXPIDController.navXTimestampedDataSource.LINEAR_ACCEL_Z); + + navXPIDController.navXTimestampedDataSource val; + + DataSource(navXPIDController.navXTimestampedDataSource val) { + this.val = val; + } + } + + /** + * The ToleranceType enumeration defines the type of tolerance to be used by the + * controller to determine whether the controller is "on_target". + */ + public enum ToleranceType { + NONE(navXPIDController.ToleranceType.NONE), + PERCENT(navXPIDController.ToleranceType.PERCENT), + ABSOLUTE(navXPIDController.ToleranceType.ABSOLUTE); + + navXPIDController.ToleranceType val; + + ToleranceType(navXPIDController.ToleranceType val) { + this.val = val; + } + } + + /** + * The PIDState class encapsulates the data used by the controller to + * communicate current state to a client of the controller. The client + * creates the instance of the PIDState, and continually provides it to the + * navxPIDController's waitForNewUpdate() and isNewDataAvailable() methods, + * depending upon whether the client wishes to block (wait) for new updates, + * or periodically poll to determine when new data is available. + */ + static public class PIDState extends navXPIDController.PIDResult { + + public PIDState() { + super(); + } + + /** + * Returns the timestamp of the last data sample processed. + */ + public long getTimestamp() { + return super.getTimestamp(); + } + + /** + * Returns true if the device arrived at the target location. + */ + public boolean isOnTarget() { + return super.isOnTarget(); + } + + /** + * Returns the output value calculated by the controller which + * corresponds to the most recent input data sample. + */ + public double getOutput() { + return super.getOutput(); + } + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/sensor/legacy/hitechnic/Gyroscope.java b/ftc-library/src/main/java/com/lasarobotics/library/sensor/legacy/hitechnic/Gyroscope.java index 231d4b2..459ac93 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/sensor/legacy/hitechnic/Gyroscope.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/legacy/hitechnic/Gyroscope.java @@ -54,7 +54,7 @@ public void update(GyroSensor g) { velCurr = getRotation(); dt = timers.getClockValue(clockName, TimeUnit.SECONDS); - heading += (velPrevious + velCurr) * .5D * dt; + heading += (velPrevious + velCurr) * .5D * dt * dt; //prepare for next values timers.resetClock(clockName); diff --git a/ftc-library/src/main/java/com/lasarobotics/library/skynet/EncodedMotor.java b/ftc-library/src/main/java/com/lasarobotics/library/skynet/EncodedMotor.java deleted file mode 100644 index 352a9ba..0000000 --- a/ftc-library/src/main/java/com/lasarobotics/library/skynet/EncodedMotor.java +++ /dev/null @@ -1,127 +0,0 @@ -package com.lasarobotics.library.skynet; - -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.hardware.DcMotorController; - -/** - * Drive encoder support - */ -public class EncodedMotor extends DcMotor { - private boolean isEncoderEnabled = true; - private boolean encodersResetting = false; - private int encoderOffset = 0; - - /** - * Initialize the encoded motor. You will need to run this during the init() method and run - * the update() method every loop(). - * - * @param motor DcMotor instance. - */ - public EncodedMotor(DcMotor motor) { - super(motor.getController(), motor.getPortNumber(), motor.getDirection()); - enableEncoder(); - } - - protected EncodedMotor(DcMotorController controller, int portNumber) { - super(controller, portNumber); - enableEncoder(); - } - - protected EncodedMotor(DcMotorController controller, int portNumber, Direction direction) { - super(controller, portNumber, direction); - enableEncoder(); - } - - /** - * Move a certain distance, in encoder counts - * If the distance is negative, the wheel will move in reverse - forward otherwise - * - * @param distance The distance to move, in encoder counts - */ - public void moveDistance(int distance) { - if (distance == 0) - return; - - resetEncoder(); - - if (distance < 0) - super.setDirection(Direction.REVERSE); - else - super.setDirection(Direction.FORWARD); - - setTargetPosition(distance); - } - - /** - * Checks if the robot has reached the expected encoder position. - * After reaching the position, it is recommended to reset the encoders. - * - * @param position Expected position, in encoder counts - * @return True if the robot has reached the position - */ - public boolean hasReachedPosition(double position) { - return Math.abs(getCurrentPosition()) > Math.abs(position); - } - - public void enableEncoder() { - //This command requires (at least ?) one execution loop to reset the encoders - super.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); - - isEncoderEnabled = true; - } - - public void disableEncoder() { - //This command requires (at least ?) one execution loop to reset the encoders - super.setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS); - - isEncoderEnabled = false; - } - - public boolean isEncoderEnabled() { - return isEncoderEnabled; - } - - public void update() { - //Check for encoder reset - if (encodersResetting && super.getCurrentPosition() == 0) { - encodersResetting = false; - encoderOffset = 0; - } - } - - /** - * Reset encoders. - */ - public void resetEncoder() { - if (encodersResetting) - return; - - //This command requires (at least ?) one execution loop to reset the encoders - super.setMode(DcMotorController.RunMode.RESET_ENCODERS); - - encodersResetting = true; - encoderOffset = -super.getCurrentPosition(); - } - - @Override - public void setTargetPosition(int position) { - super.setTargetPosition(position + encoderOffset); - } - - /** - * Tests if the encoder has reset. - * Note that encoders take at least one execution loop to fully reset. - * - * @return True for reset, false for not reset. - */ - public boolean hasEncoderReset() { - return getCurrentPosition() == 0; - } - - @Override - public int getCurrentPosition() { - if (encodersResetting) - return 0; - return super.getCurrentPosition() + encoderOffset; - } -} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/DistanceUnit.java b/ftc-library/src/main/java/com/lasarobotics/library/util/DistanceUnit.java deleted file mode 100644 index 654c203..0000000 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/DistanceUnit.java +++ /dev/null @@ -1,13 +0,0 @@ -package com.lasarobotics.library.util; - -/** - * Distance units for conversion - */ -public enum DistanceUnit { - ENCODER_COUNTS, - REVOLUTIONS, - INCHES, - FEET, - CENTIMETERS, - METERS -} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/Log.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Log.java index d4df8c1..1cb8b80 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/Log.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Log.java @@ -39,6 +39,26 @@ public void add(String tag, String data) { logEntries.add(new LogData(timers.getClockValue("log"), tag, data)); } + /** + * Add an entry to the log + * + * @param tag Tag associated with the data point + * @param data Data for log entry + */ + public void add(String tag, Double data) { + add(tag, Double.toString(data)); + } + + /** + * Add an entry to the log + * + * @param tag Tag associated with the data point + * @param data Data for log entry + */ + public void add(String tag, long data) { + add(tag, Long.toString(data)); + } + /** * Saves a log to fileName specified and with format specified *

    @@ -47,9 +67,20 @@ public void add(String tag, String data) { * @param fileType Format to write file in */ public void saveAs(FileType fileType) { + saveAs(fileType, false); + } + + /** + * Saves a log to fileName specified and with format specified + *

    + * This function will not overwrite an existing log file, but append ".1", ".2", etc. if it already exists + * + * @param fileType Format to write file in + */ + public void saveAs(FileType fileType, boolean overwrite) { try { //Use correct filename for requested file type - File f = Util.createFileOnDevice(fileDirectory, fileName + "." + fileType.toString(), false); + File f = Util.createFileOnDevice(fileDirectory, fileName + "." + fileType.toString(), overwrite); String out = ""; switch (fileType) { case JSON: diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/MathUtil.java b/ftc-library/src/main/java/com/lasarobotics/library/util/MathUtil.java index f174da0..edbcd03 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/MathUtil.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/MathUtil.java @@ -88,4 +88,80 @@ public static double coerce(double min, double max, double value) { public static boolean inBounds(double min, double max, double value) { return (value < max) && (value > min); } + + /** + * Normalize values to a specific normalizeTo value. + * The largest value is normalized to the normalizedTo value and the rest are multiplied + * by the same factor. + * For example, if given 2 and 4, normalized to 1, the result would be 0.5 and 1. + * Negatives will be treated as positive magnitudes. + * + * @param val1 First value + * @param val2 Second value + * @param normalizeUpOnly Only normalize if the factor is less than 1. In other words, + * if given 0.5 and normalize to 1, if true the result would be 0.5. + * If false, the result would 1. + * @param normalizeTo Value to normalize to + * @return Array of normalized values + */ + public static double[] normalize(double val1, double val2, double normalizeTo, boolean normalizeUpOnly) { + double max = Math.max(Math.abs(val1), Math.abs(val2)); + double factor = normalizeTo / max; + if (factor >= 1 && normalizeUpOnly) return new double[]{val1, val2}; + return new double[]{val1 * factor, val2 * factor}; + } + + /** + * Normalize values to a specific normalizeTo value. + * The largest value is normalized to the normalizedTo value and the rest are multiplied + * by the same factor. + * For example, if given 2 and 4, normalized to 1, the result would be 0.5 and 1. + * Negatives will be treated as positive magnitudes. + * + * @param vals List of values + * @param normalizeTo Value to normalize to + * @param normalizeUpOnly Only normalize if the factor is less than 1. In other words, + * if given 0.5 and normalize to 1, if true the result would be 0.5. + * If false, the result would 1. + * @return Array of normalized values + */ + public static double[] normalize(double[] vals, double normalizeTo, boolean normalizeUpOnly) { + double max = Double.MIN_VALUE; + for (double v : vals) + if (Math.abs(v) > max) max = Math.abs(v); + double factor = normalizeTo / max; + if (factor >= 1 && normalizeUpOnly) return vals; + double[] result = new double[vals.length - 1]; + for (int i = 0; i < vals.length; i++) + result[i] = vals[i] * factor; + return result; + } + + /** + * Sum an array of values + * + * @param vals Values to sum + * @return Sum of values + */ + public static double sum(double[] vals) { + double sum = 0.0; + for (double d : vals) { + sum += d; + } + return sum; + } + + /** + * Average an array of values + * + * @param vals Values to average + * @return Average of values + */ + public static double average(double[] vals) { + double sum = 0.0; + for (double d : vals) { + sum += d; + } + return sum / vals.length; + } } diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java new file mode 100644 index 0000000..4b9b78e --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java @@ -0,0 +1,407 @@ +package com.lasarobotics.library.util; + +public class Matrix { + /** + * Raw data + */ + public double[] data; + //Size + protected int columns = 0; + protected int rows = 0; + + public Matrix() { + data = null; + } + + public Matrix(int cols, int rows) { + resize(cols, rows); + } + + public Matrix(Matrix m) { + set(m); + } + + /** + * Multiply another matrix by a scalar + * + * @param m Matrix to multiply + * @param scalar Scalar to multiply + * @return Scalar-Matrix product + */ + public static Matrix multiply(Matrix m, double scalar) { + Matrix rv = m.Clone(); + rv.multiply(scalar); + return rv; + } + + /** + * Multiply two matrices + * + * @param a First matrix + * @param b Second matrix + * @return A * B + */ + public static Matrix multiply(Matrix a, Matrix b) { + Matrix rv = new Matrix(b.columns, a.rows); + int min = a.columns < b.rows ? a.columns : b.rows; + for (int i = 0; i < a.rows; i++) { + for (int j = 0; j < b.columns; j++) { + double s = 0; + for (int k = 0; k < min; k++) { + double av = a.get(k, i); + double bv = b.get(j, k); + s += av * bv; + } + rv.set(j, i, s); + } + } + return rv; + } + + /** + * A * B * A^T (transpose of A) + * Part of the process of matrix diagonalization and has other uses. + * + * @param a Matrix A + * @param b Matrix B + * @return A * B * A^T (transpose of A) + */ + public static Matrix multiplyABAT(Matrix a, Matrix b) { + Matrix rv = multiply(a, b); + Matrix t = Matrix.transpose(a); + rv.multiply(t); + return rv; + } + + /** + * Add a scalar to a matrix + * + * @param a Matrix + * @param scalar Scalar + * @return A + scalar + */ + public static Matrix add(Matrix a, double scalar) { + Matrix rv = new Matrix(a); + rv.add(scalar); + return rv; + } + + /** + * Add two matrices together (A + B) + * + * @param a First matrix + * @param b Second matrix + * @return A + B + */ + public static Matrix add(Matrix a, Matrix b) { + Matrix rv = new Matrix(a); + rv.add(b); + return rv; + } + + /** + * Subtract a scalar from a matrix + * + * @param a Matrix + * @param scalar Scalar + * @return A - scalar + */ + public static Matrix subtract(Matrix a, double scalar) { + Matrix rv = new Matrix(a); + rv.subtract(scalar); + return rv; + } + + /** + * Subtract two matrices from each other (A - B) + * + * @param a First matrix + * @param b Second matrix + * @return A - B + */ + public static Matrix subtract(Matrix a, Matrix b) { + Matrix rv = new Matrix(a); + rv.subtract(b); + return rv; + } + + + /** + * Get the trace of the index. Same as get(index, index). + * @param index Index to lookup trace + * @return Trace of the index + */ + //TODO this isn't the trace... someone doesn't know linear algebra ;) + /*public double trace(int index) + { + return get(index, index); + }*/ + + /** + * Transpose matrix m + * + * @param m Matrix to transpose + * @return Transposed matrix m (all rows -> columns, columns -> rows) + */ + public static Matrix transpose(Matrix m) { + Matrix rv = new Matrix(m.rows, m.columns); + for (int i = 0; i < m.columns; i++) { + for (int j = 0; j < m.rows; j++) { + rv.set(j, i, m.get(i, j)); + } + } + return rv; + } + + /** + * Invert a matrix m + * + * @param m Matrix to invert + * @return The inverted matrix + */ + public static Matrix invert(Matrix m) { + if (m.columns != m.rows) return null; + double det = m.getDeterminant(); + if (det == 0) return null; + + Matrix rv = new Matrix(m); + if (m.columns == 1) rv.data[0] = 1 / rv.data[0]; + det = 1 / det; + if (m.columns == 2) { + rv.data[0] = det * m.data[3]; + rv.data[3] = det * m.data[0]; + rv.data[1] = -det * m.data[2]; + rv.data[2] = -det * m.data[1]; + } + if (m.columns == 3) { + rv.data[0] = det * (m.data[8] * m.data[4]) - (m.data[7] * m.data[5]); + rv.data[1] = -det * (m.data[8] * m.data[1]) - (m.data[7] * m.data[2]); + rv.data[2] = det * (m.data[5] * m.data[1]) - (m.data[4] * m.data[2]); + + rv.data[3] = -det * (m.data[8] * m.data[3]) - (m.data[6] * m.data[5]); + rv.data[4] = det * (m.data[8] * m.data[0]) - (m.data[6] * m.data[2]); + rv.data[5] = -det * (m.data[5] * m.data[0]) - (m.data[3] * m.data[2]); + + rv.data[6] = det * (m.data[7] * m.data[3]) - (m.data[6] * m.data[4]); + rv.data[7] = -det * (m.data[7] * m.data[0]) - (m.data[6] * m.data[2]); + rv.data[8] = det * (m.data[4] * m.data[0]) - (m.data[3] * m.data[1]); + } + return rv; + } + + /** + * Set this matrix to the other matrix m. + * + * @param m Other matrix + */ + public void set(Matrix m) { + resize(m.columns, m.rows); + System.arraycopy(m.data, 0, data, 0, m.data.length); + } + + public int getColumns() { + return columns; + } + + public void setColumns(int columns) { + resize(columns, rows); + } + + public int getRows() { + return rows; + } + + public void setRows(int columns) { + resize(columns, rows); + } + + /** + * Resize the matrix + * + * @param cols Count of columns + * @param rows Count of rows + */ + public void resize(int cols, int rows) { + if ((columns == cols) && (this.rows == rows)) return; + columns = cols; + this.rows = rows; + data = new double[cols * rows]; + zero(); + } + + /** + * Clone this matrix to another + * + * @return The cloned matrix + */ + public Matrix Clone() { + Matrix m = new Matrix(); + m.resize(this.columns, this.rows); + System.arraycopy(data, 0, m.data, 0, data.length); + return m; + } + + /** + * Get a value from the matrix + * + * @param x Column location + * @param y Row location + * @return The value + */ + public double get(int x, int y) { + return data[x + y * columns]; + } + + /** + * Set the value at an index to the value of v + * + * @param x Column location + * @param y Row location + * @param v Value + */ + public void set(int x, int y, double v) { + data[x + (y * columns)] = v; + } + + /** + * Multiply the matrix by a scalar + * + * @param scalar Scalar to multiply the matrix by + */ + public void multiply(double scalar) { + for (int i = 0; i < data.length; i++) { + data[i] *= scalar; + } + } + + /** + * Multiply this matrix by another + * + * @param b Another matrix + */ + public void multiply(Matrix b) { + Matrix tmp = Matrix.multiply(this, b); + this.set(tmp); + } + + /** + * Add a scalar to this matrix + * + * @param scalar The scalar value + */ + public void add(double scalar) { + for (int i = 0; i < data.length; i++) { + data[i] += scalar; + } + } + + /** + * Add A to this matrix + * + * @param a Other matrix + */ + public void add(Matrix a) { + for (int i = 0; i < data.length; i++) { + data[i] += a.data[i]; + } + } + + /** + * Subtract a scalar from this matrix (returns this = this - scalar) + * + * @param scalar The scalar value + */ + public void subtract(double scalar) { + for (int i = 0; i < data.length; i++) { + data[i] -= scalar; + } + } + + /** + * Subtract A from this (returns this = this - A) + * + * @param a Other matrix + */ + public void subtract(Matrix a) { + for (int i = 0; i < data.length; i++) { + data[i] -= a.data[i]; + } + } + + /** + * Transpose this matrix. + */ + public void transpose() { + Matrix rv = new Matrix(this.rows, this.columns); + for (int i = 0; i < columns; i++) { + for (int j = 0; j < rows; j++) { + rv.set(j, i, this.get(i, j)); + } + } + this.set(rv); + } + + /** + * Test if this matrix is the identity matrix + * + * @return True if this matrix is an identity, false otherwise + */ + public boolean isIdentity() { + if (columns != rows) return false; + int check = columns + 1; + int j = 0; + for (double aData : data) { + if (j == check) { + j = 0; + if (aData != 1) return false; + } else { + if (aData != 0) return false; + } + j++; + } + return true; + } + + /** + * Set this matrix as an identity matrix. + * Will fail if rows != columns. + */ + public void makeIdentity() { + if (columns != rows) return; + int check = columns + 1; + int j = 0; + for (int i = 0; i < data.length; i++) { + data[i] = (j == check) ? 1 : 0; + j = j == check ? 1 : j + 1; + } + } + + /** + * Zero the matrix + */ + public void zero() { + for (int i = 0; i < data.length; i++) { + data[i] = 0; + } + } + + /** + * Get the determinant of the matrix + * + * @return The determinant of the matrix + */ + public double getDeterminant() { + if (columns != rows) return 0; + + if (columns == 0) return 0; + if (columns == 1) return data[0]; + if (columns == 2) return (data[0] * data[3]) - (data[1] * data[2]); + if (columns == 3) return + (data[0] * ((data[8] * data[4]) - (data[7] * data[5]))) - + (data[3] * ((data[8] * data[1]) - (data[7] * data[2]))) + + (data[6] * ((data[5] * data[1]) - (data[4] * data[2]))); + + // only supporting 1x1, 2x2 and 3x3 + return 0; + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/ScreenOrientation.java b/ftc-library/src/main/java/com/lasarobotics/library/util/ScreenOrientation.java new file mode 100644 index 0000000..8e716c6 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/ScreenOrientation.java @@ -0,0 +1,63 @@ +package com.lasarobotics.library.util; + +import android.view.Surface; + +/** + * Rotation relative to the horizontal positive x-axis (east) and increasing counterclockwise + */ +public enum ScreenOrientation { + LANDSCAPE(0), + PORTRAIT(90), + LANDSCAPE_WEST(180), + PORTRAIT_REVERSE(270); + + private final double angle; + + ScreenOrientation(double angle) { + this.angle = angle; + } + + public static ScreenOrientation getFromAngle(double angle) { + return getFromAngle((int) angle); + } + + public static ScreenOrientation getFromAngle(int angle) { + while (angle > 360) + angle -= 360; + while (angle < 0) + angle += 360; + + switch (angle) { + case 0: + case 360: + return LANDSCAPE; + case 90: + return PORTRAIT; + case 180: + return LANDSCAPE_WEST; + case 270: + return PORTRAIT_REVERSE; + default: + throw new RuntimeException("The input angle must be a multiple of 90 degrees!"); + } + } + + public static ScreenOrientation getFromSurface(int id) { + switch (id) { + case Surface.ROTATION_0: + return LANDSCAPE; + case Surface.ROTATION_90: + return PORTRAIT; + case Surface.ROTATION_180: + return LANDSCAPE_WEST; + case Surface.ROTATION_270: + return PORTRAIT_REVERSE; + default: + return LANDSCAPE; + } + } + + public double getAngle() { + return angle; + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java new file mode 100644 index 0000000..836ceb4 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java @@ -0,0 +1,139 @@ +package com.lasarobotics.library.util; + +/** + * Units for conversion + */ +public class Units { + + public enum Distance implements Unit { + INCHES(0.0254), + FEET(0.3048), + CENTIMETERS(1.0 / 100.0), + METERS(1); //default unit + + protected double conversionFactor; + + Distance(double conversionFactor) { + this.conversionFactor = conversionFactor; + } + + /** + * Convert a unit of angle into a unit of distance + * + * @param value Angle value + * @param wheelRadius Wheel radius + * @param radiusUnits Units of wheel radius + * @param convertFrom Unit, in Angle, of value + * @param convertTo Unit, in Distance, of value + * @return Arc length, or distance the wheel will travel after moving value angles + */ + public static double convertToAngle(double value, double wheelRadius, + Units.Distance radiusUnits, Units.Distance convertFrom, + Units.Angle convertTo) { + //1 radian = 1 arc length = 1 Circumference / 2PI + //Distance Traveled = radians * wheel radius in meters * conversion unit + //Radians = Distance traveled / wheel radius in meters / conversion unit + //Radians = m traveled / m radius + + //s = r * theta + //theta = s / r + double revolutions = convertFrom.convertTo(Distance.METERS, value) / + radiusUnits.convertTo(Distance.METERS, wheelRadius); + return Angle.REVOLUTIONS.convertTo(convertTo, revolutions); + } + + @Override + public double getConversionFactor() { + return conversionFactor; + } + + public double convertTo(Distance other, double value) { + value *= conversionFactor; + value /= other.getConversionFactor(); + return value; + } + + @Override + public Unit getDefaultUnit() { + return METERS; + } + } + + public enum Angle implements Unit { + DEGREES(1), + RADIANS(180.0 / Math.PI), + REVOLUTIONS(360.0), + ENCODER_COUNTS(2.0); //encoders have 1440 resolutions / rotation, so 1 count = 1/4 degree + // (encoder counts updated to 2 for new motors) + + protected double conversionFactor; + + Angle(double conversionFactor) { + this.conversionFactor = conversionFactor; + } + + /** + * Convert a unit of angle into a unit of distance + * + * @param value Angle value + * @param wheelRadius Wheel radius + * @param radiusUnits Units of wheel radius + * @param convertFrom Unit, in Angle, of value + * @param convertTo Unit, in Distance, of value + * @return Arc length, or distance the wheel will travel after moving value angles + */ + public static double convertToDistance(double value, double wheelRadius, + Units.Distance radiusUnits, Units.Angle convertFrom, + Units.Distance convertTo) { + //1 radian = 1 arc length = 1 Circumference / 2PI + //Distance Traveled = radians * wheel radius in meters * conversion unit + //s = r * theta + + double revolutions = convertFrom.convertTo(Angle.REVOLUTIONS, value); + return radiusUnits.convertTo(convertTo, revolutions * wheelRadius); + + //return convertFrom.convertTo(Units.Angle.RADIANS, value) * wheelRadius * + // radiusUnits.convertTo(convertTo, 1); + } + + @Override + public double getConversionFactor() { + return conversionFactor; + } + + public double convertTo(Angle other, double value) { + value *= conversionFactor; + value /= other.getConversionFactor(); + return value; + } + + public Angle getDefaultUnit() { + return DEGREES; + } + } + + private interface Unit { + /** + * Default unit. All conversion factors are relative to converting to this unit + * + * @return Default unit + */ + Unit getDefaultUnit(); + + /** + * Get conversion factor to the default unit + * + * @return Conversion factor + */ + double getConversionFactor(); + + /** + * Convert a value to another unit. + * + * @param other Another units + * @param value Value to convert + * @return Converted value, if possible + */ + //double convertTo(Unit other, double value); + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/Vector2.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Vector2.java new file mode 100644 index 0000000..a53bb23 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Vector2.java @@ -0,0 +1,38 @@ +package com.lasarobotics.library.util; + +/** + * 3D Vector : Immutable + */ +public class Vector2 { + public final T x; + public final T y; + + public Vector2(T x, T y) { + this.x = x; + this.y = y; + } + + @Override + public String toString() { + return "(" + x + ", " + y + ")"; + } + + public static class Builder { + private T x, y; + + public Builder x(T n) { + this.x = n; + return this; + } + + public Builder y(T n) { + this.y = n; + return this; + } + + // Illegal State Exception errors can be thrown if x or y is null + public Vector2 build() { + return new Vector2(x, y); + } + } +} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/util/Vector3.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Vector3.java index 10e26b7..b63c1ec 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/Vector3.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Vector3.java @@ -3,7 +3,7 @@ /** * 3D Vector : Immutable */ -public class Vector3 { +public class Vector3 { public final T x; public final T y; public final T z; @@ -19,7 +19,7 @@ public String toString() { return "(" + x + ", " + y + ", " + z + ")"; } - public static class Builder { + public static class Builder { private T x, y, z; public Builder x(T n) { diff --git a/lib-navx/.gitignore b/lib-navx/.gitignore new file mode 100644 index 0000000..796b96d --- /dev/null +++ b/lib-navx/.gitignore @@ -0,0 +1 @@ +/build diff --git a/lib-navx/build.gradle b/lib-navx/build.gradle new file mode 100644 index 0000000..9785681 --- /dev/null +++ b/lib-navx/build.gradle @@ -0,0 +1,47 @@ +apply plugin: 'com.android.library' +buildscript { + repositories { + jcenter() + } + dependencies { + classpath 'com.android.tools.build:gradle:1.3.0' + } +} +android { + compileSdkVersion 21 + buildToolsVersion "21.1.2" + + defaultConfig { + minSdkVersion 16 + targetSdkVersion 21 + } + + testOptions { + unitTests.returnDefaultValues = true + } +} + +repositories { + flatDir { + dirs = ['../libs'] + } +} + + +allprojects { + repositories { + jcenter() + flatDir { + dirs 'out' + } + } +} + +dependencies { + compile fileTree(include: ['*.jar'], dir: 'libs') + compile files('libs/android-support-v4.jar') + compile(name: 'RobotCore-release', ext: 'aar') + //compile(name: 'ModernRobotics-release', ext: 'aar') + compile(name: 'Hardware-release', ext: 'aar') + //compile(name: 'FtcCommon-release', ext: 'aar') +} diff --git a/lib-navx/src/main/AndroidManifest.xml b/lib-navx/src/main/AndroidManifest.xml new file mode 100644 index 0000000..718ba33 --- /dev/null +++ b/lib-navx/src/main/AndroidManifest.xml @@ -0,0 +1,10 @@ + + + + + + + diff --git a/lib-navx/src/main/java/com/kauailabs/navx/AHRSProtocol.java b/lib-navx/src/main/java/com/kauailabs/navx/AHRSProtocol.java new file mode 100644 index 0000000..7d4637c --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/AHRSProtocol.java @@ -0,0 +1,786 @@ +/* ============================================ + NavX-MXP source code is placed under the MIT license + Copyright (c) 2015 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ + +package com.kauailabs.navx; + + +public class AHRSProtocol extends IMUProtocol { + + /* NAVX_CAL_STATUS */ + + public static final byte NAVX_CAL_STATUS_IMU_CAL_STATE_MASK = 0x03; + public static final byte NAVX_CAL_STATUS_IMU_CAL_INPROGRESS = 0x00; + public static final byte NAVX_CAL_STATUS_IMU_CAL_ACCUMULATE = 0x01; + public static final byte NAVX_CAL_STATUS_IMU_CAL_COMPLETE = 0x02; + + public static final byte NAVX_CAL_STATUS_MAG_CAL_COMPLETE = 0x04; + public static final byte NAVX_CAL_STATUS_BARO_CAL_COMPLETE = 0x08; + + /* NAVX_SELFTEST_STATUS */ + + public static final byte NAVX_SELFTEST_STATUS_COMPLETE = (byte) 0x80; + + public static final byte NAVX_SELFTEST_RESULT_GYRO_PASSED = 0x01; + public static final byte NAVX_SELFTEST_RESULT_ACCEL_PASSED = 0x02; + public static final byte NAVX_SELFTEST_RESULT_MAG_PASSED = 0x04; + public static final byte NAVX_SELFTEST_RESULT_BARO_PASSED = 0x08; + + /* NAVX_OP_STATUS */ + + public static final byte NAVX_OP_STATUS_INITIALIZING = 0x00; + public static final byte NAVX_OP_STATUS_SELFTEST_IN_PROGRESS = 0x01; + public static final byte NAVX_OP_STATUS_ERROR = 0x02; + public static final byte NAVX_OP_STATUS_IMU_AUTOCAL_IN_PROGRESS = 0x03; + public static final byte NAVX_OP_STATUS_NORMAL = 0x04; + + /* NAVX_SENSOR_STATUS */ + public static final byte NAVX_SENSOR_STATUS_MOVING = 0x01; + public static final byte NAVX_SENSOR_STATUS_YAW_STABLE = 0x02; + public static final byte NAVX_SENSOR_STATUS_MAG_DISTURBANCE = 0x04; + public static final byte NAVX_SENSOR_STATUS_ALTITUDE_VALID = 0x08; + public static final byte NAVX_SENSOR_STATUS_SEALEVEL_PRESS_SET = 0x10; + public static final byte NAVX_SENSOR_STATUS_FUSED_HEADING_VALID = 0x20; + + /* NAVX_REG_CAPABILITY_FLAGS (Aligned w/NAV6 Flags, see IMUProtocol.h) */ + + public static final short NAVX_CAPABILITY_FLAG_OMNIMOUNT = 0x0004; + public static final short NAVX_CAPABILITY_FLAG_OMNIMOUNT_CONFIG_MASK = 0x0038; + public static final short NAVX_CAPABILITY_FLAG_VEL_AND_DISP = 0x0040; + public static final short NAVX_CAPABILITY_FLAG_YAW_RESET = 0x0080; + + /* NAVX_OMNIMOUNT_CONFIG */ + + public static final byte OMNIMOUNT_DEFAULT = 0; /* Same as Y_Z_UP */ + public static final byte OMNIMOUNT_YAW_X_UP = 1; + public static final byte OMNIMOUNT_YAW_X_DOWN = 2; + public static final byte OMNIMOUNT_YAW_Y_UP = 3; + public static final byte OMNIMOUNT_YAW_Y_DOWN = 4; + public static final byte OMNIMOUNT_YAW_Z_UP = 5; + public static final byte OMNIMOUNT_YAW_Z_DOWN = 6; + + /* NAVX_INTEGRATION_CTL */ + + public static final byte NAVX_INTEGRATION_CTL_RESET_VEL_X = 0x01; + public static final byte NAVX_INTEGRATION_CTL_RESET_VEL_Y = 0x02; + public static final byte NAVX_INTEGRATION_CTL_RESET_VEL_Z = 0x04; + public static final byte NAVX_INTEGRATION_CTL_RESET_DISP_X = 0x08; + public static final byte NAVX_INTEGRATION_CTL_RESET_DISP_Y = 0x10; + public static final byte NAVX_INTEGRATION_CTL_RESET_DISP_Z = 0x20; + public static final byte NAVX_INTEGRATION_CTL_RESET_YAW = (byte) 0x80; + public final static char BINARY_PACKET_INDICATOR_CHAR = '#'; + public final static byte MSGID_AHRS_UPDATE = 'a'; + public final static byte MSGID_AHRSPOS_UPDATE = 'p'; + // Data Get Request: Tuning Variable, Mag Cal, Board Identity (Response message depends upon request type) + public final static byte MSGID_DATA_REQUEST = 'D'; + + /* AHRS Protocol encodes certain data in binary format, unlike the IMU */ + /* protocol, which encodes all data in ASCII characters. Thus, the */ + /* packet start and message termination sequences may occur within the */ + /* message content itself. To support the binary format, the binary */ + /* message has this format: */ + /* */ + /* [start][binary indicator][len][msgid][checksum][terminator] */ + /* */ + /* (The binary indicator and len are not present in the ASCII protocol) */ + /* */ + /* The [len] does not include the length of the start and binary */ + /* indicator characters, but does include all other message items, */ + /* including the checksum and terminator sequence. */ + // Data Set Response Packet + public final static byte MSGID_DATA_SET_RESPONSE = 'v'; + /* Integration Control Command Packet */ + public final static byte MSGID_INTEGRATION_CONTROL_CMD = 'I'; + /* Integration Control Response Packet */ + public final static byte MSGID_INTEGRATION_CONTROL_RESP = 'i'; + // Magnetometer Calibration Packet - e.g., !m[x_bias][y_bias][z_bias][m1,1 ... m3,3][cr][lf] + public final static byte MSGID_MAG_CAL_CMD = 'M'; + // Tuning Variable Packet + public final static byte MSGID_FUSION_TUNING_CMD = 'T'; + // Board Identity Response Packet- e.g., !c[type][hw_rev][fw_major][fw_minor][unique_id[12]] + public final static byte MSGID_BOARD_IDENTITY_RESPONSE = 'i'; + final static int AHRS_UPDATE_YAW_VALUE_INDEX = 4; /* Degrees. Signed Hundredths */ + final static int AHRS_UPDATE_PITCH_VALUE_INDEX = 6; /* Degrees. Signed Hundredeths */ + final static int AHRS_UPDATE_ROLL_VALUE_INDEX = 8; /* Degrees. Signed Hundredths */ + final static int AHRS_UPDATE_HEADING_VALUE_INDEX = 10; /* Degrees. Unsigned Hundredths */ + final static int AHRS_UPDATE_ALTITUDE_VALUE_INDEX = 12; /* Meters. Signed 16:16 */ + final static int AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX = 16; /* Degrees. Unsigned Hundredths */ + final static int AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX = 18; /* Inst. G. Signed Thousandths */ + final static int AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX = 20; /* Inst. G. Signed Thousandths */ + final static int AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX = 22; /* Inst. G. Signed Thousandths */ + final static int AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX = 24; /* Int16 (Device Units) */ + final static int AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX = 26; /* Int16 (Device Units) */ + final static int AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX = 28; /* Int16 (Device Units) */ + final static int AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX = 30; /* Ratio. Unsigned Hundredths */ + final static int AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX = 32; /* Coefficient. Signed 16:16 */ + final static int AHRS_UPDATE_MPU_TEMP_VAUE_INDEX = 36; /* Centigrade. Signed Hundredths */ + final static int AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX = 38; /* INT16 (Device Units */ + final static int AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX = 40; /* INT16 (Device Units */ + final static int AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX = 42; /* INT16 (Device Units */ + final static int AHRS_UPDATE_QUAT_W_VALUE_INDEX = 44; /* INT16 */ + final static int AHRS_UPDATE_QUAT_X_VALUE_INDEX = 46; /* INT16 */ + final static int AHRS_UPDATE_QUAT_Y_VALUE_INDEX = 48; /* INT16 */ + final static int AHRS_UPDATE_QUAT_Z_VALUE_INDEX = 50; /* INT16 */ + final static int AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX = 52; /* millibar. Signed 16:16 */ + final static int AHRS_UPDATE_BARO_TEMP_VAUE_INDEX = 56; /* Centigrade. Signed Hundredths */ + final static int AHRS_UPDATE_OPSTATUS_VALUE_INDEX = 58; /* NAVX_OP_STATUS_XXX */ + final static int AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX = 59; /* NAVX_SENSOR_STATUS_XXX */ + + // AHRSAndPositioning Update Packet (similar to AHRS, but removes magnetometer and adds velocity/displacement) */ + final static int AHRS_UPDATE_CAL_STATUS_VALUE_INDEX = 60; /* NAVX_CAL_STATUS_XXX */ + final static int AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX = 61; /* NAVX_SELFTEST_STATUS_XXX */ + final static int AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX = 62; + final static int AHRS_UPDATE_MESSAGE_TERMINATOR_INDEX = 64; + final static int AHRS_UPDATE_MESSAGE_LENGTH = 66; + final static int AHRSPOS_UPDATE_YAW_VALUE_INDEX = 4; /* Degrees. Signed Hundredths */ + final static int AHRSPOS_UPDATE_PITCH_VALUE_INDEX = 6; /* Degrees. Signed Hundredeths */ + final static int AHRSPOS_UPDATE_ROLL_VALUE_INDEX = 8; /* Degrees. Signed Hundredths */ + final static int AHRSPOS_UPDATE_HEADING_VALUE_INDEX = 10; /* Degrees. Unsigned Hundredths */ + final static int AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX = 12; /* Meters. Signed 16:16 */ + final static int AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX = 16; /* Degrees. Unsigned Hundredths */ + final static int AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX = 18; /* Inst. G. Signed Thousandths */ + final static int AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX = 20; /* Inst. G. Signed Thousandths */ + final static int AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX = 22; /* Inst. G. Signed Thousandths */ + final static int AHRSPOS_UPDATE_VEL_X_VALUE_INDEX = 24; /* Signed 16:16, in meters/sec */ + final static int AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX = 28; /* Signed 16:16, in meters/sec */ + final static int AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX = 32; /* Signed 16:16, in meters/sec */ + final static int AHRSPOS_UPDATE_DISP_X_VALUE_INDEX = 36; /* Signed 16:16, in meters */ + final static int AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX = 40; /* Signed 16:16, in meters */ + final static int AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX = 44; /* Signed 16:16, in meters */ + final static int AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX = 48; /* INT16 */ + final static int AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX = 50; /* INT16 */ + final static int AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX = 52; /* INT16 */ + final static int AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX = 54; /* INT16 */ + final static int AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX = 56; /* Centigrade. Signed Hundredths */ + final static int AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX = 58; /* NAVX_OP_STATUS_XXX */ + final static int AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX = 59; /* NAVX_SENSOR_STATUS_XXX */ + final static int AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX = 60; /* NAVX_CAL_STATUS_XXX */ + final static int AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX = 61; /* NAVX_SELFTEST_STATUS_XXX */ + final static int AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX = 62; + final static int AHRSPOS_UPDATE_MESSAGE_TERMINATOR_INDEX = 64; + final static int AHRSPOS_UPDATE_MESSAGE_LENGTH = 66; + public final static int MAX_BINARY_MESSAGE_LENGTH = AHRSPOS_UPDATE_MESSAGE_LENGTH; + final static int DATA_REQUEST_DATATYPE_VALUE_INDEX = 4; + final static int DATA_REQUEST_VARIABLEID_VALUE_INDEX = 5; + final static int DATA_REQUEST_CHECKSUM_INDEX = 6; + final static int DATA_REQUEST_TERMINATOR_INDEX = 8; + final static int DATA_REQUEST_MESSAGE_LENGTH = 10; + final static int DATA_SET_RESPONSE_DATATYPE_VALUE_INDEX = 4; + final static int DATA_SET_RESPONSE_VARID_VALUE_INDEX = 5; + final static int DATA_SET_RESPONSE_STATUS_VALUE_INDEX = 6; + final static int DATA_SET_RESPONSE_MESSAGE_CHECKSUM_INDEX = 7; + final static int DATA_SET_RESPONSE_MESSAGE_TERMINATOR_INDEX = 9; + final static int DATA_SET_RESPONSE_MESSAGE_LENGTH = 11; + final static int INTEGRATION_CONTROL_CMD_ACTION_INDEX = 4; + final static int INTEGRATION_CONTROL_CMD_PARAMETER_INDEX = 5; + final static int INTEGRATION_CONTROL_CMD_MESSAGE_CHECKSUM_INDEX = 9; + final static int INTEGRATION_CONTROL_CMD_MESSAGE_TERMINATOR_INDEX = 11; + final static int INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH = 13; + final static int INTEGRATION_CONTROL_RESP_ACTION_INDEX = 4; + final static int INTEGRATION_CONTROL_RESP_PARAMETER_INDEX = 5; + final static int INTEGRATION_CONTROL_RESP_MESSAGE_CHECKSUM_INDEX = 9; + final static int INTEGRATION_CONTROL_RESP_MESSAGE_TERMINATOR_INDEX = 11; + final static int INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH = 13; + final static int MAG_CAL_DATA_ACTION_VALUE_INDEX = 4; + final static int MAG_X_BIAS_VALUE_INDEX = 5; /* signed short */ + final static int MAG_Y_BIAS_VALUE_INDEX = 7; + final static int MAG_Z_BIAS_VALUE_INDEX = 9; + final static int MAG_XFORM_1_1_VALUE_INDEX = 11; /* signed 16:16 */ + final static int MAG_XFORM_1_2_VALUE_INDEX = 15; + final static int MAG_XFORM_1_3_VALUE_INDEX = 19; + final static int MAG_XFORM_2_1_VALUE_INDEX = 23; + final static int MAG_XFORM_2_2_VALUE_INDEX = 25; + final static int MAG_XFORM_2_3_VALUE_INDEX = 31; + final static int MAG_XFORM_3_1_VALUE_INDEX = 35; + final static int MAG_XFORM_3_2_VALUE_INDEX = 39; + final static int MAG_XFORM_3_3_VALUE_INDEX = 43; + final static int MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX = 47; + final static int MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX = 51; + final static int MAG_CAL_CMD_MESSAGE_TERMINATOR_INDEX = 53; + final static int MAG_CAL_CMD_MESSAGE_LENGTH = 55; + final static int FUSION_TUNING_DATA_ACTION_VALUE_INDEX = 4; + final static int FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX = 5; + final static int FUSION_TUNING_CMD_VAR_VALUE_INDEX = 6; + final static int FUSION_TUNING_CMD_MESSAGE_CHECKSUM_INDEX = 10; + final static int FUSION_TUNING_CMD_MESSAGE_TERMINATOR_INDEX = 12; + final static int FUSION_TUNING_CMD_MESSAGE_LENGTH = 14; + final static int BOARD_IDENTITY_BOARDTYPE_VALUE_INDEX = 4; + final static int BOARD_IDENTITY_HWREV_VALUE_INDEX = 5; + final static int BOARD_IDENTITY_FW_VER_MAJOR = 6; + final static int BOARD_IDENTITY_FW_VER_MINOR = 7; + final static int BOARD_IDENTITY_FW_VER_REVISION_VALUE_INDEX = 8; + final static int BOARD_IDENTITY_UNIQUE_ID_0 = 10; + final static int BOARD_IDENTITY_UNIQUE_ID_1 = 11; + final static int BOARD_IDENTITY_UNIQUE_ID_2 = 12; + final static int BOARD_IDENTITY_UNIQUE_ID_3 = 13; + final static int BOARD_IDENTITY_UNIQUE_ID_4 = 14; + final static int BOARD_IDENTITY_UNIQUE_ID_5 = 15; + final static int BOARD_IDENTITY_UNIQUE_ID_6 = 16; + final static int BOARD_IDENTITY_UNIQUE_ID_7 = 17; + final static int BOARD_IDENTITY_UNIQUE_ID_8 = 18; + final static int BOARD_IDENTITY_UNIQUE_ID_9 = 19; + final static int BOARD_IDENTITY_UNIQUE_ID_10 = 20; + final static int BOARD_IDENTITY_UNIQUE_ID_11 = 21; + final static int BOARD_IDENTITY_RESPONSE_CHECKSUM_INDEX = 22; + final static int BOARD_IDENTITY_RESPONSE_TERMINATOR_INDEX = 24; + final static int BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH = 26; + static final int CRC7_POLY = 0x0091; + + public static int decodeAHRSUpdate(byte[] buffer, + int offset, + int length, + AHRSUpdate u) { + if (length < AHRS_UPDATE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == PACKET_START_CHAR) && + (buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[offset + 2] == AHRS_UPDATE_MESSAGE_LENGTH - 2) && + (buffer[offset + 3] == MSGID_AHRS_UPDATE)) { + + if (!verifyChecksum(buffer, offset, AHRS_UPDATE_MESSAGE_CHECKSUM_INDEX)) { + return 0; + } + u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_YAW_VALUE_INDEX); + u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_PITCH_VALUE_INDEX); + u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_ROLL_VALUE_INDEX); + u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_HEADING_VALUE_INDEX); + u.altitude = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_ALTITUDE_VALUE_INDEX); + u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_FUSED_HEADING_VALUE_INDEX); + u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX); + u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX); + u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX); + u.cal_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_X_VALUE_INDEX); + u.cal_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Y_VALUE_INDEX); + u.cal_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_CAL_MAG_Z_VALUE_INDEX); + u.mag_field_norm_ratio = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRS_UPDATE_CAL_MAG_NORM_RATIO_VALUE_INDEX); + u.mag_field_norm_scalar = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_CAL_MAG_SCALAR_VALUE_INDEX); + u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_MPU_TEMP_VAUE_INDEX); + u.raw_mag_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_X_VALUE_INDEX); + u.raw_mag_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Y_VALUE_INDEX); + u.raw_mag_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_RAW_MAG_Z_VALUE_INDEX); + u.quat_w = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_W_VALUE_INDEX); + u.quat_x = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_X_VALUE_INDEX); + u.quat_y = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Y_VALUE_INDEX); + u.quat_z = decodeBinaryInt16(buffer, offset + AHRS_UPDATE_QUAT_Z_VALUE_INDEX); + u.barometric_pressure = decodeProtocol1616Float(buffer, offset + AHRS_UPDATE_BARO_PRESSURE_VALUE_INDEX); + u.baro_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRS_UPDATE_BARO_TEMP_VAUE_INDEX); + u.op_status = buffer[AHRS_UPDATE_OPSTATUS_VALUE_INDEX]; + u.sensor_status = buffer[AHRS_UPDATE_SENSOR_STATUS_VALUE_INDEX]; + u.cal_status = buffer[AHRS_UPDATE_CAL_STATUS_VALUE_INDEX]; + u.selftest_status = buffer[AHRS_UPDATE_SELFTEST_STATUS_VALUE_INDEX]; + return AHRS_UPDATE_MESSAGE_LENGTH; + } + return 0; + } + + public static int decodeAHRSPosUpdate(byte[] buffer, + int offset, + int length, + AHRSPosUpdate u) { + if (length < AHRSPOS_UPDATE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == PACKET_START_CHAR) && + (buffer[offset + 1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[offset + 2] == AHRSPOS_UPDATE_MESSAGE_LENGTH - 2) && + (buffer[offset + 3] == MSGID_AHRSPOS_UPDATE)) { + + if (!verifyChecksum(buffer, offset, AHRSPOS_UPDATE_MESSAGE_CHECKSUM_INDEX)) { + return 0; + } + u.yaw = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_YAW_VALUE_INDEX); + u.pitch = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_PITCH_VALUE_INDEX); + u.roll = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_ROLL_VALUE_INDEX); + u.compass_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_HEADING_VALUE_INDEX); + u.altitude = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_ALTITUDE_VALUE_INDEX); + u.fused_heading = decodeProtocolUnsignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_FUSED_HEADING_VALUE_INDEX); + u.linear_accel_x = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_X_VALUE_INDEX); + u.linear_accel_y = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Y_VALUE_INDEX); + u.linear_accel_z = decodeProtocolSignedThousandthsFloat(buffer, offset + AHRSPOS_UPDATE_LINEAR_ACCEL_Z_VALUE_INDEX); + u.vel_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_X_VALUE_INDEX); + u.vel_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Y_VALUE_INDEX); + u.vel_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_VEL_Z_VALUE_INDEX); + u.disp_x = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_X_VALUE_INDEX); + u.disp_y = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Y_VALUE_INDEX); + u.disp_z = decodeProtocol1616Float(buffer, offset + AHRSPOS_UPDATE_DISP_Z_VALUE_INDEX); + u.mpu_temp = decodeProtocolSignedHundredthsFloat(buffer, offset + AHRSPOS_UPDATE_MPU_TEMP_VAUE_INDEX); + u.quat_w = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_W_VALUE_INDEX); + u.quat_x = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_X_VALUE_INDEX); + u.quat_y = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Y_VALUE_INDEX); + u.quat_z = decodeBinaryInt16(buffer, offset + AHRSPOS_UPDATE_QUAT_Z_VALUE_INDEX); + u.op_status = buffer[AHRSPOS_UPDATE_OPSTATUS_VALUE_INDEX]; + u.sensor_status = buffer[AHRSPOS_UPDATE_SENSOR_STATUS_VALUE_INDEX]; + u.cal_status = buffer[AHRSPOS_UPDATE_CAL_STATUS_VALUE_INDEX]; + u.selftest_status = buffer[AHRSPOS_UPDATE_SELFTEST_STATUS_VALUE_INDEX]; + return AHRSPOS_UPDATE_MESSAGE_LENGTH; + } + return 0; + } + + /* Mag Cal, Tuning Variable, or Board ID Retrieval Request */ + public static int encodeDataGetRequest(byte[] buffer, + byte type, + byte var_id) { + // Header + buffer[0] = PACKET_START_CHAR; + buffer[1] = BINARY_PACKET_INDICATOR_CHAR; + buffer[2] = DATA_REQUEST_MESSAGE_LENGTH - 2; + buffer[3] = MSGID_DATA_REQUEST; + // Data + buffer[DATA_REQUEST_DATATYPE_VALUE_INDEX] = type; + buffer[DATA_REQUEST_VARIABLEID_VALUE_INDEX] = var_id; + // Footer + encodeTermination(buffer, DATA_REQUEST_MESSAGE_LENGTH, DATA_REQUEST_MESSAGE_LENGTH - 4); + return DATA_REQUEST_MESSAGE_LENGTH; + } + + /* Mag Cal Data Storage Request */ + public static int encodeMagCalDataSetRequest(byte[] buffer, + MagCalData d) { + // Header + buffer[0] = PACKET_START_CHAR; + buffer[1] = BINARY_PACKET_INDICATOR_CHAR; + buffer[2] = MAG_CAL_CMD_MESSAGE_LENGTH - 2; + buffer[3] = MSGID_MAG_CAL_CMD; + + // Data + buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX] = d.action; + for (int i = 0; i < 3; i++) { + encodeBinaryInt16(d.mag_bias[i], + buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2)); + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + encodeProtocol1616Float(d.mag_xform[i][j], + buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2)); + } + } + encodeProtocol1616Float(d.earth_mag_field_norm, buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX); + // Footer + encodeTermination(buffer, MAG_CAL_CMD_MESSAGE_LENGTH, MAG_CAL_CMD_MESSAGE_LENGTH - 4); + return MAG_CAL_CMD_MESSAGE_LENGTH; + } + + /* Mag Cal Data Retrieval Response */ + public static int decodeMagCalDataGetResponse(byte[] buffer, + int offset, + int length, + MagCalData d) { + if (length < MAG_CAL_CMD_MESSAGE_LENGTH) return 0; + if ((buffer[0] == PACKET_START_CHAR) && + (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[2] == MAG_CAL_CMD_MESSAGE_LENGTH - 2) && + (buffer[3] == MSGID_MAG_CAL_CMD)) { + if (!verifyChecksum(buffer, offset, MAG_CAL_CMD_MESSAGE_CHECKSUM_INDEX)) return 0; + + d.action = buffer[MAG_CAL_DATA_ACTION_VALUE_INDEX]; + for (int i = 0; i < 3; i++) { + d.mag_bias[i] = decodeBinaryInt16(buffer, MAG_X_BIAS_VALUE_INDEX + (i * 2)); + } + for (int i = 0; i < 3; i++) { + for (int j = 0; j < 3; j++) { + d.mag_xform[i][j] = decodeProtocol1616Float(buffer, MAG_XFORM_1_1_VALUE_INDEX + (i * 6) + (j * 2)); + } + } + d.earth_mag_field_norm = decodeProtocol1616Float(buffer, MAG_CAL_EARTH_MAG_FIELD_NORM_VALUE_INDEX); + return MAG_CAL_CMD_MESSAGE_LENGTH; + } + return 0; + } + + /* Tuning Variable Storage Request */ + public static int encodeTuningVarSetRequest(byte[] buffer, + TuningVar r) { + // Header + buffer[0] = PACKET_START_CHAR; + buffer[1] = BINARY_PACKET_INDICATOR_CHAR; + buffer[2] = FUSION_TUNING_CMD_MESSAGE_LENGTH - 2; + buffer[3] = MSGID_FUSION_TUNING_CMD; + // Data + buffer[FUSION_TUNING_DATA_ACTION_VALUE_INDEX] = r.action; + buffer[FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX] = r.var_id; + encodeProtocol1616Float(r.value, buffer, FUSION_TUNING_CMD_VAR_VALUE_INDEX); + // Footer + encodeTermination(buffer, FUSION_TUNING_CMD_MESSAGE_LENGTH, FUSION_TUNING_CMD_MESSAGE_LENGTH - 4); + return FUSION_TUNING_CMD_MESSAGE_LENGTH; + } + + /* Tuning Variable Retrieval Response */ + public static int decodeTuningVarGetResponse(byte[] buffer, + int offset, + int length, + TuningVar r) { + if (length < FUSION_TUNING_CMD_MESSAGE_LENGTH) return 0; + if ((buffer[0] == PACKET_START_CHAR) && + (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[2] == FUSION_TUNING_CMD_MESSAGE_LENGTH - 2) && + (buffer[3] == MSGID_FUSION_TUNING_CMD)) { + if (!verifyChecksum(buffer, offset, FUSION_TUNING_CMD_MESSAGE_CHECKSUM_INDEX)) return 0; + + // Data + r.action = buffer[FUSION_TUNING_DATA_ACTION_VALUE_INDEX]; + r.var_id = buffer[FUSION_TUNING_CMD_VAR_ID_VALUE_INDEX]; + r.value = decodeProtocol1616Float(buffer, FUSION_TUNING_CMD_VAR_VALUE_INDEX); + return FUSION_TUNING_CMD_MESSAGE_LENGTH; + } + return 0; + } + + public static int encodeIntegrationControlCmd(byte[] buffer, IntegrationControl u) { + // Header + buffer[0] = PACKET_START_CHAR; + buffer[1] = BINARY_PACKET_INDICATOR_CHAR; + buffer[2] = INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH - 2; + buffer[3] = MSGID_INTEGRATION_CONTROL_CMD; + // Data + buffer[INTEGRATION_CONTROL_CMD_ACTION_INDEX] = u.action; + encodeBinaryUint32(u.parameter, buffer, INTEGRATION_CONTROL_CMD_PARAMETER_INDEX); + // Footer + encodeTermination(buffer, INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH, INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH - 4); + return INTEGRATION_CONTROL_CMD_MESSAGE_LENGTH; + } + + public static int decodeIntegrationControlResponse(byte[] buffer, int offset, int length, IntegrationControl u) { + if (length < INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH) return 0; + if ((buffer[0] == PACKET_START_CHAR) && + (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[2] == INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH - 2) && + (buffer[3] == MSGID_INTEGRATION_CONTROL_RESP)) { + if (!verifyChecksum(buffer, offset, INTEGRATION_CONTROL_RESP_MESSAGE_CHECKSUM_INDEX)) + return 0; + + // Data + u.action = buffer[INTEGRATION_CONTROL_RESP_ACTION_INDEX]; + u.parameter = decodeBinaryUint32(buffer, INTEGRATION_CONTROL_RESP_PARAMETER_INDEX); + return INTEGRATION_CONTROL_RESP_MESSAGE_LENGTH; + } + return 0; + } + + /* MagCal or Tuning Variable Storage Response */ + public static int decodeDataSetResponse(byte[] buffer, + int offset, + int length, + DataSetResponse d) { + if (length < DATA_SET_RESPONSE_MESSAGE_LENGTH) return 0; + if ((buffer[0] == PACKET_START_CHAR) && + (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[2] == DATA_SET_RESPONSE_MESSAGE_LENGTH - 2) && + (buffer[3] == MSGID_DATA_SET_RESPONSE)) { + if (!verifyChecksum(buffer, offset, DATA_SET_RESPONSE_MESSAGE_CHECKSUM_INDEX)) return 0; + + d.data_type = buffer[DATA_SET_RESPONSE_DATATYPE_VALUE_INDEX]; + d.var_id = buffer[DATA_SET_RESPONSE_VARID_VALUE_INDEX]; + d.status = buffer[DATA_SET_RESPONSE_STATUS_VALUE_INDEX]; + return DATA_SET_RESPONSE_MESSAGE_LENGTH; + } + return 0; + } + + /* Board ID Retrieval Response */ + public static int decodeBoardIDGetResponse(byte[] buffer, + int offset, + int length, + BoardID id) { + if (length < BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH) return 0; + if ((buffer[0] == PACKET_START_CHAR) && + (buffer[1] == BINARY_PACKET_INDICATOR_CHAR) && + (buffer[2] == BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH - 2) && + (buffer[3] == MSGID_BOARD_IDENTITY_RESPONSE)) { + if (!verifyChecksum(buffer, offset, BOARD_IDENTITY_RESPONSE_CHECKSUM_INDEX)) return 0; + id.type = buffer[BOARD_IDENTITY_BOARDTYPE_VALUE_INDEX]; + id.hw_rev = buffer[BOARD_IDENTITY_HWREV_VALUE_INDEX]; + id.fw_ver_major = buffer[BOARD_IDENTITY_FW_VER_MAJOR]; + id.fw_ver_minor = buffer[BOARD_IDENTITY_FW_VER_MINOR]; + id.fw_revision = decodeBinaryUint16(buffer, BOARD_IDENTITY_FW_VER_REVISION_VALUE_INDEX); + for (int i = 0; i < 12; i++) { + id.unique_id[i] = buffer[BOARD_IDENTITY_UNIQUE_ID_0 + i]; + } + return BOARD_IDENTITY_RESPONSE_MESSAGE_LENGTH; + } + return 0; + } + + /* protocol data is encoded little endian, convert to Java's big endian format */ + public static short decodeBinaryUint16(byte[] buffer, int offset) { + short lowbyte = (short) (((short) buffer[offset]) & 0xff); + short highbyte = (short) buffer[offset + 1]; + highbyte <<= 8; + short decoded_value = (short) (highbyte + lowbyte); + return decoded_value; + } + + public static void encodeBinaryUint16(short val, byte[] buffer, int offset) { + buffer[offset + 0] = (byte) (val & 0xFF); + buffer[offset + 1] = (byte) ((val >> 8) & 0xFF); + } + + public static int decodeBinaryUint32(byte[] buffer, int offset) { + int lowlowbyte = (((int) buffer[offset]) & 0xff); + int lowhighbyte = (((int) buffer[offset + 1]) & 0xff); + int highlowbyte = (((int) buffer[offset + 2]) & 0xff); + int highhighbyte = (((int) buffer[offset + 3])); + + lowhighbyte <<= 8; + highlowbyte <<= 16; + highhighbyte <<= 24; + + int result = highhighbyte + highlowbyte + lowhighbyte + lowlowbyte; + return result; + } + + public static void encodeBinaryUint32(int val, byte[] buffer, int offset) { + buffer[offset + 0] = (byte) (val & 0xFF); + buffer[offset + 1] = (byte) ((val >> 8) & 0xFF); + buffer[offset + 2] = (byte) ((val >> 16) & 0xFF); + buffer[offset + 3] = (byte) ((val >> 24) & 0xFF); + } + + public static short decodeBinaryInt16(byte[] buffer, int offset) { + return decodeBinaryUint16(buffer, offset); + } + + public static void encodeBinaryInt16(short val, byte[] buffer, int offset) { + encodeBinaryUint16(val, buffer, offset); + } + + /* -327.68 to +327.68 */ + public static float decodeProtocolSignedHundredthsFloat(byte[] buffer, int offset) { + float signed_angle = (float) decodeBinaryUint16(buffer, offset); + signed_angle /= 100; + return signed_angle; + } + + public static void encodeProtocolSignedHundredthsFloat(float input, byte[] buffer, int offset) { + short input_as_int = (short) (input * 100); + encodeBinaryInt16(input_as_int, buffer, offset); + } + + public static short encodeSignedHundredthsFloat(float input) { + return (short) (input * 100); + } + + public static short encodeUnsignedHundredthsFloat(float input) { + return (short) (input * 100); + } + + public static float encodeRatioFloat(float input_ratio) { + return input_ratio *= 32768; + } + + public static float encodeSignedThousandthsFloat(float input) { + return input * 1000; + } + + /* 0 to 655.35 */ + public static float decodeProtocolUnsignedHundredthsFloat(byte[] buffer, int offset) { + int uint16 = (int) decodeBinaryUint16(buffer, offset); + if (uint16 < 0) { + uint16 += 65536; + } + float unsigned_float = (float) uint16; + unsigned_float /= 100; + return unsigned_float; + } + + public static void encodeProtocolUnsignedHundredthsFloat(float input, byte[] buffer, int offset) { + short input_as_uint = (short) (input * 100); + encodeBinaryUint16(input_as_uint, buffer, offset); + } + + /* -32.768 to +32.768 */ + public static float decodeProtocolSignedThousandthsFloat(byte[] buffer, int offset) { + float signed_angle = (float) decodeBinaryUint16(buffer, offset); + signed_angle /= 1000; + return signed_angle; + } + + public static void encodeProtocolSignedThousandthsFloat(float input, byte[] buffer, int offset) { + short input_as_int = (short) (input * 1000); + encodeBinaryInt16(input_as_int, buffer, offset); + } + + /* In units of -1 to 1, multiplied by 16384 */ + public static float decodeProtocolRatio(byte[] buffer, int offset) { + float ratio = (float) decodeBinaryUint16(buffer, offset); + ratio /= 32768; + return ratio; + } + + public static void encodeProtocolRatio(float ratio, byte[] buffer, int offset) { + ratio *= 32768; + encodeBinaryInt16((short) ratio, buffer, offset); + } + + /* . (-32768.9999 to 32767.9999) */ + public static float decodeProtocol1616Float(byte[] buffer, int offset) { + float result = (float) decodeBinaryUint32(buffer, offset); + result /= 65536; + return result; + } + + public static void encodeProtocol1616Float(float val, byte[] buffer, int offset) { + val *= 65536; + int int_val = (int) val; + encodeBinaryUint32(int_val, buffer, offset); + } + + public static byte getCRC(byte[] buffer, int length) { + int i, j, crc = 0; + + for (i = 0; i < length; i++) { + crc ^= 0x00ff & buffer[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x0001) != 0) { + crc ^= CRC7_POLY; + } + crc >>= 1; + } + } + return (byte) crc; + } + + static public class AHRSUpdate { + public float yaw; + public float pitch; + public float roll; + public float compass_heading; + public float altitude; + public float fused_heading; + public float linear_accel_x; + public float linear_accel_y; + public float linear_accel_z; + public short cal_mag_x; + public short cal_mag_y; + public short cal_mag_z; + public float mag_field_norm_ratio; + public float mag_field_norm_scalar; + public float mpu_temp; + public short raw_mag_x; + public short raw_mag_y; + public short raw_mag_z; + public short quat_w; + public short quat_x; + public short quat_y; + public short quat_z; + public float barometric_pressure; + public float baro_temp; + public byte op_status; + public byte sensor_status; + public byte cal_status; + public byte selftest_status; + } + + static public class AHRSPosUpdate { + public float yaw; + public float pitch; + public float roll; + public float compass_heading; + public float altitude; + public float fused_heading; + public float linear_accel_x; + public float linear_accel_y; + public float linear_accel_z; + public float vel_x; + public float vel_y; + public float vel_z; + public float disp_x; + public float disp_y; + public float disp_z; + public float mpu_temp; + public short quat_w; + public short quat_x; + public short quat_y; + public short quat_z; + public float barometric_pressure; + public float baro_temp; + public byte op_status; + public byte sensor_status; + public byte cal_status; + public byte selftest_status; + } + + static public class DataSetResponse { + public byte data_type; + public byte var_id; /* If type = TUNING_VARIABLE */ + public byte status; + } + + static public class IntegrationControl { + public byte action; + public int parameter; + } + + static public class MagCalData { + public short mag_bias[]; /* 3 Values */ + public float mag_xform[][]; /* 3 x 3 Values */ + public float earth_mag_field_norm; + byte action; + + public MagCalData() { + mag_bias = new short[3]; + mag_xform = new float[3][3]; + } + } + + static public class TuningVar { + public byte action; + public byte var_id; /* If type = TUNING_VARIABLE */ + float value; + } + + static public class BoardID { + public byte type; + public byte hw_rev; + public byte fw_ver_major; + public byte fw_ver_minor; + public short fw_revision; + public byte unique_id[]; + + public BoardID() { + unique_id = new byte[12]; + } + } + + public class AHRS_TUNING_VAR_ID { + public static final byte UNSPECIFIED = 0; + public static final byte MOTION_THRESHOLD = 1; /* In G */ + public static final byte YAW_STABLE_THRESHOLD = 2; /* In Degrees */ + public static final byte MAG_DISTURBANCE_THRESHOLD = 3; /* Ratio */ + public static final byte SEA_LEVEL_PRESSURE = 4; /* Millibars */ + } + + public class AHRS_DATA_TYPE { + public static final byte TUNING_VARIABLE = 0; + public static final byte MAG_CALIBRATION = 1; + public static final byte BOARD_IDENTITY = 2; + } + + public class AHRS_DATA_ACTION { + public static final byte DATA_GET = 0; + public static final byte DATA_SET = 1; + } + + +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/IMUProtocol.java b/lib-navx/src/main/java/com/kauailabs/navx/IMUProtocol.java new file mode 100644 index 0000000..88a2a6f --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/IMUProtocol.java @@ -0,0 +1,355 @@ +/* ============================================ + Nav6 source code is placed under the MIT license + Copyright (c) 2013 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ +package com.kauailabs.navx; + +public class IMUProtocol { + + public final static byte PACKET_START_CHAR = '!'; + // Yaw/Pitch/Roll (YPR) Update Packet - e.g., !y[yaw][pitch][roll][compass_heading] + public final static byte MSGID_YPR_UPDATE = 'y'; + // Quaternion Data Update Packet - e.g., !r[q1][q2][q3][q4][accelx][accely][accelz][magx][magy][magz] + public final static byte MSGID_QUATERNION_UPDATE = 'q'; + public final static byte MSGID_GYRO_UPDATE = 'g'; + // EnableStream Command Packet - e.g., !S[stream type][checksum][cr][lf] + public final static byte MSGID_STREAM_CMD = 'S'; + public final static int STREAM_CMD_STREAM_TYPE_YPR = MSGID_YPR_UPDATE; + public final static int STREAM_CMD_STREAM_TYPE_QUATERNION = MSGID_QUATERNION_UPDATE; + public final static int STREAM_CMD_STREAM_TYPE_GYRO = MSGID_GYRO_UPDATE; + // EnableStream Response Packet - e.g., !s[stream type][gyro full scale range][accel full scale range][update rate hz][yaw_offset_degrees][flags][checksum][cr][lf] + public final static byte MSG_ID_STREAM_RESPONSE = 's'; + public final static byte STREAM_MSG_TERMINATION_CHAR = (byte) '\n'; + public final static short NAV6_FLAG_MASK_CALIBRATION_STATE = 0x03; + public final static short NAV6_CALIBRATION_STATE_WAIT = 0x00; + public final static short NAV6_CALIBRATION_STATE_ACCUMULATE = 0x01; + public final static short NAV6_CALIBRATION_STATE_COMPLETE = 0x02; + final protected static byte[] hexArray + = new byte[]{(byte) '0', (byte) '1', (byte) '2', (byte) '3', + (byte) '4', (byte) '5', (byte) '6', (byte) '7', + (byte) '8', (byte) '9', (byte) 'A', (byte) 'B', + (byte) 'C', (byte) 'D', (byte) 'E', (byte) 'F'}; + final static int PROTOCOL_FLOAT_LENGTH = 7; + final static int CHECKSUM_LENGTH = 2; + final static int TERMINATOR_LENGTH = 2; + final static int YPR_UPDATE_YAW_VALUE_INDEX = 2; + final static int YPR_UPDATE_PITCH_VALUE_INDEX = 9; + final static int YPR_UPDATE_ROLL_VALUE_INDEX = 16; + final static int YPR_UPDATE_COMPASS_VALUE_INDEX = 23; + final static int YPR_UPDATE_CHECKSUM_INDEX = 30; + final static int YPR_UPDATE_TERMINATOR_INDEX = 32; + final static int YPR_UPDATE_MESSAGE_LENGTH = 34; + final static int QUATERNION_UPDATE_MESSAGE_LENGTH = 53; + public final static int IMU_PROTOCOL_MAX_MESSAGE_LENGTH = QUATERNION_UPDATE_MESSAGE_LENGTH; + + // Gyro/Raw Data Update packet - e.g., !g[gx][gy][gz][accelx][accely][accelz][magx][magy][magz][temp_c][cr][lf] + final static int QUATERNION_UPDATE_QUAT1_VALUE_INDEX = 2; + final static int QUATERNION_UPDATE_QUAT2_VALUE_INDEX = 6; + final static int QUATERNION_UPDATE_QUAT3_VALUE_INDEX = 10; + final static int QUATERNION_UPDATE_QUAT4_VALUE_INDEX = 14; + final static int QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX = 18; + final static int QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX = 22; + final static int QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX = 26; + final static int QUATERNION_UPDATE_MAG_X_VALUE_INDEX = 30; + final static int QUATERNION_UPDATE_MAG_Y_VALUE_INDEX = 34; + final static int QUATERNION_UPDATE_MAG_Z_VALUE_INDEX = 38; + final static int QUATERNION_UPDATE_TEMP_VALUE_INDEX = 42; + final static int QUATERNION_UPDATE_CHECKSUM_INDEX = 49; + final static int QUATERNION_UPDATE_TERMINATOR_INDEX = 51; + final static int GYRO_UPDATE_GYRO_X_VALUE_INDEX = 2; + final static int GYRO_UPDATE_GYRO_Y_VALUE_INDEX = 6; + final static int GYRO_UPDATE_GYRO_Z_VALUE_INDEX = 10; + final static int GYRO_UPDATE_ACCEL_X_VALUE_INDEX = 14; + final static int GYRO_UPDATE_ACCEL_Y_VALUE_INDEX = 18; + final static int GYRO_UPDATE_ACCEL_Z_VALUE_INDEX = 22; + final static int GYRO_UPDATE_MAG_X_VALUE_INDEX = 26; + final static int GYRO_UPDATE_MAG_Y_VALUE_INDEX = 30; + final static int GYRO_UPDATE_MAG_Z_VALUE_INDEX = 34; + final static int GYRO_UPDATE_TEMP_VALUE_INDEX = 38; + final static int GYRO_UPDATE_CHECKSUM_INDEX = 42; + final static int GYRO_UPDATE_TERMINATOR_INDEX = 44; + final static int GYRO_UPDATE_MESSAGE_LENGTH = 46; + final static int STREAM_CMD_STREAM_TYPE_INDEX = 2; + final static int STREAM_CMD_UPDATE_RATE_HZ_INDEX = 3; + final static int STREAM_CMD_CHECKSUM_INDEX = 5; + final static int STREAM_CMD_TERMINATOR_INDEX = 7; + final static int STREAM_CMD_MESSAGE_LENGTH = 9; + final static int STREAM_RESPONSE_MESSAGE_LENGTH = 46; + final static int STREAM_RESPONSE_STREAM_TYPE_INDEX = 2; + final static int STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE = 3; + final static int STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE = 7; + final static int STREAM_RESPONSE_UPDATE_RATE_HZ = 11; + final static int STREAM_RESPONSE_YAW_OFFSET_DEGREES = 15; + final static int STREAM_RESPONSE_QUAT1_OFFSET = 22; + final static int STREAM_RESPONSE_QUAT2_OFFSET = 26; + final static int STREAM_RESPONSE_QUAT3_OFFSET = 30; + final static int STREAM_RESPONSE_QUAT4_OFFSET = 34; + final static int STREAM_RESPONSE_FLAGS = 38; + final static int STREAM_RESPONSE_CHECKSUM_INDEX = 42; + final static int STREAM_RESPONSE_TERMINATOR_INDEX = 44; + + public static int encodeStreamCommand(byte[] protocol_buffer, byte stream_type, byte update_rate_hz) { + // Header + protocol_buffer[0] = PACKET_START_CHAR; + protocol_buffer[1] = MSGID_STREAM_CMD; + + // Data + protocol_buffer[STREAM_CMD_STREAM_TYPE_INDEX] = stream_type; + byteToHex(update_rate_hz, protocol_buffer, STREAM_CMD_UPDATE_RATE_HZ_INDEX); + + // Footer + encodeTermination(protocol_buffer, STREAM_CMD_MESSAGE_LENGTH, STREAM_CMD_MESSAGE_LENGTH - 4); + + return STREAM_CMD_MESSAGE_LENGTH; + } + + public static int decodeStreamResponse(byte[] buffer, int offset, int length, StreamResponse r) { + + if (length < STREAM_RESPONSE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSG_ID_STREAM_RESPONSE)) { + if (!verifyChecksum(buffer, offset, STREAM_RESPONSE_CHECKSUM_INDEX)) { + return 0; + } + + r.stream_type = buffer[offset + 2]; + r.gyro_fsr_dps = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_GYRO_FULL_SCALE_DPS_RANGE); + r.accel_fsr_g = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_ACCEL_FULL_SCALE_G_RANGE); + r.update_rate_hz = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_UPDATE_RATE_HZ); + r.yaw_offset_degrees = decodeProtocolFloat(buffer, offset + STREAM_RESPONSE_YAW_OFFSET_DEGREES); + r.q1_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT1_OFFSET); + r.q2_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT2_OFFSET); + r.q3_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT3_OFFSET); + r.q4_offset = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_QUAT4_OFFSET); + r.flags = decodeProtocolUint16(buffer, offset + STREAM_RESPONSE_FLAGS); + + return STREAM_RESPONSE_MESSAGE_LENGTH; + } + return 0; + } + + public static int decodeStreamCommand(byte[] buffer, int offset, int length, StreamCommand c) { + if (length < STREAM_CMD_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == '!') && (buffer[offset + 1] == MSGID_STREAM_CMD)) { + if (!verifyChecksum(buffer, offset, STREAM_CMD_CHECKSUM_INDEX)) { + return 0; + } + + c.stream_type = buffer[offset + STREAM_CMD_STREAM_TYPE_INDEX]; + return STREAM_CMD_MESSAGE_LENGTH; + } + return 0; + } + + public static int decodeYPRUpdate(byte[] buffer, int offset, int length, YPRUpdate u) { + if (length < YPR_UPDATE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == '!') && (buffer[offset + 1] == 'y')) { + if (!verifyChecksum(buffer, offset, YPR_UPDATE_CHECKSUM_INDEX)) { + return 0; + } + + u.yaw = decodeProtocolFloat(buffer, offset + YPR_UPDATE_YAW_VALUE_INDEX); + u.pitch = decodeProtocolFloat(buffer, offset + YPR_UPDATE_PITCH_VALUE_INDEX); + u.roll = decodeProtocolFloat(buffer, offset + YPR_UPDATE_ROLL_VALUE_INDEX); + u.compass_heading = decodeProtocolFloat(buffer, offset + YPR_UPDATE_COMPASS_VALUE_INDEX); + return YPR_UPDATE_MESSAGE_LENGTH; + } + return 0; + } + + public static int decodeQuaternionUpdate(byte[] buffer, int offset, int length, + QuaternionUpdate u) { + if (length < QUATERNION_UPDATE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSGID_QUATERNION_UPDATE)) { + if (!verifyChecksum(buffer, offset, QUATERNION_UPDATE_CHECKSUM_INDEX)) { + return 0; + } + + u.q1 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT1_VALUE_INDEX); + u.q2 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT2_VALUE_INDEX); + u.q3 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT3_VALUE_INDEX); + u.q4 = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_QUAT4_VALUE_INDEX); + u.accel_x = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_X_VALUE_INDEX); + u.accel_y = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_Y_VALUE_INDEX); + u.accel_z = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_ACCEL_Z_VALUE_INDEX); + u.mag_x = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_X_VALUE_INDEX); + u.mag_y = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_Y_VALUE_INDEX); + u.mag_z = decodeProtocolUint16(buffer, offset + QUATERNION_UPDATE_MAG_Z_VALUE_INDEX); + u.temp_c = decodeProtocolFloat(buffer, offset + QUATERNION_UPDATE_TEMP_VALUE_INDEX); + return QUATERNION_UPDATE_MESSAGE_LENGTH; + } + return 0; + } + + public static int decodeGyroUpdate(byte[] buffer, int offset, int length, + GyroUpdate u) { + if (length < GYRO_UPDATE_MESSAGE_LENGTH) { + return 0; + } + if ((buffer[offset + 0] == PACKET_START_CHAR) && (buffer[offset + 1] == MSGID_GYRO_UPDATE)) { + if (!verifyChecksum(buffer, offset, GYRO_UPDATE_CHECKSUM_INDEX)) { + return 0; + } + + u.gyro_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_X_VALUE_INDEX); + u.gyro_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_Y_VALUE_INDEX); + u.gyro_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_GYRO_Z_VALUE_INDEX); + u.accel_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_X_VALUE_INDEX); + u.accel_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_Y_VALUE_INDEX); + u.accel_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_ACCEL_Z_VALUE_INDEX); + u.mag_x = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_X_VALUE_INDEX); + u.mag_y = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_Y_VALUE_INDEX); + u.mag_z = decodeProtocolUint16(buffer, offset + GYRO_UPDATE_MAG_Z_VALUE_INDEX); + u.temp_c = decodeProtocolUnsignedHundredthsFloat(buffer, offset + GYRO_UPDATE_TEMP_VALUE_INDEX); + return GYRO_UPDATE_MESSAGE_LENGTH; + } + return 0; + } + + public static void encodeTermination(byte[] buffer, int total_length, int content_length) { + if ((total_length >= (CHECKSUM_LENGTH + TERMINATOR_LENGTH)) && (total_length >= content_length + (CHECKSUM_LENGTH + TERMINATOR_LENGTH))) { + // Checksum + byte checksum = 0; + for (int i = 0; i < content_length; i++) { + checksum += buffer[i]; + } + // convert checksum to two ascii bytes + + byteToHex(checksum, buffer, content_length); + // Message Terminator + buffer[content_length + CHECKSUM_LENGTH + 0] = '\r'; + buffer[content_length + CHECKSUM_LENGTH + 1] = '\n'; + } + } + + public static void byteToHex(byte thebyte, byte[] dest, int offset) { + int v = thebyte & 0xFF; + dest[offset + 0] = hexArray[v >> 4]; + dest[offset + 1] = hexArray[v & 0x0F]; + } + + public static short decodeProtocolUint16(byte[] uint16_string, int offset) { + short decoded_uint16 = 0; + int shift_left = 12; + for (int i = offset + 0; i < offset + 4; i++) { + byte digit = (byte) (uint16_string[i] <= '9' ? uint16_string[i] - '0' : ((uint16_string[i] - 'A') + 10)); + decoded_uint16 += (((short) digit) << shift_left); + shift_left -= 4; + } + return decoded_uint16; + } + + /* 0 to 655.35 */ + public static float decodeProtocolUnsignedHundredthsFloat(byte[] uint8_unsigned_hundredths_float, int offset) { + float unsigned_float = (float) decodeProtocolUint16(uint8_unsigned_hundredths_float, offset); + unsigned_float /= 100; + return unsigned_float; + } + + public static boolean verifyChecksum(byte[] buffer, int offset, int content_length) { + // Calculate Checksum + byte checksum = 0; + for (int i = 0; i < content_length; i++) { + checksum += buffer[offset + i]; + } + + // Decode Checksum + byte decoded_checksum = decodeUint8(buffer, offset + content_length); + + return (checksum == decoded_checksum); + } + + public static byte decodeUint8(byte[] checksum, int offset) { + byte first_digit = (byte) (checksum[0 + offset] <= '9' ? checksum[0 + offset] - '0' : ((checksum[0 + offset] - 'A') + 10)); + byte second_digit = (byte) (checksum[1 + offset] <= '9' ? checksum[1 + offset] - '0' : ((checksum[1 + offset] - 'A') + 10)); + byte decoded_checksum = (byte) ((first_digit * 16) + second_digit); + return decoded_checksum; + } + + public static float decodeProtocolFloat(byte[] buffer, int offset) { + String float_string = new String(buffer, offset, PROTOCOL_FLOAT_LENGTH); + return Float.parseFloat(float_string); + } + + static public class YPRUpdate { + + public float yaw; + public float pitch; + public float roll; + public float compass_heading; + } + + static public class StreamCommand { + + public byte stream_type; + } + + static public class StreamResponse { + + public byte stream_type; + public short gyro_fsr_dps; + public short accel_fsr_g; + public short update_rate_hz; + public float yaw_offset_degrees; + public short q1_offset; + public short q2_offset; + public short q3_offset; + public short q4_offset; + public short flags; + } + + static public class QuaternionUpdate { + + public short q1; + public short q2; + public short q3; + public short q4; + public short accel_x; + public short accel_y; + public short accel_z; + public short mag_x; + public short mag_y; + public short mag_z; + public float temp_c; + } + + static public class GyroUpdate { + + public short gyro_x; + public short gyro_y; + public short gyro_z; + public short accel_x; + public short accel_y; + public short accel_z; + public short mag_x; + public short mag_y; + public short mag_z; + public float temp_c; + } +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/IMURegisters.java b/lib-navx/src/main/java/com/kauailabs/navx/IMURegisters.java new file mode 100644 index 0000000..782cc1a --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/IMURegisters.java @@ -0,0 +1,213 @@ +/* ============================================ + NavX-MXP source code is placed under the MIT license + Copyright (c) 2015 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ +package com.kauailabs.navx; + +public class IMURegisters { + + /**********************************************/ + /* Device Identification Registers */ + /**********************************************/ + + public static final byte NAVX_REG_WHOAMI = 0x00; /* IMU_MODEL_XXX */ + public static final byte NAVX_REG_HW_REV = 0x01; + public static final byte NAVX_REG_FW_VER_MAJOR = 0x02; + public static final byte NAVX_REG_FW_VER_MINOR = 0x03; + + /**********************************************/ + /* Status and Control Registers */ + /**********************************************/ + + /* Read-write */ + public static final byte NAVX_REG_UPDATE_RATE_HZ = 0x04; /* Range: 4 - 50 [unsigned byte] */ + /* Read-only */ + /* Accelerometer Full-Scale Range: in units of G [unsigned byte] */ + public static final byte NAVX_REG_ACCEL_FSR_G = 0x05; + /* Gyro Full-Scale Range (Degrees/Sec): Range: 250, 500, 1000 or 2000 [unsigned short] */ + public static final byte NAVX_REG_GYRO_FSR_DPS_L = 0x06; /* Lower 8-bits of Gyro Full-Scale Range */ + public static final byte NAVX_REG_GYRO_FSR_DPS_H = 0x07; /* Upper 8-bits of Gyro Full-Scale Range */ + public static final byte NAVX_REG_OP_STATUS = 0x08; /* NAVX_OP_STATUS_XXX */ + public static final byte NAVX_REG_CAL_STATUS = 0x09; /* NAVX_CAL_STATUS_XXX */ + public static final byte NAVX_REG_SELFTEST_STATUS = 0x0A; /* NAVX_SELFTEST_STATUS_XXX */ + public static final byte NAVX_REG_CAPABILITY_FLAGS_L = 0x0B; + public static final byte NAVX_REG_CAPABILITY_FLAGS_H = 0x0C; + + /**********************************************/ + /* Processed Data Registers */ + /**********************************************/ + + public static final byte NAVX_REG_SENSOR_STATUS_L = 0x10; /* NAVX_SENSOR_STATUS_XXX */ + public static final byte NAVX_REG_SENSOR_STATUS_H = 0x11; + /* Timestamp: [unsigned long] */ + public static final byte NAVX_REG_TIMESTAMP_L_L = 0x12; + public static final byte NAVX_REG_TIMESTAMP_L_H = 0x13; + public static final byte NAVX_REG_TIMESTAMP_H_L = 0x14; + public static final byte NAVX_REG_TIMESTAMP_H_H = 0x15; + + /* Yaw, Pitch, Roll: Range: -180.00 to 180.00 [signed hundredths] */ + /* Compass Heading: Range: 0.00 to 360.00 [unsigned hundredths] */ + /* Altitude in Meters: In units of meters [16:16] */ + + public static final byte NAVX_REG_YAW_L = 0x16; /* Lower 8 bits of Yaw */ + public static final byte NAVX_REG_YAW_H = 0x17; /* Upper 8 bits of Yaw */ + public static final byte NAVX_REG_ROLL_L = 0x18; /* Lower 8 bits of Roll */ + public static final byte NAVX_REG_ROLL_H = 0x19; /* Upper 8 bits of Roll */ + public static final byte NAVX_REG_PITCH_L = 0x1A; /* Lower 8 bits of Pitch */ + public static final byte NAVX_REG_PITCH_H = 0x1B; /* Upper 8 bits of Pitch */ + public static final byte NAVX_REG_HEADING_L = 0x1C; /* Lower 8 bits of Heading */ + public static final byte NAVX_REG_HEADING_H = 0x1D; /* Upper 8 bits of Heading */ + public static final byte NAVX_REG_FUSED_HEADING_L = 0x1E; /* Upper 8 bits of Fused Heading */ + public static final byte NAVX_REG_FUSED_HEADING_H = 0x1F; /* Upper 8 bits of Fused Heading */ + public static final byte NAVX_REG_ALTITUDE_I_L = 0x20; + public static final byte NAVX_REG_ALTITUDE_I_H = 0x21; + public static final byte NAVX_REG_ALTITUDE_D_L = 0x22; + public static final byte NAVX_REG_ALTITUDE_D_H = 0x23; + + /* World-frame Linear Acceleration: In units of +/- G * 1000 [signed thousandths] */ + + public static final byte NAVX_REG_LINEAR_ACC_X_L = 0x24; /* Lower 8 bits of Linear Acceleration X */ + public static final byte NAVX_REG_LINEAR_ACC_X_H = 0x25; /* Upper 8 bits of Linear Acceleration X */ + public static final byte NAVX_REG_LINEAR_ACC_Y_L = 0x26; /* Lower 8 bits of Linear Acceleration Y */ + public static final byte NAVX_REG_LINEAR_ACC_Y_H = 0x27; /* Upper 8 bits of Linear Acceleration Y */ + public static final byte NAVX_REG_LINEAR_ACC_Z_L = 0x28; /* Lower 8 bits of Linear Acceleration Z */ + public static final byte NAVX_REG_LINEAR_ACC_Z_H = 0x29; /* Upper 8 bits of Linear Acceleration Z */ + + /* Quaternion: Range -1 to 1 [signed short ratio] */ + + public static final byte NAVX_REG_QUAT_W_L = 0x2A; /* Lower 8 bits of Quaternion W */ + public static final byte NAVX_REG_QUAT_W_H = 0x2B; /* Upper 8 bits of Quaternion W */ + public static final byte NAVX_REG_QUAT_X_L = 0x2C; /* Lower 8 bits of Quaternion X */ + public static final byte NAVX_REG_QUAT_X_H = 0x2D; /* Upper 8 bits of Quaternion X */ + public static final byte NAVX_REG_QUAT_Y_L = 0x2E; /* Lower 8 bits of Quaternion Y */ + public static final byte NAVX_REG_QUAT_Y_H = 0x2F; /* Upper 8 bits of Quaternion Y */ + public static final byte NAVX_REG_QUAT_Z_L = 0x30; /* Lower 8 bits of Quaternion Z */ + public static final byte NAVX_REG_QUAT_Z_H = 0x31; /* Upper 8 bits of Quaternion Z */ + + /**********************************************/ + /* Raw Data Registers */ + /**********************************************/ + + /* Sensor Die Temperature: Range +/- 150, In units of Centigrade * 100 [signed hundredths float */ + + public static final byte NAVX_REG_MPU_TEMP_C_L = 0x32; /* Lower 8 bits of Temperature */ + public static final byte NAVX_REG_MPU_TEMP_C_H = 0x33; /* Upper 8 bits of Temperature */ + + /* Raw, Calibrated Angular Rotation, in device units. Value in DPS = units / GYRO_FSR_DPS [signed short] */ + + public static final byte NAVX_REG_GYRO_X_L = 0x34; + public static final byte NAVX_REG_GYRO_X_H = 0x35; + public static final byte NAVX_REG_GYRO_Y_L = 0x36; + public static final byte NAVX_REG_GYRO_Y_H = 0x37; + public static final byte NAVX_REG_GYRO_Z_L = 0x38; + public static final byte NAVX_REG_GYRO_Z_H = 0x39; + + /* Raw, Calibrated, Acceleration Data, in device units. Value in G = units / ACCEL_FSR_G [signed short] */ + + public static final byte NAVX_REG_ACC_X_L = 0x3A; + public static final byte NAVX_REG_ACC_X_H = 0x3B; + public static final byte NAVX_REG_ACC_Y_L = 0x3C; + public static final byte NAVX_REG_ACC_Y_H = 0x3D; + public static final byte NAVX_REG_ACC_Z_L = 0x3E; + public static final byte NAVX_REG_ACC_Z_H = 0x3F; + + /* Raw, Calibrated, Un-tilt corrected Magnetometer Data, in device units. 1 unit = 0.15 uTesla [signed short] */ + + public static final byte NAVX_REG_MAG_X_L = 0x40; + public static final byte NAVX_REG_MAG_X_H = 0x41; + public static final byte NAVX_REG_MAG_Y_L = 0x42; + public static final byte NAVX_REG_MAG_Y_H = 0x43; + public static final byte NAVX_REG_MAG_Z_L = 0x44; + public static final byte NAVX_REG_MAG_Z_H = 0x45; + + /* Calibrated Pressure in millibars Valid Range: 10.00 Max: 1200.00 [16:16 float] */ + + public static final byte NAVX_REG_PRESSURE_IL = 0x46; + public static final byte NAVX_REG_PRESSURE_IH = 0x47; + public static final byte NAVX_REG_PRESSURE_DL = 0x48; + public static final byte NAVX_REG_PRESSURE_DH = 0x49; + + /* Pressure Sensor Die Temperature: Range +/- 150.00C [signed hundredths] */ + + public static final byte NAVX_REG_PRESSURE_TEMP_L = 0x4A; + public static final byte NAVX_REG_PRESSURE_TEMP_H = 0x4B; + + /**********************************************/ + /* Calibration Registers */ + /**********************************************/ + + /* Yaw Offset: Range -180.00 to 180.00 [signed hundredths] */ + + public static final byte NAVX_REG_YAW_OFFSET_L = 0x4C; /* Lower 8 bits of Yaw Offset */ + public static final byte NAVX_REG_YAW_OFFSET_H = 0x4D; /* Upper 8 bits of Yaw Offset */ + + /* Quaternion Offset: Range: -1 to 1 [signed short ratio] */ + + public static final byte NAVX_REG_QUAT_OFFSET_W_L = 0x4E; /* Lower 8 bits of Quaternion W */ + public static final byte NAVX_REG_QUAT_OFFSET_W_H = 0x4F; /* Upper 8 bits of Quaternion W */ + public static final byte NAVX_REG_QUAT_OFFSET_X_L = 0x50; /* Lower 8 bits of Quaternion X */ + public static final byte NAVX_REG_QUAT_OFFSET_X_H = 0x51; /* Upper 8 bits of Quaternion X */ + public static final byte NAVX_REG_QUAT_OFFSET_Y_L = 0x52; /* Lower 8 bits of Quaternion Y */ + public static final byte NAVX_REG_QUAT_OFFSET_Y_H = 0x53; /* Upper 8 bits of Quaternion Y */ + public static final byte NAVX_REG_QUAT_OFFSET_Z_L = 0x54; /* Lower 8 bits of Quaternion Z */ + public static final byte NAVX_REG_QUAT_OFFSET_Z_H = 0x55; /* Upper 8 bits of Quaternion Z */ + + /**********************************************/ + /* Integrated Data Registers */ + /**********************************************/ + + /* Integration Control (Write-Only) */ + public static final byte NAVX_REG_INTEGRATION_CTL = 0x56; + public static final byte NAVX_REG_PAD_UNUSED = 0x57; + + /* Velocity: Range -32768.9999 - 32767.9999 in units of Meters/Sec */ + + public static final byte NAVX_REG_VEL_X_I_L = 0x58; + public static final byte NAVX_REG_VEL_X_I_H = 0x59; + public static final byte NAVX_REG_VEL_X_D_L = 0x5A; + public static final byte NAVX_REG_VEL_X_D_H = 0x5B; + public static final byte NAVX_REG_VEL_Y_I_L = 0x5C; + public static final byte NAVX_REG_VEL_Y_I_H = 0x5D; + public static final byte NAVX_REG_VEL_Y_D_L = 0x5E; + public static final byte NAVX_REG_VEL_Y_D_H = 0x5F; + public static final byte NAVX_REG_VEL_Z_I_L = 0x60; + public static final byte NAVX_REG_VEL_Z_I_H = 0x61; + public static final byte NAVX_REG_VEL_Z_D_L = 0x62; + public static final byte NAVX_REG_VEL_Z_D_H = 0x63; + + /* Displacement: Range -32768.9999 - 32767.9999 in units of Meters */ + + public static final byte NAVX_REG_DISP_X_I_L = 0x64; + public static final byte NAVX_REG_DISP_X_I_H = 0x65; + public static final byte NAVX_REG_DISP_X_D_L = 0x66; + public static final byte NAVX_REG_DISP_X_D_H = 0x67; + public static final byte NAVX_REG_DISP_Y_I_L = 0x68; + public static final byte NAVX_REG_DISP_Y_I_H = 0x69; + public static final byte NAVX_REG_DISP_Y_D_L = 0x6A; + public static final byte NAVX_REG_DISP_Y_D_H = 0x6B; + public static final byte NAVX_REG_DISP_Z_I_L = 0x6C; + public static final byte NAVX_REG_DISP_Z_I_H = 0x6D; + public static final byte NAVX_REG_DISP_Z_D_L = 0x6E; + public static final byte NAVX_REG_DISP_Z_D_H = 0x6F; + + public static final byte NAVX_REG_LAST = NAVX_REG_DISP_Z_D_H; +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/ftc/AHRS.java b/lib-navx/src/main/java/com/kauailabs/navx/ftc/AHRS.java new file mode 100644 index 0000000..e3aa824 --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/AHRS.java @@ -0,0 +1,1761 @@ +/* ============================================ + NavX-MXP and NavX-Micro source code is placed under the MIT license + Copyright (c) 2015 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ +package com.kauailabs.navx.ftc; + +import android.os.Process; +import android.os.SystemClock; +import android.util.Log; + +import com.kauailabs.navx.AHRSProtocol; +import com.kauailabs.navx.IMUProtocol; +import com.kauailabs.navx.IMURegisters; +import com.qualcomm.robotcore.hardware.DeviceInterfaceModule; +import com.qualcomm.robotcore.hardware.I2cController; +import com.qualcomm.robotcore.hardware.I2cDevice; + +import java.util.Arrays; + +/** + * The AHRS class provides an interface to AHRS capabilities + * of the KauaiLabs navX Robotics Navigation Sensor via I2C on the Android- + * based FTC robotics control system, where communications occur via the + * "Core Device Interface Module" produced by Modern Robotics, inc. + *

    + * The AHRS class enables access to basic connectivity and state information, + * as well as key 6-axis and 9-axis orientation information (yaw, pitch, roll, + * compass heading, fused (9-axis) heading and magnetic disturbance detection. + *

    + * Additionally, the ARHS class also provides access to extended information + * including linear acceleration, motion detection, rotation detection and sensor + * temperature. + *

    + * If used with navX-Aero-enabled devices, the AHRS class also provides access to + * altitude, barometric pressure and pressure sensor temperature data + * + * @author Scott + */ +public class AHRS { + + private static final int NAVX_DEFAULT_UPDATE_RATE_HZ = 50; + private static AHRS instance = null; + private static boolean enable_logging = false; + private static DimStateTracker global_dim_state_tracker; + final int NAVX_I2C_DEV_7BIT_ADDRESS = 0x32; + final int NAVX_I2C_DEV_8BIT_ADDRESS = NAVX_I2C_DEV_7BIT_ADDRESS << 1; + private final int MAX_NUM_CALLBACKS = 3; + private final float DEV_UNITS_MAX = 32768.0f; + private final float UTESLA_PER_DEV_UNIT = 0.15f; + AHRSProtocol.AHRSPosUpdate curr_data; + BoardState board_state; + AHRSProtocol.BoardID board_id; + IMUProtocol.GyroUpdate raw_data_update; + private DeviceInterfaceModule dim = null; + private navXIOThread io_thread_obj; + private Thread io_thread; + private int update_rate_hz = NAVX_DEFAULT_UPDATE_RATE_HZ; + private IDataArrivalSubscriber callbacks[]; + + protected AHRS(DeviceInterfaceModule dim, int dim_i2c_port, + DeviceDataType data_type, int update_rate_hz) { + this.callbacks = new IDataArrivalSubscriber[MAX_NUM_CALLBACKS]; + this.dim = dim; + this.update_rate_hz = update_rate_hz; + this.curr_data = new AHRSProtocol.AHRSPosUpdate(); + this.board_state = new BoardState(); + this.board_id = new AHRSProtocol.BoardID(); + this.raw_data_update = new IMUProtocol.GyroUpdate(); + + io_thread_obj = new navXIOThread(dim_i2c_port, update_rate_hz, data_type, curr_data); + io_thread_obj.start(); + + io_thread = new Thread(io_thread_obj); + io_thread.start(); + } + + /** + * Returns the single instance (singleton) of the AHRS class. If the singleton + * does not alrady exist, it will be created using the parameters passed in. + * The default update rate will be used. + *

    + * If the singleton already exists, the parameters passed in will be ignored. + * + * @return The singleton AHRS class instance. + */ + public static AHRS getInstance(DeviceInterfaceModule dim, int dim_i2c_port, + DeviceDataType data_type) { + if (instance == null) { + instance = new AHRS(dim, dim_i2c_port, data_type, NAVX_DEFAULT_UPDATE_RATE_HZ); + } + return instance; + } + + /** + * Returns the single instance (singleton) of the AHRS class. If the singleton + * does not alrady exist, it will be created using the parameters passed in, + * including a custom update rate. Use this function if an update rate other than + * the default is desired. + *

    + * NOTE: The range of valid requested update rates is from 4 to 66. However, the + * actual update does not always match the requested update rate. The following table + * summarizes the requested to actual update rate lookup table: + *

    + * Actual Requested + * 66.6 58-66 + * 50 45-57 + * 40 37-44 + * 33.3 31-36 + * 28.57 27-30 + * 25 25-26 + * 22.22 22-23 + * 20 20-21 + * 18.18 18-19 + * 16.67 17 + * 15.38 15-16 + * 14.28 14 + *

    + * Requested values below 14Hz result in an actual rate which is accurate to within 1Hz. + *

    + * The reason for this difference is that the update rate must be an even multiple of + * a 200Hz clock (5ms). So an actual of 66.6 (15ms/sample) can be calculated as follows: + * actual_rate = 200 / (200 / requested_rate) + *

    + * The getActualUpdateRate() can be used to calculate this value. + * + * @return The singleton AHRS class instance. + */ + public static AHRS getInstance(DeviceInterfaceModule dim, int dim_i2c_port, + DeviceDataType data_type, byte update_rate_hz) { + if (instance == null) { + instance = new AHRS(dim, dim_i2c_port, data_type, update_rate_hz); + } + return instance; + } + + /* Returns 'true' if AHRS class logging is enabled, otherwise + * returns 'false'. + */ + public static boolean getLogging() { + return enable_logging; + } + + /* Configures the AHRS class logging. To enable logging, + * the input parameter should be set to 'true'. + */ + public static void setLogging(boolean enabled) { + enable_logging = enabled; + } + + /** + * Registers a callback interface. This interface + * will be called back when new data is available, + * based upon a change in the sensor timestamp. + *

    + * Note that this callback will occur within the context of the + * device IO thread, which is not the same thread context the + * caller typically executes in. + */ + public boolean registerCallback(IDataArrivalSubscriber callback) { + boolean registered = false; + for (int i = 0; i < this.callbacks.length; i++) { + if (this.callbacks[i] == null) { + this.callbacks[i] = callback; + registered = true; + break; + } + } + return registered; + } + + /** + * Deregisters a previously registered callback interface. + *

    + * Be sure to deregister any callback which have been + * previously registered, to ensure that the object + * implementing the callback interface does not continue + * to be accessed when no longer necessary. + */ + public boolean deregisterCallback(IDataArrivalSubscriber callback) { + boolean deregistered = false; + for (int i = 0; i < this.callbacks.length; i++) { + if (this.callbacks[i] == callback) { + this.callbacks[i] = null; + deregistered = true; + break; + } + } + return deregistered; + } + + public void close() { + io_thread_obj.stop(); + for (int i = 0; i < callbacks.length; i++) { + callbacks[i] = null; + } + try { + io_thread.join(); + } catch (InterruptedException e) { + e.printStackTrace(); + } + instance = null; + } + + /* Returns the "port number" on the Core Device Interface Module + in which the navX-Model device is connected. + */ + public int getDimI2cPort() { + return this.io_thread_obj.dim_port; + } + + /* Returns the currently configured DeviceDataType. + */ + public DeviceDataType getDeviceDataType() { + return this.io_thread_obj.data_type; + } + + /** + * Returns the current pitch value (in degrees, from -180 to 180) + * reported by the sensor. Pitch is a measure of rotation around + * the X Axis. + * + * @return The current pitch value in degrees (-180 to 180). + */ + public float getPitch() { + return curr_data.pitch; + } + + /** + * Returns the current roll value (in degrees, from -180 to 180) + * reported by the sensor. Roll is a measure of rotation around + * the X Axis. + * + * @return The current roll value in degrees (-180 to 180). + */ + public float getRoll() { + return curr_data.roll; + } + + /** + * Returns the current yaw value (in degrees, from -180 to 180) + * reported by the sensor. Yaw is a measure of rotation around + * the Z Axis (which is perpendicular to the earth). + *

    + * Note that the returned yaw value will be offset by a user-specified + * offset value; this user-specified offset value is set by + * invoking the zeroYaw() method. + * + * @return The current yaw value in degrees (-180 to 180). + */ + public float getYaw() { + return curr_data.yaw; + } + + /** + * Returns the current tilt-compensated compass heading + * value (in degrees, from 0 to 360) reported by the sensor. + *

    + * Note that this value is sensed by a magnetometer, + * which can be affected by nearby magnetic fields (e.g., the + * magnetic fields generated by nearby motors). + *

    + * Before using this value, ensure that (a) the magnetometer + * has been calibrated and (b) that a magnetic disturbance is + * not taking place at the instant when the compass heading + * was generated. + * + * @return The current tilt-compensated compass heading, in degrees (0-360). + */ + public float getCompassHeading() { + return curr_data.compass_heading; + } + + /** + * Sets the user-specified yaw offset to the current + * yaw value reported by the sensor. + *

    + * This user-specified yaw offset is automatically + * subtracted from subsequent yaw values reported by + * the getYaw() method. + */ + public void zeroYaw() { + io_thread_obj.zeroYaw(); + } + + /** + * Returns true if the sensor is currently performing automatic + * gyro/accelerometer calibration. Automatic calibration occurs + * when the sensor is initially powered on, during which time the + * sensor should be held still, with the Z-axis pointing up + * (perpendicular to the earth). + *

    + * NOTE: During this automatic calibration, the yaw, pitch and roll + * values returned may not be accurate. + *

    + * Once calibration is complete, the sensor will automatically remove + * an internal yaw offset value from all reported values. + *

    + * + * @return Returns true if the sensor is currently automatically + * calibrating the gyro and accelerometer sensors. + */ + + public boolean isCalibrating() { + return !((curr_data.cal_status & + AHRSProtocol.NAVX_CAL_STATUS_IMU_CAL_STATE_MASK) == + AHRSProtocol.NAVX_CAL_STATUS_IMU_CAL_COMPLETE); + } + + /** + * Indicates whether the sensor is currently connected + * to the host computer. A connection is considered established + * whenever communication with the sensor has occurred recently. + *

    + * + * @return Returns true if a valid update has been recently received + * from the sensor. + */ + + public boolean isConnected() { + return io_thread_obj.isConnected(); + } + + /** + * Returns the navX-Model device's currently configured update + * rate. Note that the update rate that can actually be realized + * is a value evenly divisible by the navX-Model device's internal + * motion processor sample clock (200Hz). Therefore, the rate that + * is returned may be lower than the requested sample rate. + *

    + * The actual sample rate is rounded down to the nearest integer + * that is divisible by the number of Digital Motion Processor clock + * ticks. For instance, a request for 58 Hertz will result in + * an actual rate of 66Hz (200 / (200 / 58), using integer + * math. + * + * @return Returns the current actual update rate in Hz + * (cycles per second). + */ + + public byte getActualUpdateRate() { + final int NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ = 200; + int integer_update_rate = board_state.update_rate_hz; + int realized_update_rate = NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ / + (NAVX_MOTION_PROCESSOR_UPDATE_RATE_HZ / integer_update_rate); + return (byte) realized_update_rate; + } + + /** + * Returns the current number of data samples being transferred + * from the navX-Model device in the last second. Note that this + * number may be greater than the sensors update rate. + * + * @return Returns the count of data samples received in the + * last second. + */ + + public int getCurrentTransferRate() { + return this.io_thread_obj.last_second_hertz; + } + + public int getDuplicateDataCount() { + return this.io_thread_obj.getDuplicateDataCount(); + } + + /** + * Returns the count in bytes of data received from the + * sensor. This could can be useful for diagnosing + * connectivity issues. + *

    + * If the byte count is increasing, but the update count + * (see getUpdateCount()) is not, this indicates a software + * misconfiguration. + * + * @return The number of bytes received from the sensor. + */ + public double getByteCount() { + return io_thread_obj.getByteCount(); + } + + /* Returns the number of navX-Model processed data samples + * that were retrieved from the sensor that had a sensor + * timestamp value equal to the timestamp of the last + * sensor data sample. + * + * This information can be used to match the navX-Model + * sensor update rate w/the effective update rate which + * can be achieved in the Robot Controller, taking into + * account the communication over the network with the + * Core Device Interface Module. + */ + + /** + * Returns the count of valid updates which have + * been received from the sensor. This count should increase + * at the same rate indicated by the configured update rate. + * + * @return The number of valid updates received from the sensor. + */ + public double getUpdateCount() { + return io_thread_obj.getUpdateCount(); + } + + /** + * Returns the current linear acceleration in the X-axis (in G). + *

    + * World linear acceleration refers to raw acceleration data, which + * has had the gravity component removed, and which has been rotated to + * the same reference frame as the current yaw value. The resulting + * value represents the current acceleration in the x-axis of the + * body (e.g., the robot) on which the sensor is mounted. + *

    + * + * @return Current world linear acceleration in the X-axis (in G). + */ + public float getWorldLinearAccelX() { + return curr_data.linear_accel_x; + } + + /** + * Returns the current linear acceleration in the Y-axis (in G). + *

    + * World linear acceleration refers to raw acceleration data, which + * has had the gravity component removed, and which has been rotated to + * the same reference frame as the current yaw value. The resulting + * value represents the current acceleration in the Y-axis of the + * body (e.g., the robot) on which the sensor is mounted. + *

    + * + * @return Current world linear acceleration in the Y-axis (in G). + */ + public float getWorldLinearAccelY() { + return curr_data.linear_accel_y; + } + + /** + * Returns the current linear acceleration in the Z-axis (in G). + *

    + * World linear acceleration refers to raw acceleration data, which + * has had the gravity component removed, and which has been rotated to + * the same reference frame as the current yaw value. The resulting + * value represents the current acceleration in the Z-axis of the + * body (e.g., the robot) on which the sensor is mounted. + *

    + * + * @return Current world linear acceleration in the Z-axis (in G). + */ + public float getWorldLinearAccelZ() { + return curr_data.linear_accel_z; + } + + /** + * Indicates if the sensor is currently detecting motion, + * based upon the X and Y-axis world linear acceleration values. + * If the sum of the absolute values of the X and Y axis exceed + * a "motion threshold", the motion state is indicated. + *

    + * + * @return Returns true if the sensor is currently detecting motion. + */ + public boolean isMoving() { + return (curr_data.sensor_status & + AHRSProtocol.NAVX_SENSOR_STATUS_MOVING) != 0; + } + + /** + * Indicates if the sensor is currently detecting yaw rotation, + * based upon whether the change in yaw over the last second + * exceeds the "Rotation Threshold." + *

    + * Yaw Rotation can occur either when the sensor is rotating, or + * when the sensor is not rotating AND the current gyro calibration + * is insufficiently calibrated to yield the standard yaw drift rate. + *

    + * + * @return Returns true if the sensor is currently detecting motion. + */ + public boolean isRotating() { + return !((curr_data.sensor_status & + AHRSProtocol.NAVX_SENSOR_STATUS_YAW_STABLE) != 0); + } + + /** + * Returns the current altitude, based upon calibrated readings + * from a barometric pressure sensor, and the currently-configured + * sea-level barometric pressure [navX Aero only]. This value is in units of meters. + *

    + * NOTE: This value is only valid sensors including a pressure + * sensor. To determine whether this value is valid, see + * isAltitudeValid(). + *

    + * + * @return Returns current altitude in meters (as long as the sensor includes + * an installed on-board pressure sensor). + */ + public float getAltitude() { + return curr_data.altitude; + } + + /** + * Indicates whether the current altitude (and barometric pressure) data is + * valid. This value will only be true for a sensor with an onboard + * pressure sensor installed. + *

    + * If this value is false for a board with an installed pressure sensor, + * this indicates a malfunction of the onboard pressure sensor. + *

    + * + * @return Returns true if a working pressure sensor is installed. + */ + public boolean isAltitudeValid() { + return (curr_data.sensor_status & + AHRSProtocol.NAVX_SENSOR_STATUS_ALTITUDE_VALID) != 0; + } + + /** + * Returns the "fused" (9-axis) heading. + *

    + * The 9-axis heading is the fusion of the yaw angle, the tilt-corrected + * compass heading, and magnetic disturbance detection. Note that the + * magnetometer calibration procedure is required in order to + * achieve valid 9-axis headings. + *

    + * The 9-axis Heading represents the sensor's best estimate of current heading, + * based upon the last known valid Compass Angle, and updated by the change in the + * Yaw Angle since the last known valid Compass Angle. The last known valid Compass + * Angle is updated whenever a Calibrated Compass Angle is read and the sensor + * has recently rotated less than the Compass Noise Bandwidth (~2 degrees). + * + * @return Fused Heading in Degrees (range 0-360) + */ + public float getFusedHeading() { + return curr_data.fused_heading; + } + + /** + * Indicates whether the current magnetic field strength diverges from the + * calibrated value for the earth's magnetic field by more than the currently- + * configured Magnetic Disturbance Ratio. + *

    + * This function will always return false if the sensor's magnetometer has + * not yet been calibrated; see isMagnetometerCalibrated(). + * + * @return true if a magnetic disturbance is detected (or the magnetometer is uncalibrated). + */ + public boolean isMagneticDisturbance() { + return (curr_data.sensor_status & + AHRSProtocol.NAVX_SENSOR_STATUS_MAG_DISTURBANCE) != 0; + } + + /** + * Indicates whether the magnetometer has been calibrated. + *

    + * Magnetometer Calibration must be performed by the user. + *

    + * Note that if this function does indicate the magnetometer is calibrated, + * this does not necessarily mean that the calibration quality is sufficient + * to yield valid compass headings. + *

    + * + * @return Returns true if magnetometer calibration has been performed. + */ + public boolean isMagnetometerCalibrated() { + return (curr_data.cal_status & + AHRSProtocol.NAVX_CAL_STATUS_MAG_CAL_COMPLETE) != 0; + } + + /** + * Returns the imaginary portion (W) of the Orientation Quaternion which + * fully describes the current sensor orientation with respect to the + * reference angle defined as the angle at which the yaw was last "zeroed". + *

    + * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 + * to 2. This total range (4) can be associated with a unit circle, since + * each circle is comprised of 4 PI Radians. + *

    + * For more information on Quaternions and their use, please see this definition. + * + * @return Returns the imaginary portion (W) of the quaternion. + */ + public float getQuaternionW() { + return ((float) curr_data.quat_w / 16384.0f); + } + + /** + * Returns the real portion (X axis) of the Orientation Quaternion which + * fully describes the current sensor orientation with respect to the + * reference angle defined as the angle at which the yaw was last "zeroed". + *

    + * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 + * to 2. This total range (4) can be associated with a unit circle, since + * each circle is comprised of 4 PI Radians. + *

    + * For more information on Quaternions and their use, please see this description. + * + * @return Returns the real portion (X) of the quaternion. + */ + public float getQuaternionX() { + return ((float) curr_data.quat_x / 16384.0f); + } + + /* Unit Quaternions */ + + /** + * Returns the real portion (X axis) of the Orientation Quaternion which + * fully describes the current sensor orientation with respect to the + * reference angle defined as the angle at which the yaw was last "zeroed". + *

    + * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 + * to 2. This total range (4) can be associated with a unit circle, since + * each circle is comprised of 4 PI Radians. + *

    + * For more information on Quaternions and their use, please see: + *

    + * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation + * + * @return Returns the real portion (X) of the quaternion. + */ + public float getQuaternionY() { + return ((float) curr_data.quat_y / 16384.0f); + } + + /** + * Returns the real portion (X axis) of the Orientation Quaternion which + * fully describes the current sensor orientation with respect to the + * reference angle defined as the angle at which the yaw was last "zeroed". + *

    + * Each quaternion value (W,X,Y,Z) is expressed as a value ranging from -2 + * to 2. This total range (4) can be associated with a unit circle, since + * each circle is comprised of 4 PI Radians. + *

    + * For more information on Quaternions and their use, please see: + *

    + * https://en.wikipedia.org/wiki/Quaternions_and_spatial_rotation + * + * @return Returns the real portion (X) of the quaternion. + */ + public float getQuaternionZ() { + return ((float) curr_data.quat_z / 16384.0f); + } + + /** + * Returns the current temperature (in degrees centigrade) reported by + * the sensor's gyro/accelerometer circuit. + *

    + * This value may be useful in order to perform advanced temperature- + * correction of raw gyroscope and accelerometer values. + *

    + * + * @return The current temperature (in degrees centigrade). + */ + public float getTempC() { + return curr_data.mpu_temp; + } + + /** + * Returns information regarding which sensor board axis (X,Y or Z) and + * direction (up/down) is currently configured to report Yaw (Z) angle + * values. NOTE: If the board firmware supports Omnimount, the board yaw + * axis/direction are configurable. + *

    + * For more information on Omnimount, please see: + *

    + * http://navx-mxp.kauailabs.com/navx-mxp/installation/omnimount/ + *

    + * + * @return The currently-configured board yaw axis/direction. + */ + public BoardYawAxis getBoardYawAxis() { + BoardYawAxis yaw_axis = new BoardYawAxis(); + short yaw_axis_info = (short) (board_state.capability_flags >> 3); + yaw_axis_info &= 7; + if (yaw_axis_info == AHRSProtocol.OMNIMOUNT_DEFAULT) { + yaw_axis.up = true; + yaw_axis.board_axis = BoardAxis.kBoardAxisZ; + } else { + yaw_axis.up = (yaw_axis_info & 0x01) != 0; + yaw_axis_info >>= 1; + switch ((byte) yaw_axis_info) { + case 0: + yaw_axis.board_axis = BoardAxis.kBoardAxisX; + break; + case 1: + yaw_axis.board_axis = BoardAxis.kBoardAxisY; + break; + case 2: + default: + yaw_axis.board_axis = BoardAxis.kBoardAxisZ; + break; + } + } + return yaw_axis; + } + + /** + * Returns the version number of the firmware currently executing + * on the sensor. + *

    + * To update the firmware to the latest version, please see: + *

    + * http://navx-mxp.kauailabs.com/navx-mxp/support/updating-firmware/ + *

    + * + * @return The firmware version in the format [MajorVersion].[MinorVersion] + */ + public String getFirmwareVersion() { + double version_number = (double) board_id.fw_ver_major; + version_number += ((double) board_id.fw_ver_minor / 10); + String fw_version = Double.toString(version_number); + return fw_version; + } + + /** + * Returns the current raw (unprocessed) X-axis gyro rotation rate (in degrees/sec). NOTE: this + * value is un-processed, and should only be accessed by advanced users. + * Typically, rotation about the X Axis is referred to as "Pitch". Calibrated + * and Integrated Pitch data is accessible via the {@link #getPitch()} method. + *

    + * + * @return Returns the current rotation rate (in degrees/sec). + */ + public float getRawGyroX() { + return this.raw_data_update.gyro_x / (DEV_UNITS_MAX / (float) this.board_state.gyro_fsr_dps); + } + + /** + * Returns the current raw (unprocessed) Y-axis gyro rotation rate (in degrees/sec). NOTE: this + * value is un-processed, and should only be accessed by advanced users. + * Typically, rotation about the T Axis is referred to as "Roll". Calibrated + * and Integrated Pitch data is accessible via the {@link #getRoll()} method. + *

    + * + * @return Returns the current rotation rate (in degrees/sec). + */ + public float getRawGyroY() { + return this.raw_data_update.gyro_y / (DEV_UNITS_MAX / (float) this.board_state.gyro_fsr_dps); + } + + /** + * Returns the current raw (unprocessed) Z-axis gyro rotation rate (in degrees/sec). NOTE: this + * value is un-processed, and should only be accessed by advanced users. + * Typically, rotation about the T Axis is referred to as "Yaw". Calibrated + * and Integrated Pitch data is accessible via the {@link #getYaw()} method. + *

    + * + * @return Returns the current rotation rate (in degrees/sec). + */ + public float getRawGyroZ() { + return this.raw_data_update.gyro_z / (DEV_UNITS_MAX / (float) this.board_state.gyro_fsr_dps); + } + + /** + * Returns the current raw (unprocessed) X-axis acceleration rate (in G). NOTE: this + * value is unprocessed, and should only be accessed by advanced users. This raw value + * has not had acceleration due to gravity removed from it, and has not been rotated to + * the world reference frame. Gravity-corrected, world reference frame-corrected + * X axis acceleration data is accessible via the {@link #getWorldLinearAccelX()} method. + *

    + * + * @return Returns the current acceleration rate (in G). + */ + public float getRawAccelX() { + return this.raw_data_update.accel_x / (DEV_UNITS_MAX / (float) this.board_state.accel_fsr_g); + } + + /** + * Returns the current raw (unprocessed) Y-axis acceleration rate (in G). NOTE: this + * value is unprocessed, and should only be accessed by advanced users. This raw value + * has not had acceleration due to gravity removed from it, and has not been rotated to + * the world reference frame. Gravity-corrected, world reference frame-corrected + * Y axis acceleration data is accessible via the {@link #getWorldLinearAccelY()} method. + *

    + * + * @return Returns the current acceleration rate (in G). + */ + public float getRawAccelY() { + return this.raw_data_update.accel_y / (DEV_UNITS_MAX / (float) this.board_state.accel_fsr_g); + } + + /** + * Returns the current raw (unprocessed) Z-axis acceleration rate (in G). NOTE: this + * value is unprocessed, and should only be accessed by advanced users. This raw value + * has not had acceleration due to gravity removed from it, and has not been rotated to + * the world reference frame. Gravity-corrected, world reference frame-corrected + * Z axis acceleration data is accessible via the {@link #getWorldLinearAccelZ()} method. + *

    + * + * @return Returns the current acceleration rate (in G). + */ + public float getRawAccelZ() { + return this.raw_data_update.accel_z / (DEV_UNITS_MAX / (float) this.board_state.accel_fsr_g); + } + + /** + * Returns the current raw (unprocessed) X-axis magnetometer reading (in uTesla). NOTE: + * this value is unprocessed, and should only be accessed by advanced users. This raw value + * has not been tilt-corrected, and has not been combined with the other magnetometer axis + * data to yield a compass heading. Tilt-corrected compass heading data is accessible + * via the {@link #getCompassHeading()} method. + *

    + * + * @return Returns the mag field strength (in uTesla). + */ + public float getRawMagX() { + return this.raw_data_update.mag_x / UTESLA_PER_DEV_UNIT; + } + + /** + * Returns the current raw (unprocessed) Y-axis magnetometer reading (in uTesla). NOTE: + * this value is unprocessed, and should only be accessed by advanced users. This raw value + * has not been tilt-corrected, and has not been combined with the other magnetometer axis + * data to yield a compass heading. Tilt-corrected compass heading data is accessible + * via the {@link #getCompassHeading()} method. + *

    + * + * @return Returns the mag field strength (in uTesla). + */ + public float getRawMagY() { + return this.raw_data_update.mag_y / UTESLA_PER_DEV_UNIT; + } + + /** + * Returns the current raw (unprocessed) Z-axis magnetometer reading (in uTesla). NOTE: + * this value is unprocessed, and should only be accessed by advanced users. This raw value + * has not been tilt-corrected, and has not been combined with the other magnetometer axis + * data to yield a compass heading. Tilt-corrected compass heading data is accessible + * via the {@link #getCompassHeading()} method. + *

    + * + * @return Returns the mag field strength (in uTesla). + */ + public float getRawMagZ() { + return this.raw_data_update.mag_z / UTESLA_PER_DEV_UNIT; + } + + /** + * Returns the current barometric pressure (in millibar) [navX Aero only]. + *

    + * This value is valid only if a barometric pressure sensor is onboard. + * + * @return Returns the current barometric pressure (in millibar). + */ + public float getPressure() { + // TODO implement for navX-Aero. + return 0; + } + + public DimStateTracker getDimStateTrackerInstance() { + if (global_dim_state_tracker == null) { + global_dim_state_tracker = new DimStateTracker(); + } + return global_dim_state_tracker; + } + + /** + * Identifies one of the three sensing axes on the navX sensor board. Note that these axes are + * board-relative ("Board Frame"), and are not necessarily the same as the logical axes of the + * chassis on which the sensor is mounted. + *

    + * For more information on sensor orientation, please see the navX sensor + * Orientation page. + */ + public enum BoardAxis { + kBoardAxisX(0), + kBoardAxisY(1), + kBoardAxisZ(2); + + private int value; + + BoardAxis(int value) { + this.value = value; + } + + public int getValue() { + return this.value; + } + } + + /** + * The DeviceDataType specifies the + * type of data to be retrieved from the sensor. Due to limitations in the + * communication bandwidth, only a subset of all available data can be streamed + * and still maintain a 50Hz update rate via the Core Device Interface Module, + * since it is limited to a maximum of one 26-byte transfer every 10ms. + * Note that if all data types are required, + */ + public enum DeviceDataType { + /** + * (default): All 6 and 9-axis processed data, sensor status and timestamp + */ + kProcessedData(0), + /** + * Unit Quaternions and unprocessed data from each individual sensor. Note that + * this data does not include a timestamp. If a timestamp is needed, use + * the kAll flag instead. + */ + kQuatAndRawData(1), + /** + * All processed and raw data, sensor status and timestamps. Note that on a + * Android-based FTC robot using the "Core Device Interface Module", acquiring + * all data requires to I2C transactions. + */ + kAll(3); + + private int value; + + DeviceDataType(int value) { + this.value = value; + } + + public int getValue() { + return this.value; + } + } + + enum DimState { + UNKNOWN, + WAIT_FOR_MODE_CONFIG_COMPLETE, + WAIT_FOR_I2C_TRANSFER_COMPLETION, + WAIT_FOR_HOST_BUFFER_TRANSFER_COMPLETION, + READY + } + + interface IoCallback { + boolean ioComplete(boolean read, int address, int len, byte[] data); + } + + /** + * Indicates which sensor board axis is used as the "yaw" (gravity) axis. + *

    + * This selection may be modified via the Omnimount feature. + */ + static public class BoardYawAxis { + public BoardAxis board_axis; + public boolean up; + } + + private class BoardState { + public short capability_flags; + public byte update_rate_hz; + public short accel_fsr_g; + public short gyro_fsr_dps; + public byte op_status; + public byte cal_status; + public byte selftest_status; + } + + class navXIOThread implements Runnable, AHRS.IoCallback { + + final int NAVX_REGISTER_FIRST = IMURegisters.NAVX_REG_WHOAMI; + final int NAVX_REGISTER_PROC_FIRST = IMURegisters.NAVX_REG_SENSOR_STATUS_L; + final int NAVX_REGISTER_RAW_FIRST = IMURegisters.NAVX_REG_QUAT_W_L; + final int I2C_TIMEOUT_MS = 500; + protected boolean keep_running; + int dim_port; + int update_rate_hz; + boolean request_zero_yaw; + boolean is_connected; + int byte_count; + int update_count; + DeviceDataType data_type; + AHRSProtocol.AHRSPosUpdate ahrspos_update; + long curr_sensor_timestamp; + boolean cancel_all_reads; + boolean first_bank; + long last_valid_sensor_timestamp; + int duplicate_sensor_data_count; + int last_second_hertz; + int hertz_counter; + Object io_thread_event; + + public navXIOThread(int port, int update_rate_hz, DeviceDataType data_type, + AHRSProtocol.AHRSPosUpdate ahrspos_update) { + this.dim_port = port; + this.keep_running = false; + this.update_rate_hz = update_rate_hz; + this.request_zero_yaw = false; + this.is_connected = false; + this.byte_count = 0; + this.update_count = 0; + this.ahrspos_update = ahrspos_update; + this.data_type = data_type; + this.cancel_all_reads = false; + this.first_bank = true; + this.last_valid_sensor_timestamp = 0; + this.duplicate_sensor_data_count = 0; + this.last_second_hertz = 0; + this.hertz_counter = 0; + this.io_thread_event = new Object(); + + android.os.Process.setThreadPriority(Process.THREAD_PRIORITY_MORE_FAVORABLE); + } + + public void start() { + keep_running = true; + } + + public void stop() { + keep_running = false; + signalThread(); + } + + public void zeroYaw() { + request_zero_yaw = true; + signalThread(); + } + + public int getByteCount() { + return byte_count; + } + + public int getDuplicateDataCount() { + return duplicate_sensor_data_count; + } + + public void addToByteCount(int new_byte_count) { + byte_count += new_byte_count; + } + + public int getUpdateCount() { + return update_count; + } + + public void incrementUpdateCount() { + update_count++; + } + + public boolean isConnected() { + return is_connected; + } + + public void setConnected(boolean new_connected) { + is_connected = new_connected; + if (!is_connected) { + signalThread(); + } + } + + private void signalThread() { + synchronized (io_thread_event) { + io_thread_event.notify(); + } + } + + public boolean ioComplete(boolean read, int address, int len, byte[] data) { + boolean restart = false; + long system_timestamp = SystemClock.elapsedRealtime(); + if (address == NAVX_REGISTER_PROC_FIRST) { + if (!decodeNavxProcessedData(data, + NAVX_REGISTER_PROC_FIRST, data.length)) { + setConnected(false); + } else { + if (curr_sensor_timestamp != last_valid_sensor_timestamp) { + addToByteCount(len); + incrementUpdateCount(); + for (int i = 0; i < callbacks.length; i++) { + IDataArrivalSubscriber callback = callbacks[i]; + if (callback != null) { + callback.timestampedDataReceived(system_timestamp, + curr_sensor_timestamp, + DeviceDataType.kProcessedData); + } + } + if (data_type == DeviceDataType.kAll) { + signalThread(); + first_bank = false; + } + } else { + duplicate_sensor_data_count++; + } + + if ((curr_sensor_timestamp % 1000) < (last_valid_sensor_timestamp % 1000)) { + /* Second roll over. Start the Hertz accumulator */ + last_second_hertz = hertz_counter; + hertz_counter = 1; + } else { + hertz_counter++; + } + + last_valid_sensor_timestamp = curr_sensor_timestamp; + if (data_type == DeviceDataType.kProcessedData) { + if (!cancel_all_reads) { + restart = true; + } + } else if (data_type == DeviceDataType.kAll) { + signalThread(); + } + } + } else if (address == this.NAVX_REGISTER_RAW_FIRST) { + if (!decodeNavxQuatAndRawData(data, + NAVX_REGISTER_RAW_FIRST, len)) { + setConnected(false); + } else { + addToByteCount(len); + incrementUpdateCount(); + for (int i = 0; i < callbacks.length; i++) { + IDataArrivalSubscriber callback = callbacks[i]; + if (callback != null) { + callback.untimestampedDataReceived(system_timestamp, + DeviceDataType.kQuatAndRawData); + } + } + if (data_type == DeviceDataType.kQuatAndRawData) { + if (!cancel_all_reads) { + restart = true; + } + } else if (data_type == DeviceDataType.kAll) { + first_bank = true; + } + } + } + + return restart; + } + + @Override + public void run() { + + final int DIM_MAX_I2C_READ_LEN = 26; + final int NAVX_WRITE_COMMAND_BIT = 0x80; + DimI2cDeviceReader navxReader[] = new DimI2cDeviceReader[3]; + I2cDevice navXDevice = null; + DimI2cDeviceWriter navxUpdateRateWriter = null; + DimI2cDeviceWriter navxZeroYawWriter = null; + + byte[] update_rate_command = new byte[1]; + update_rate_command[0] = (byte) update_rate_hz; + byte[] zero_yaw_command = new byte[1]; + zero_yaw_command[0] = AHRSProtocol.NAVX_INTEGRATION_CTL_RESET_YAW; + + if (enable_logging) { + Log.i("navx_ftc", "Opening device on DIM port " + Integer.toString(dim_port)); + } + navXDevice = new I2cDevice(dim, dim_port); + + navxReader[0] = new DimI2cDeviceReader(navXDevice, NAVX_I2C_DEV_8BIT_ADDRESS, + NAVX_REGISTER_FIRST, DIM_MAX_I2C_READ_LEN); + navxReader[1] = new DimI2cDeviceReader(navXDevice, NAVX_I2C_DEV_8BIT_ADDRESS, + NAVX_REGISTER_PROC_FIRST, DIM_MAX_I2C_READ_LEN); + navxReader[2] = new DimI2cDeviceReader(navXDevice, NAVX_I2C_DEV_8BIT_ADDRESS, + NAVX_REGISTER_RAW_FIRST, DIM_MAX_I2C_READ_LEN); + + /* The board state reader uses synchronous I/O. The processed and raw data + readers use an asynchronous IO Completion mechanism. + */ + navxReader[1].registerIoCallback(this); + navxReader[2].registerIoCallback(this); + + setConnected(false); + + while (keep_running) { + try { + if (!is_connected) { + this.last_second_hertz = 0; + this.last_valid_sensor_timestamp = 0; + this.byte_count = 0; + this.update_count = 0; + this.first_bank = true; + this.hertz_counter = 0; + this.duplicate_sensor_data_count = 0; + byte[] board_data = navxReader[0].startAndWaitForCompletion(I2C_TIMEOUT_MS); + if (board_data != null) { + if (decodeNavxBoardData(board_data, NAVX_REGISTER_FIRST, board_data.length)) { + setConnected(true); + first_bank = true; + + /* To handle the case where the device is reset, reconfigure the */ + /* update rate whenever reconecting to the device. */ + navxUpdateRateWriter = new DimI2cDeviceWriter(navXDevice, + NAVX_I2C_DEV_8BIT_ADDRESS, + NAVX_WRITE_COMMAND_BIT | IMURegisters.NAVX_REG_UPDATE_RATE_HZ, + update_rate_command); + navxUpdateRateWriter.waitForCompletion(I2C_TIMEOUT_MS); + + board_data = navxReader[0].startAndWaitForCompletion(I2C_TIMEOUT_MS); + if ((board_data == null) || + !decodeNavxBoardData(board_data, NAVX_REGISTER_FIRST, board_data.length)) { + setConnected(false); + } + } + } + } else { + /* If connected, read sensor data and optionally zero yaw if requested */ + if (request_zero_yaw) { + + /* if any reading is underway, wait for it to complete. */ + cancel_all_reads = true; + if (navxReader[1].isBusy()) { + navxReader[1].waitForCompletion(I2C_TIMEOUT_MS); + } + if (navxReader[2].isBusy()) { + navxReader[2].waitForCompletion(I2C_TIMEOUT_MS); + } + cancel_all_reads = false; + + navxZeroYawWriter = new DimI2cDeviceWriter(navXDevice, + NAVX_I2C_DEV_8BIT_ADDRESS, + NAVX_WRITE_COMMAND_BIT | IMURegisters.NAVX_REG_INTEGRATION_CTL, + zero_yaw_command); + navxZeroYawWriter.waitForCompletion(I2C_TIMEOUT_MS); + request_zero_yaw = false; + } + + /* Read Processed Data (kProcessedData or kAll) */ + + if ((data_type == DeviceDataType.kProcessedData) || + ((data_type == DeviceDataType.kAll) && first_bank)) { + if (!navxReader[1].isBusy()) { + navxReader[1].start(I2C_TIMEOUT_MS, + (data_type == DeviceDataType.kProcessedData)); + } + } + + /* Read Quaternion/Raw Data (kQuatAndRawData or kAll) */ + + if ((data_type == DeviceDataType.kQuatAndRawData) || + ((data_type == DeviceDataType.kAll) && !first_bank)) { + if (!navxReader[2].isBusy()) { + navxReader[2].start(I2C_TIMEOUT_MS, + (data_type == DeviceDataType.kQuatAndRawData)); + } + } + } + /* Wait for a disconnect, read completion or shutdown event */ + synchronized (io_thread_event) { + try { + io_thread_event.wait(50); + } catch (InterruptedException ex) { + ex.printStackTrace(); + } + } + } catch (Exception ex) { + } + } + + /* Thread shutdown requested. */ + /* Cancel any pending IO, and wait for them to complete (or timeout) */ + cancel_all_reads = true; + + if (navxReader[1].isBusy()) { + navxReader[1].waitForCompletion(I2C_TIMEOUT_MS); + } + if (navxReader[2].isBusy()) { + navxReader[2].waitForCompletion(I2C_TIMEOUT_MS); + } + + navxReader[1].deregisterIoCallback(this); + navxReader[2].deregisterIoCallback(this); + + global_dim_state_tracker.reset(); + + if (enable_logging) { + Log.i("navx_ftc", "Closing I2C device."); + } + navXDevice.close(); + } + + boolean decodeNavxBoardData(byte[] curr_data, int first_address, int len) { + final int I2C_NAVX_DEVICE_TYPE = 50; + boolean valid_data; + if (curr_data[IMURegisters.NAVX_REG_WHOAMI - first_address] == I2C_NAVX_DEVICE_TYPE) { + valid_data = true; + board_id.hw_rev = curr_data[IMURegisters.NAVX_REG_HW_REV - first_address]; + board_id.fw_ver_major = curr_data[IMURegisters.NAVX_REG_FW_VER_MAJOR - first_address]; + board_id.fw_ver_minor = curr_data[IMURegisters.NAVX_REG_FW_VER_MINOR - first_address]; + board_id.type = curr_data[IMURegisters.NAVX_REG_WHOAMI - first_address]; + + board_state.gyro_fsr_dps = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_GYRO_FSR_DPS_L - first_address); + board_state.accel_fsr_g = (short) curr_data[IMURegisters.NAVX_REG_ACCEL_FSR_G - first_address]; + board_state.update_rate_hz = curr_data[IMURegisters.NAVX_REG_UPDATE_RATE_HZ - first_address]; + board_state.capability_flags = AHRSProtocol.decodeBinaryUint16(curr_data, IMURegisters.NAVX_REG_CAPABILITY_FLAGS_L - first_address); + board_state.op_status = curr_data[IMURegisters.NAVX_REG_OP_STATUS - first_address]; + board_state.selftest_status = curr_data[IMURegisters.NAVX_REG_SELFTEST_STATUS - first_address]; + board_state.cal_status = curr_data[IMURegisters.NAVX_REG_CAL_STATUS - first_address]; + } else { + valid_data = false; + } + return valid_data; + } + + boolean doesDataAppearValid(byte[] curr_data) { + boolean data_valid = false; + boolean all_zeros = true; + boolean all_ones = true; + for (int i = 0; i < curr_data.length; i++) { + if (curr_data[i] != (byte) 0) { + all_zeros = false; + } + if (curr_data[i] != (byte) 0xFF) { + all_ones = false; + } + if (!all_zeros && !all_ones) { + data_valid = true; + break; + } + } + return data_valid; + } + + boolean decodeNavxProcessedData(byte[] curr_data, int first_address, int len) { + long timestamp_low, timestamp_high; + + boolean data_valid = doesDataAppearValid(curr_data); + if (!data_valid) { + Arrays.fill(curr_data, (byte) 0); + } + curr_sensor_timestamp = (long) AHRSProtocol.decodeBinaryUint32(curr_data, IMURegisters.NAVX_REG_TIMESTAMP_L_L - first_address); + ahrspos_update.sensor_status = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_L - first_address]; + /* Update calibration status from the "shadow" in the upper 8-bits of sensor status. */ + ahrspos_update.cal_status = curr_data[IMURegisters.NAVX_REG_SENSOR_STATUS_H - first_address]; + ahrspos_update.yaw = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_YAW_L - first_address); + ahrspos_update.pitch = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_PITCH_L - first_address); + ahrspos_update.roll = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_ROLL_L - first_address); + ahrspos_update.compass_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_HEADING_L - first_address); + ahrspos_update.fused_heading = AHRSProtocol.decodeProtocolUnsignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_FUSED_HEADING_L - first_address); + ahrspos_update.altitude = AHRSProtocol.decodeProtocol1616Float(curr_data, IMURegisters.NAVX_REG_ALTITUDE_I_L - first_address); + ahrspos_update.linear_accel_x = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_X_L - first_address); + ahrspos_update.linear_accel_y = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Y_L - first_address); + ahrspos_update.linear_accel_z = AHRSProtocol.decodeProtocolSignedThousandthsFloat(curr_data, IMURegisters.NAVX_REG_LINEAR_ACC_Z_L - first_address); + + return data_valid; + } + + boolean decodeNavxQuatAndRawData(byte[] curr_data, int first_address, int len) { + boolean data_valid = doesDataAppearValid(curr_data); + if (!data_valid) { + Arrays.fill(curr_data, (byte) 0); + } + ahrspos_update.quat_w = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_W_L - first_address); + ahrspos_update.quat_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_X_L - first_address); + ahrspos_update.quat_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Y_L - first_address); + ahrspos_update.quat_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_QUAT_Z_L - first_address); + + ahrspos_update.mpu_temp = AHRSProtocol.decodeProtocolSignedHundredthsFloat(curr_data, IMURegisters.NAVX_REG_MPU_TEMP_C_L - first_address); + + raw_data_update.gyro_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_X_L - first_address); + raw_data_update.gyro_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Y_L - first_address); + raw_data_update.gyro_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_GYRO_Z_L - first_address); + raw_data_update.accel_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_X_L - first_address); + raw_data_update.accel_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Y_L - first_address); + raw_data_update.accel_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_ACC_Z_L - first_address); + raw_data_update.mag_x = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_X_L - first_address); + raw_data_update.mag_y = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Y_L - first_address); + /* Unfortunately, the 26-byte I2C Transfer limit means we can't transfer the Z-axis magnetometer data. */ + /* This magnetomer axis typically isn't used, so it's likely not going to be missed. */ + //raw_data_update.mag_z = AHRSProtocol.decodeBinaryInt16(curr_data, IMURegisters.NAVX_REG_MAG_Z_L-first_address); + + return data_valid; + } + + } + + public class DimStateTracker { + private boolean read_mode; + private int device_address; + private int mem_address; + private int num_bytes; + private DimState state; + + public DimStateTracker() { + reset(); + } + + public void reset() { + read_mode = false; + device_address = -1; + mem_address = -1; + num_bytes = -1; + state = DimState.UNKNOWN; + } + + public void setMode(boolean read_mode, int device_address, + int mem_address, int num_bytes) { + this.read_mode = read_mode; + this.device_address = device_address; + this.mem_address = mem_address; + this.num_bytes = num_bytes; + } + + public boolean isModeCurrent(boolean read_mode, int device_address, + int mem_address, int num_bytes) { + return ((this.read_mode == read_mode) && + (this.device_address == device_address) && + (this.mem_address == mem_address) && + (this.num_bytes == num_bytes)); + } + + public DimState getState() { + return this.state; + } + + public void setState(DimState new_state) { + this.state = new_state; + } + } + + public class DimI2cDeviceWriter { + private final I2cDevice device; + private final int dev_address; + private final int mem_address; + private final int num_bytes; + private boolean done; + private Object synchronization_event; + private DimStateTracker state_tracker; + + public DimI2cDeviceWriter(I2cDevice i2cDevice, int i2cAddress, int memAddress, byte[] data) { + this.device = i2cDevice; + this.dev_address = i2cAddress; + this.mem_address = memAddress; + this.num_bytes = data.length; + done = false; + this.synchronization_event = new Object(); + this.state_tracker = getDimStateTrackerInstance(); + i2cDevice.copyBufferIntoWriteBuffer(data); + i2cDevice.registerForI2cPortReadyCallback(new I2cController.I2cPortReadyCallback() { + public void portIsReady(int port) { + DimI2cDeviceWriter.this.portDone(); + } + }); + if (enable_logging) { + Log.i("navx_ftc", "Writer registerForPortReadyCallback"); + } + if (!state_tracker.isModeCurrent(false, dev_address, mem_address, data.length)) { + this.state_tracker.setMode(false, dev_address, mem_address, data.length); + state_tracker.setState(DimState.WAIT_FOR_MODE_CONFIG_COMPLETE); + device.enableI2cWriteMode(i2cAddress, memAddress, data.length); + if (enable_logging) { + Log.i("navx_ftc", "Writer enableI2cWiteMode"); + } + + } else { + state_tracker.setState(DimState.WAIT_FOR_I2C_TRANSFER_COMPLETION); + device.setI2cPortActionFlag(); + device.writeI2cCacheToController(); + if (enable_logging) { + Log.i("navx_ftc", "Writer setI2cPortActionFlag/writeI2cCacheToController"); + } + } + } + + public boolean isDone() { + return this.done; + } + + private void portDone() { + DimState dim_state = state_tracker.getState(); + switch (dim_state) { + + case WAIT_FOR_MODE_CONFIG_COMPLETE: + state_tracker.setState(DimState.WAIT_FOR_I2C_TRANSFER_COMPLETION); + device.setI2cPortActionFlag(); + device.writeI2cCacheToController(); + if (enable_logging) { + Log.i("navx_ftc", "Writer WAIT_FOR_MODE_CONFIG_COMPLETE - " + + Integer.toString(this.mem_address) + ", " + + Integer.toString(this.num_bytes)); + } + break; + + case WAIT_FOR_I2C_TRANSFER_COMPLETION: + state_tracker.setState(DimState.READY); + device.deregisterForPortReadyCallback(); + if (enable_logging) { + Log.i("navx_ftc", "Writer WAIT_FOR_I2C_TRANSFER_COMPLETION; deregisterForPortReadyCallback()"); + } + done = true; + synchronized (synchronization_event) { + synchronization_event.notify(); + } + break; + } + } + + public boolean waitForCompletion(long timeout_ms) { + if (done) return true; + boolean success; + synchronized (synchronization_event) { + try { + synchronization_event.wait(timeout_ms); + success = done; + if (!success) { + Log.w("navx_ftc", "Writer waitForCompletion() timeout"); + /* Unregister the callback.v*/ + if (enable_logging) { + Log.i("navx_ftc", "Writer deregisterForPortReadyCallback due to timeout"); + } + device.deregisterForPortReadyCallback(); + } + } catch (InterruptedException ex) { + ex.printStackTrace(); + success = false; + } + } + return success; + } + } + + public class DimI2cDeviceReader { + private final I2cDevice device; + private final int dev_address; + private final int mem_address; + private final int num_bytes; + I2cController.I2cPortReadyCallback callback; + IoCallback ioCallback; + DimState dim_state; + DimStateTracker state_tracker; + long read_start_timestamp; + long timeout_ms; + private byte[] device_data; + private Object synchronization_event; + private boolean registered; + private boolean busy; + private boolean continuous_read; + private boolean continuous_first; + + public DimI2cDeviceReader(I2cDevice i2cDevice, int i2cAddress, + int memAddress, int num_bytes) { + this.ioCallback = null; + this.device = i2cDevice; + this.dev_address = i2cAddress; + this.mem_address = memAddress; + this.num_bytes = num_bytes; + this.synchronization_event = new Object(); + this.registered = false; + this.state_tracker = getDimStateTrackerInstance(); + this.busy = false; + this.continuous_read = false; + this.continuous_first = false; + this.callback = new I2cController.I2cPortReadyCallback() { + public void portIsReady(int port) { + portDone(); + } + }; + } + + public void registerIoCallback(IoCallback ioCallback) { + if (enable_logging) { + Log.i("navx_ftc", "Reader Registering reader IO Callbacks."); + } + this.ioCallback = ioCallback; + } + + public void deregisterIoCallback(IoCallback ioCallback) { + this.ioCallback = null; + if (enable_logging) { + Log.i("navx_ftc", "Reader Deregistering reader IO Callbacks."); + } + } + + public void start(long timeout_ms, boolean continuous) { + start_internal(timeout_ms, continuous, true); + } + + private void start_internal(long timeout_ms, boolean continuous, boolean first) { + device_data = null; + this.continuous_read = continuous; + this.continuous_first = continuous && first; + DimState dim_state = state_tracker.getState(); + + /* Start a countdown timer, in case the IO doesn't complete as expected. */ + this.timeout_ms = timeout_ms; + read_start_timestamp = SystemClock.elapsedRealtime(); + + if (!registered) { + if (enable_logging) { + Log.i("navx_ftc", "Reader registerForI2cPortReadyCallback"); + } + device.registerForI2cPortReadyCallback(callback); + registered = true; + } + if (state_tracker.getState() == DimState.UNKNOWN || + (!state_tracker.isModeCurrent(true, dev_address, mem_address, num_bytes))) { + /* Force a read-mode transition (if address changed, or of was in write mode) */ + state_tracker.setMode(true, dev_address, mem_address, num_bytes); + state_tracker.setState(DimState.WAIT_FOR_MODE_CONFIG_COMPLETE); + device.enableI2cReadMode(dev_address, mem_address, num_bytes); + if (enable_logging) { + Log.i("navx_ftc", "Reader enableI2cReadMode"); + } + } else { + if (!device.isI2cPortReady() || !device.isI2cPortInReadMode()) { + boolean fail = true; + } + /* Already in read mode at the correct address. Initiate the I2C Bus Read. */ + state_tracker.setState(DimState.WAIT_FOR_I2C_TRANSFER_COMPLETION); + if (first || !continuous_read) { + device.setI2cPortActionFlag(); + device.writeI2cCacheToController(); + if (enable_logging) { + Log.i("navx_ftc", "Reader setI2cPortActionFlag/writeI2cCacheToController"); + } + } else { + /* In this case, the I2C Bus Read was previously initiated in the portDone() + callback function, in the WAIT_FOR_I2C_TRANSFER_COMPLETION case. */ + } + } + busy = true; + } + + public boolean isBusy() { + long busy_period = SystemClock.elapsedRealtime() - read_start_timestamp; + if (busy && (busy_period >= this.timeout_ms)) { + if (enable_logging) { + Log.w("navx_ftc", "Reader TIMEOUT!!!"); + } + busy = false; + state_tracker.reset(); + } + return busy; + } + + private void portDone() { + + DimState dim_state = state_tracker.getState(); + boolean fallthrough = false; + switch (dim_state) { + + case WAIT_FOR_MODE_CONFIG_COMPLETE: + /* The controller is now in Read Mode with the specified address/len. */ + device.setI2cPortActionFlag(); + state_tracker.setState(DimState.WAIT_FOR_I2C_TRANSFER_COMPLETION); + device.writeI2cCacheToController(); + if (enable_logging) { + Log.i("navx_ftc", "Reader WAIT_FOR_MODE_CONFIG_COMPLETE - " + + Integer.toString(this.mem_address) + ", " + + Integer.toString(this.num_bytes)); + } + break; + + case WAIT_FOR_I2C_TRANSFER_COMPLETION: + state_tracker.setState(DimState.WAIT_FOR_HOST_BUFFER_TRANSFER_COMPLETION); + device.readI2cCacheFromController(); + if (enable_logging) { + Log.i("navx_ftc", "Reader WAIT_FOR_I2C_TRANSFER_COMPLETION - " + + (continuous_read ? "Continuous " : "") + + (continuous_first ? "First" : "")); + } + if (continuous_read) { + /* Piggy-back the request for the next read. */ + /* For this to work, the write cache already has the read address/len */ + /* configured, and the only thing needed to trigger the IO is to change */ + /* the "port action" flag. This is a very useful technique, since it */ + /* allows another I2C read to start at the same time the read cache is */ + /* being transferred from the controller over USB; Setting only the */ + /* port action flag avoid over-writing the other bytes in the cache, */ + /* which by may (race condition) have already been updated with the */ + /* data read in the just-completed I2C read transaction. */ + device.setI2cPortActionFlag(); + device.writeI2cPortFlagOnlyToController(); /* Write *only* the flag. */ + if (continuous_first) { + continuous_first = false; + break; + } else { + /* Fall through to WAIT_FOR_HOST_BUFFER_TRANSFER_COMPLETION case */ + fallthrough = true; + } + } else { + break; + } + + case WAIT_FOR_HOST_BUFFER_TRANSFER_COMPLETION: + /* The Read Cache has been successfully transferred to this host. */ + device_data = this.device.getCopyOfReadBuffer(); + boolean restarted = false; + boolean notify = false; + + if (enable_logging) { + Log.i("navx_ftc", "Reader WAIT_FOR_HOST_BUFFER_TRANSFER_COMPLETION. " + + (fallthrough ? "(Fallthrough)" : "") + + ((device_data != null) ? " Valid Data" : " Null Data")); + } + + if (this.ioCallback != null) { + boolean repeat = ioCallback.ioComplete(true, this.mem_address, + this.num_bytes, device_data); + device_data = null; + if (repeat) { + start_internal(timeout_ms, continuous_read, false); + restarted = true; + } else { + state_tracker.setState(DimState.READY); + busy = false; + continuous_read = false; + notify = true; + } + } else { + state_tracker.setState(DimState.READY); + busy = false; + continuous_read = false; + notify = true; + } + if (!restarted) { + if (enable_logging) { + Log.i("navx_ftc", "Reader deregisterForPortReadyCallback"); + } + device.deregisterForPortReadyCallback(); + registered = false; + } + /* Finally, after all other processing is complete, notify waiters. */ + if (notify) { + synchronized (synchronization_event) { + synchronization_event.notify(); + } + } + + break; + } + } + + public byte[] startAndWaitForCompletion(long timeout_ms) { + start(timeout_ms, false); + return waitForCompletion(timeout_ms); + } + + public byte[] waitForCompletion(long timeout_ms) { + if (!busy && (device_data == null)) return device_data; + byte[] data; + synchronized (synchronization_event) { + try { + synchronization_event.wait(timeout_ms); + data = device_data; + if (busy && (data == null)) { + Log.w("navx_ftc", "Reader waitForCompletion() timeout"); + /* Unregister the callback. */ + if (enable_logging) { + Log.i("navx_ftc", "Reader deregisterForPortReadyCallback due to timeout"); + } + device.deregisterForPortReadyCallback(); + registered = false; + } + } catch (InterruptedException ex) { + ex.printStackTrace(); + data = null; + } + } + return data; + } + + public byte[] getReadBuffer() { + return device_data; + } + } +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/ftc/IDataArrivalSubscriber.java b/lib-navx/src/main/java/com/kauailabs/navx/ftc/IDataArrivalSubscriber.java new file mode 100644 index 0000000..bdcf299 --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/IDataArrivalSubscriber.java @@ -0,0 +1,79 @@ +/* ============================================ + NavX-MXP and NavX-Micro source code is placed under the MIT license + Copyright (c) 2015 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ + +package com.kauailabs.navx.ftc; + +/** + * The IDataArrivalSubscriber interface provides a method for consumers + * of navX-Model device data to be rapidly notified whenever new data + * has arrived. + *

    + * Two separate "callback" functions are provided: + *

    + * - timesetampedDataReceived(): reception of sensor-timestamped data + * - untimestampedDataReceived(): reception of data without a sensor- + * provided timestamp. + *

    + * Both timestamps are at millisecond resolution. A "system timestamp" + * is provided in both casees, which represents as accurately as possible + * the time at which this process acquired the data. + *

    + * If available, sensor timestamps are preferred, as they are generated + * by a real-time system and thus have a greater accuracy than the + * system timestamp which is vulnerable to latencies introduced by the + * non-realtime Android operating system. + *

    + * To support data retrieval of various different types of data from a + * single data publisher, a "kind" object is also provided which can be + * used to indicate the type of data being referred to in the + * notification. + *

    + * This callback is invoked by the AHRS class whenever new data is r + * eceived from the sensor. Note that this callback is occurring + * within the context of the AHRS class IO thread, and it may + * interrupt the thread running this opMode. Therefore, it is + * very important to use thread synchronization techniques when + * communicating between this callback and the rest of the + * code in this opMode. + *

    + * The sensor_timestamp is accurate to 1 millisecond, and reflects + * the time that the data was actually acquired. This callback will + * only be invoked when a sensor data sample newer than the last + * is received, so it is guaranteed that the same data sample will + * not be delivered to this callback more than once. + *

    + * If the sensor is reset for some reason, the sensor timestamp + * will be reset to 0. Therefore, if the sensor timestamp is ever + * less than the previous sample, this indicates the sensor has + * been reset. + *

    + * In order to be called back, this interface must be registered + * via the AHRS registerCallback() method. + */ + +public interface IDataArrivalSubscriber { + void untimestampedDataReceived(long system_timestamp, Object kind); + + void timestampedDataReceived(long system_timestamp, long sensor_timestamp, Object kind); +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java new file mode 100644 index 0000000..97a7b16 --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java @@ -0,0 +1,649 @@ +/* ============================================ + NavX-MXP and NavX-Micro source code is placed under the MIT license + Copyright (c) 2015 Kauai Labs + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. + =============================================== + */ +package com.kauailabs.navx.ftc; + +/** + * The navXPIDController implements a timestamped PID controller (designed to deal + * with the jitter which is typically present in a networked control system scenario). + *

    + * The navXPIDController can use any of the various data sources on a navX-Model device + * as an input (process variable); when instantiating a navXPIDController simply + * provide an AHRS class instance and specify which navX-Model device variable you + * wish to use as the input. Then, configure the navXPIDController's setPoint, + * outputRange, whether it should operate in continuous mode or not, and the + * P, I, D and F coefficients which will be used to calculate the output value. + *

    + * An example of using the navXPIDController to rotate a FTC robot to a target angle is + * provided online. + *

    + * The general PID algorithm used herein is discussed in detail on Wikipedia. + *

    + *

    + * In addition to the P,I,D terms, a FeedForward term is optionally available + * which may be useful in cases where velocity is being controlled (e.g., to + * achieve a continuous rotational velocity using a yaw rate gyro). The FeedForward + * concept is discussed further here. + *

    + * This algorithm implements two features with respect to the integral gain calculated + * based on the integral (i) coefficient: + *

    + * - Anti-Windup: Ensures the integral gain doesn't exceed the min/max output range, as discussed + * here. + * - Time-Correction: Adjust the integral gain in cases when timestamps indicate that + * data samples were lost. + *

    + * This algorithm implements this feature with respect to the derivative gain, as discussed + * here. + */ + +public class navXPIDController implements IDataArrivalSubscriber { + + /* Coefficients */ + protected double p; + protected double i; + protected double d; + protected double ff; + /* Error statistics */ + protected double error_current = 0.0; + protected double error_previous = 0.0; + protected double error_total = 0.0; + protected double error_d = 0; + navXTimestampedDataSource timestamped_src; + navXUntimestampedDataSource untimestamped_src; + AHRS navx_device; + long last_system_timestamp = 0; + long last_sensor_timestamp = 0; + double tolerance_amount; + private Object sync_event = new Object(); + private boolean timestamped = true; + /* Input/Output Clamps */ + private double max_input = 0.0; + private double min_input = 0.0; + private double max_output = 1; + private double min_output = -1.0; + private ToleranceType tolerance_type; + /* Behavior */ + private boolean continuous = false; + private boolean enabled = false; + private double setpoint = 0.0; + private double result = 0.0; + + /** + * This navXPIDController constructor is used when the PID Controller is to be + * driven by a navX-Model device input data source which is accompanied by a + * high-accuracy "sensor" timestamp. + *

    + * The data source specified automatically determines the navXPIDController's + * input data range. + **/ + public navXPIDController(AHRS navx_device, navXTimestampedDataSource src) { + this.navx_device = navx_device; + this.timestamped = true; + this.timestamped_src = src; + switch (src) { + case YAW: + this.setInputRange(-180.0, 180.0); + break; + case PITCH: + case ROLL: + this.setInputRange(-90.0, 90.0); + break; + case COMPASS_HEADING: + case FUSED_HEADING: + this.setInputRange(0.0, 360.0); + break; + case LINEAR_ACCEL_X: + case LINEAR_ACCEL_Y: + case LINEAR_ACCEL_Z: + this.setInputRange(-2.0, 2.0); + break; + } + navx_device.registerCallback(this); + } + + /** + * This navXPIDController constructor is used when the PID Controller is to be + * driven by a navX-Model device input data source which is not accompanied by a + * high-accuracy "sensor" timestamp. + *

    + * The data source specified automatically determines the navXPIDController's + * input data range. + **/ + public navXPIDController(AHRS navx_device, navXUntimestampedDataSource src) { + this.navx_device = navx_device; + this.timestamped = false; + this.untimestamped_src = src; + navx_device.registerCallback(this); + } + + @Override + public void untimestampedDataReceived(long curr_system_timestamp, Object kind) { + if (enabled && (!timestamped) && (kind.getClass() == AHRS.DeviceDataType.class)) { + double process_value; + switch (untimestamped_src) { + case RAW_GYRO_X: + process_value = navx_device.getRawGyroX(); + break; + case RAW_GYRO_Y: + process_value = navx_device.getRawGyroY(); + break; + case RAW_GYRO_Z: + process_value = navx_device.getRawGyroZ(); + break; + case RAW_ACCEL_X: + process_value = navx_device.getRawAccelX(); + break; + case RAW_ACCEL_Y: + process_value = navx_device.getRawAccelY(); + break; + case RAW_MAG_X: + process_value = navx_device.getRawMagX(); + break; + case RAW_MAG_Y: + process_value = navx_device.getRawMagY(); + break; + case RAW_MAG_Z: + process_value = navx_device.getRawMagZ(); + break; + default: + process_value = 0.0; + break; + } + int num_missed_samples = 0; /* TODO */ + last_system_timestamp = curr_system_timestamp; + double output = this.stepController(process_value, num_missed_samples); + synchronized (sync_event) { + sync_event.notify(); + } + } + } + + @Override + public void timestampedDataReceived(long curr_system_timestamp, + long curr_sensor_timestamp, Object kind) { + if (enabled && timestamped && (kind.getClass() == AHRS.DeviceDataType.class)) { + double process_value; + switch (timestamped_src) { + case YAW: + process_value = navx_device.getYaw(); + break; + case PITCH: + process_value = navx_device.getPitch(); + break; + case ROLL: + process_value = navx_device.getRoll(); + break; + case COMPASS_HEADING: + process_value = navx_device.getCompassHeading(); + break; + case FUSED_HEADING: + process_value = navx_device.getFusedHeading(); + break; + case LINEAR_ACCEL_X: + process_value = navx_device.getWorldLinearAccelX(); + break; + case LINEAR_ACCEL_Y: + process_value = navx_device.getWorldLinearAccelY(); + break; + case LINEAR_ACCEL_Z: + process_value = navx_device.getWorldLinearAccelZ(); + break; + default: + process_value = 0.0; + break; + } + int num_missed_samples = 0; /* TODO */ + last_system_timestamp = curr_system_timestamp; + last_sensor_timestamp = curr_sensor_timestamp; + double output = this.stepController(process_value, num_missed_samples); + synchronized (sync_event) { + sync_event.notify(); + } + } + } + + public void close() { + enable(false); + navx_device.deregisterCallback(this); + } + + /** + * isNewUpdateAvailable() should be called by clients of the navXPIDController + * which need to "poll" to determine whether new navX-Model device data has + * been received. Whether or not new data has been received, this method + * returns immediately and does not block. + *

    + * + * @return Returns true if new data has been received since the last time + * this function was called, otherwise returns false. If true, the result + * will updated to reflect the newly-calculated controller values. + */ + public boolean isNewUpdateAvailable(PIDResult result) { + boolean new_data_available; + if (enabled && + ((timestamped && (result.timestamp < this.last_sensor_timestamp)) || + (result.timestamp < this.last_system_timestamp))) { + new_data_available = true; + result.on_target = this.isOnTarget(); + result.output = this.get(); + if (timestamped) { + result.timestamp = last_sensor_timestamp; + } else { + result.timestamp = last_system_timestamp; + } + } else { + new_data_available = false; + } + return new_data_available; + } + + /** + * waitForNewUpdate() should be called by clients of the navXPIDController + * which want to "wait" until new navX-Model device data has been received. + * This method will return immediately only if new data has been received + * since the last time it was called; otherwise, it will block and not return + * until new data has been received, or a specified timeout period has passed. + *

    + * + * @return Returns true when new data has been received. If false is returned, + * this indicates a timeout has occurred while waiting for new data. + */ + public boolean waitForNewUpdate(PIDResult result, int timeout_ms) throws InterruptedException { + boolean ready = isNewUpdateAvailable(result); + if (!ready && !Thread.currentThread().isInterrupted()) { + synchronized (sync_event) { + sync_event.wait(timeout_ms); + } + ready = isNewUpdateAvailable(result); + } + return ready; + } + + /** + * Returns the current amount of error calculated by the navXPIDController + * based upon the last navX-Model device data sample. By definition, this + * error is equal to the set point minus the last device data sample. + */ + public double getError() { + return error_current; + } + + /** + * Used to specify the tolerance used to determine whether the navXPIDController + * is "on_target". The tolerance threshold is defined by the tolerance_type and + * the tolerance amount. For example, if the YAW data source is used and thus the + * values used are in units of degrees, ToleranceType.ABSOLUTE and tolerance_amount + * of 2.0 would result in the navXPIDController deciding it was "on_target" whenever + * the error was within a range of +/- 2 degrees. + */ + public synchronized void setTolerance(ToleranceType tolerance_type, double tolerance_amount) { + this.tolerance_amount = tolerance_amount; + this.tolerance_type = tolerance_type; + } + + /** + * Indicates whether the navXPIDController has determined that it is "on_target" based upon + * whether the current error is within the range defined by the tolerance. + */ + public boolean isOnTarget() { + boolean on_target = false; + switch (tolerance_type) { + case NONE: + on_target = (getError() == 0); + break; + case PERCENT: + on_target = (Math.abs(getError()) < + (tolerance_amount / 100 * (max_input - min_input))); + break; + case ABSOLUTE: + on_target = (Math.abs(getError()) < tolerance_amount); + break; + } + return on_target; + } + + /** + * stepController() is the PIDController worker function, which is used internally + * by the navXPIDController whenever a new navX-Model device sensor data value + * is received. + */ + public double stepController(double process_variable, int num_missed_samples) { + double local_result; + double i_adj; + double d_adj; + + synchronized (this) { + + error_current = setpoint - process_variable; + + /* If a continuous controller, if > 1/2 way from the target */ + /* modify the error to ensure the output doesn't change */ + /* direction, but processed onward until error is zero. */ + if (continuous) { + double range = max_input - min_input; + if (Math.abs(error_current) > (range / 2)) { + if (error_current > 0) + error_current -= range; + else + error_current += range; + } + } + + /* If samples were missed, reduce the d gain by dividing */ + /* by the total number of samples since the last update. */ + /* For more discussion on this topic, see: */ + /* http://www.diva-portal.org/smash/get/diva2:570067/FULLTEXT01.pdf */ + if (num_missed_samples > 0) { + i_adj = i / (1 + num_missed_samples); + } else { + i_adj = i; + } + /* Process integral term. Perform "anti-windup" processing */ + /* by preventing the integral term from accumulating when */ + /* the output reaches it's minimum or maximum limits. */ + + if (i != 0) { + double estimated_i = (error_total + error_current) * i_adj; + if (estimated_i < max_output) { + if (estimated_i > min_output) { + error_total += error_current; + } else { + error_total = min_output / i_adj; + } + } else { + error_total = max_output / i_adj; + } + } + + /* If samples were missed, reduce the d gain by dividing */ + /* by the total number of samples since the last update. */ + if (num_missed_samples > 0) { + d_adj = d / (1 + num_missed_samples); + } else { + d_adj = d; + } + + /* Calculate result w/P, I, D & F terms */ + error_d = error_current - error_previous; + result = p * error_current + + i_adj * error_total + + d_adj * (error_current - error_previous) + + ff * setpoint; + error_previous = error_current; + + /* Clamp result to output range */ + if (result > max_output) { + result = max_output; + } else if (result < min_output) { + result = min_output; + } + + local_result = result; + } + return local_result; + } + + /** + * setPID() is used to set the Proportional, Integral and Differential coefficients + * used by the navXPIDController. + */ + public synchronized void setPID(double p, double i, double d) { + this.p = p; + this.i = i; + this.d = d; + stepController(error_previous, 0); /* Update cached values */ + } + + /** + * setPID() is used to set the Proportional, Integral, Differential and FeedForward + * coefficients used by the navXPIDController. + */ + public synchronized void setPID(double p, double i, double d, double ff) { + this.p = p; + this.i = i; + this.d = d; + this.ff = ff; + stepController(error_previous, 0); /* Update cached values */ + } + + /** + * setContinuous() is used to enable/disable the continuous mode of operation. + *

    + * When continuous mode is disabled, the min/max input range values are used as + * two separate points at the ends of the range of possible input values. This + * mode of operation is typically used for reaching a position within a linear + * range. + *

    + * When continuous mode is enabled, the min/max input range are considered to + * represent the sampe point. This mode of operation is typically used for reaching + * a position within a circular range, and allows the navXPIDController to determine + * the shortest possible route to the setpoint. For example, when using YAW as + * the input data source, and using the PID controller to rotatie to a given angle, + * if the setpoint is 150 degrees and the current input value is -150 degrees, the + * controller will calculate an output such that it travels across the boundary + * between -180 and 180 degrees (for a total traveled distance of 60 degrees), rather + * than traveling 300 degrees as it would if continuous mode were disabled. + */ + public synchronized void setContinuous(boolean continuous) { + this.continuous = continuous; + stepController(error_previous, 0); /* Update cached values */ + } + + /** + * Returns the current output value calculated by the navXPIDController based upon the + * last navX-Model device data sample received. + */ + public synchronized double get() { + return result; + } + + /** + * Defines the range of output values calculated by the navXPIDController. + *

    + * For example, when the navXPIDController is used to calculate an output value to be + * sent to a motor controller whose valid range is -1 to 1, the output range should be + * sent to -1, 1. + *

    + * Note that the units of the output range are not necessarily the same as the units + * of the input range. + */ + public synchronized void setOutputRange(double min_output, double max_output) { + if (min_output <= max_output) { + this.min_output = min_output; + this.max_output = max_output; + } + stepController(error_previous, 0); /* Update cached values */ + } + + /** + * Defines the range of possible input values received from the currently-selected + * navX-Model device data source. For example, if YAW is the data source, the + * input range would be -180.0, 180.0. + *

    + * Note that the navXPIDController constructor automatically sets the input range + * based upon the data source specified to the constructor. + */ + public synchronized void setInputRange(double min_input, double max_input) { + if (min_input <= max_input) { + this.min_input = min_input; + this.max_input = max_input; + setSetpoint(setpoint); + } + } + + /** + * Returns the currently configured setpoint. + */ + public synchronized double getSetpoint() { + return setpoint; + } + + /** + * Defines the "target" value the navXPIDController attempts to reach. This value + * is in the same units as the input, and should be within the input range. For + * example, if YAW is the data source, the setput should be between -180.0 and 180.0. + */ + public synchronized void setSetpoint(double setpoint) { + if (max_input > min_input) { + if (setpoint > max_input) { + this.setpoint = max_input; + } else if (setpoint < min_input) { + this.setpoint = min_input; + } else { + this.setpoint = setpoint; + } + } else { + this.setpoint = setpoint; + } + stepController(error_previous, 0); /* Update cached values */ + } + + /** + * Enables/disables the PID controller. By default, the navXPIDController is + * disabled, thus this method must be invoked before attempting to + * use the navXPIDController's output values. + */ + public synchronized void enable(boolean enabled) { + this.enabled = enabled; + if (!enabled) { + reset(); + } + } + + /** + * Returns true if the navXPIDController is currently enabled, otherwise + * return false. + */ + public synchronized boolean isEnabled() { + return this.enabled; + } + + /** + * Resets the PIDController's current error value as well as the integrated + * error. Also disables the controller. The enable() method must be used + * to re-enable the controller. + */ + public synchronized void reset() { + this.enabled = false; + error_current = 0.0; + error_previous = 0.0; + error_total = 0.0; + result = 0.0; + } + + public enum TimestampType {SENSOR, SYSTEM} + + /** + * The navXTimestampedDataSources specifies the navX-Model device + * sensor data source type used by the navXPIDController as it's input data source. + * These data sources are timestamped by the navX-Model device and thus are delivered + * with sufficient data (a highly-accurate "sensor timestamp") to allow the + * navXPIDController to compensate for any jitter in the transmission from the + * navX-Model device to the navXPIDController. + */ + public enum navXTimestampedDataSource { + YAW, + PITCH, + ROLL, + COMPASS_HEADING, + FUSED_HEADING, + ALTITUDE, + LINEAR_ACCEL_X, + LINEAR_ACCEL_Y, + LINEAR_ACCEL_Z + } + + /** + * The navXUntimestampedDataSources specifies the navX-Model device + * sensor data source type used by the navXPIDController as it's input data source. + * These data sources are timestamped with the Android "system" timestamp only, and + * thus may not have sufficient data to allow the navXPIDController to compensate + * for any jitter in the transmission from the navX-Model device to the navXPIDController. + */ + public enum navXUntimestampedDataSource { + RAW_GYRO_X, + RAW_GYRO_Y, + RAW_GYRO_Z, + RAW_ACCEL_X, + RAW_ACCEL_Y, + RAW_ACCEL_Z, + RAW_MAG_X, + RAW_MAG_Y, + RAW_MAG_Z + } + + /** + * The ToleranceType enumeration defines the type of tolerance to be used by the + * navXPIDController to determine whether the controller is "on_target". + */ + public enum ToleranceType { + NONE, PERCENT, ABSOLUTE + } + + /** + * The PIDResult class encapsulates the data used by the navXPIDController to + * communicate current state to a client of the navXPIDController. The client + * creates the instance of the PIDResult, and continually provides it to the + * navxPIDController's waitForNewUpdate() and isNewDataAvailable() methods, + * depending upon whether the client wishes to block (wait) for new updates, + * or periodically poll to determine when new data is available. + */ + + static public class PIDResult { + public double output; + public long timestamp; + public boolean on_target; + + public PIDResult() { + output = 0.0; + timestamp = 0; + on_target = false; + } + + /** + * Returns the timestamp of the last data sample processed by the + * navXPIDController. + */ + public long getTimestamp() { + return timestamp; + } + + /** + * Returns true if the navXPIDController indicated that it is currently + * "on target" as defined by the configured tolerances and the most + * recent input data sample. + */ + public boolean isOnTarget() { + return on_target; + } + + /** + * Returns the output value calculated by the navXPIDController which + * corresponds to the most recent input data sample. + */ + public double getOutput() { + return output; + } + } +} diff --git a/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPerformanceMonitor.java b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPerformanceMonitor.java new file mode 100644 index 0000000..afcde3f --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPerformanceMonitor.java @@ -0,0 +1,211 @@ +package com.kauailabs.navx.ftc; + +import com.qualcomm.robotcore.util.ElapsedTime; + +/** + * The navXPerformanceMonitor class is designed to provide performance + * data to help tune the navx_ftc library's AHRS class to retrieve + * navX-Model device data using tuning parameters appropriate for a + * particular FTC robotic controller configuration. + *

    + * For an example of using the navXPerformanceMonitor class, see the + * + * navX Performance Tuning Opmode + */ +public class navXPerformanceMonitor implements IDataArrivalSubscriber { + + final int MS_PER_SEC = 1000; + final int NAVX_TIMESTAMP_JITTER_MS = 2; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + private long last_system_timestamp = 0; + private long last_sensor_timestamp = 0; + private long sensor_timestamp_delta = 0; + private long system_timestamp_delta = 0; + private byte sensor_update_rate_hz = 40; + private int missing_sensor_sample_count = 0; + private int estimated_missing_sensor_sample_count = 0; + private boolean first_sample_received = false; + private int hertz_counter = 0; + private int last_second_hertz = 0; + private int NORMAL_DIM_TRANSFER_ITTER_MS = 10; + + /** + * Constructor for the navXPerformanceMonitor class. + * + * @param navx_device The instance of the navX-Model device to monitor. + */ + public navXPerformanceMonitor(AHRS navx_device) { + this.navx_device = navx_device; + reset(); + } + + /** + * Resets the peformance monitoring statistics. + */ + public void reset() { + last_system_timestamp = 0; + last_sensor_timestamp = 0; + sensor_timestamp_delta = 0; + system_timestamp_delta = 0; + sensor_update_rate_hz = 40; + missing_sensor_sample_count = 0; + first_sample_received = false; + hertz_counter = 0; + last_second_hertz = 0; + } + + /** + * The delivered rate is the rate at which samples are delivered + * to the Android-based FTC robotics control application. + * + * @return The rate of navX-Model device sample delivery in Hz. + */ + public int getDeliveredRateHz() { + return last_second_hertz; + } + + /** + * The sensor tate is the rate at which the navX-Model device sensor + * is currently configured to deliver sample. This rate may be + * greater than the delivered rate. + * + * @return The rate at which the navX-Model device is currently configured + * to generate samples, in Hz. + */ + public int getSensorRateHz() { + return navx_device.getActualUpdateRate(); + } + + /** + * The rate at which the Core Device Interface Module (DIM) is currently + * delivering data samples to the navx_ftc library IO thread. + * + * @return The current DIM transfer rate in Hz. + */ + public int getDimTransferRateHz() { + return navx_device.getCurrentTransferRate(); + } + + /** + * The number of samples which were expected to be received from the + * navX-Model device which never arrived, since the last time the + * navXPerformanceMonitor's statistics were reset. + * + * @return The number of navX-Model device samples not received. + */ + public int getNumMissedSensorTimestampedSamples() { + return missing_sensor_sample_count; + } + + /** + * The number of samples which were expected to be received from the + * navX-Model device which are believed to have not arrived based upon + * the Android OS system timestamp. Note that this timestamp is not + * as accurate as the navX-Model device sensor timestamp, and thus the + * number of missed unstimestamped samples is an estimate, and is not + * deterministic. + * + * @return The estimated number of navX-Model device untimestamped + * samples not received, since the last time the navXPerformanceMonitor's + * statistics were reset. + */ + public int getNumEstimatedMissedUntimestampedSamples() { + return estimated_missing_sensor_sample_count; + } + + /** + * The last sensor timestamp delta indicates the period of time between + * reception of the last two samples from the navX-Model device, for those + * data samples which are timestamped with a sensor timestamp. + * + * @return The last sensor timestamp delta, in milliseconds. + */ + public long getLastSensorTimestampDeltaMS() { + return sensor_timestamp_delta; + } + + /** + * The last system timestamp delta indicates the period of time between + * reception of the last two samples from the navX-Model device, for those + * data samples which do not provide a corresponding sensor timestamp and + * are instead timestamped using the Android OS system timestamp. + * + * @return The last system timestamp delta, in milliseconds. + */ + public long getLastSystemTimestampDeltaMS() { + return system_timestamp_delta; + } + + @Override + public void untimestampedDataReceived(long curr_system_timestamp, Object kind) { + byte sensor_update_rate = navx_device.getActualUpdateRate(); + long num_dropped = 0; + system_timestamp_delta = curr_system_timestamp - last_system_timestamp; + int expected_sample_time_ms = MS_PER_SEC / (int) sensor_update_rate; + + if (!navx_device.isConnected()) { + reset(); + } else { + if ((curr_system_timestamp % 1000) < (last_system_timestamp % 1000)) { + /* Second roll over. Start the Hertz accumulator */ + last_second_hertz = hertz_counter; + hertz_counter = 1; + } else { + hertz_counter++; + } + if (!first_sample_received) { + last_sensor_timestamp = curr_system_timestamp; + first_sample_received = true; + estimated_missing_sensor_sample_count = 0; + } else { + if (system_timestamp_delta > (expected_sample_time_ms + NORMAL_DIM_TRANSFER_ITTER_MS)) { + long estimated_dropped_samples = (system_timestamp_delta / expected_sample_time_ms) - 1; + if (estimated_dropped_samples > 0) { + estimated_missing_sensor_sample_count += estimated_dropped_samples; + } + } + } + } + + last_system_timestamp = curr_system_timestamp; + } + + @Override + public void timestampedDataReceived(long curr_system_timestamp, + long curr_sensor_timestamp, + Object kind) { + long num_dropped = 0; + byte sensor_update_rate = navx_device.getActualUpdateRate(); + sensor_timestamp_delta = curr_sensor_timestamp - last_sensor_timestamp; + system_timestamp_delta = curr_system_timestamp - last_system_timestamp; + int expected_sample_time_ms = MS_PER_SEC / (int) sensor_update_rate; + + if (!navx_device.isConnected()) { + reset(); + } else { + if ((curr_system_timestamp % 1000) < (last_system_timestamp % 1000)) { + /* Second roll over. Start the Hertz accumulator */ + last_second_hertz = hertz_counter; + hertz_counter = 1; + } else { + hertz_counter++; + } + if (!first_sample_received) { + last_sensor_timestamp = curr_sensor_timestamp; + first_sample_received = true; + missing_sensor_sample_count = 0; + } else { + if (sensor_timestamp_delta > (expected_sample_time_ms + NAVX_TIMESTAMP_JITTER_MS)) { + long dropped_samples = (sensor_timestamp_delta / expected_sample_time_ms) - 1; + if (dropped_samples > 0) { + missing_sensor_sample_count += dropped_samples; + } + } + } + } + + last_sensor_timestamp = curr_sensor_timestamp; + last_system_timestamp = curr_system_timestamp; + } +} diff --git a/lib-navx/src/main/java/overview.htm b/lib-navx/src/main/java/overview.htm new file mode 100644 index 0000000..3efe2e8 --- /dev/null +++ b/lib-navx/src/main/java/overview.htm @@ -0,0 +1,70 @@ + + + + + + + + + + + +

    + +

    The navX-Device Java Library for Android is based upon the Java + FTC "RobotCore" Library, and provides an interface to the navX-MXP and navX-Micro "Attitude/Heading + Reference System" (AHRS) functionality.

    + +

    To get started w/the navX-Device Java Library for FTC, + please see the Getting + Started online documentation.

    + +

    The AHRS Class is the primary class used to access navX-Device + AHRS functionality.  To use the AHRS class, instantiate the class using one of + the constructors.

    + +

    For source code which demonstrates + using the AHRS class in Java, please see the navX-Device FTC Examples.

    + +
    + + + + diff --git a/lib-navx/src/main/res/values/strings.xml b/lib-navx/src/main/res/values/strings.xml new file mode 100644 index 0000000..06f3d86 --- /dev/null +++ b/lib-navx/src/main/res/values/strings.xml @@ -0,0 +1,3 @@ + + navx_ftc + diff --git a/ftc-library/libs/Analytics-release.aar b/libs/Analytics-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/Analytics-release.aar rename to libs/Analytics-release.aar diff --git a/ftc-library/libs/FtcCommon-release.aar b/libs/FtcCommon-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/FtcCommon-release.aar rename to libs/FtcCommon-release.aar diff --git a/ftc-library/libs/Hardware-release.aar b/libs/Hardware-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/Hardware-release.aar rename to libs/Hardware-release.aar diff --git a/ftc-library/libs/ModernRobotics-release.aar b/libs/ModernRobotics-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/ModernRobotics-release.aar rename to libs/ModernRobotics-release.aar diff --git a/ftc-library/libs/README.txt b/libs/README.txt old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/README.txt rename to libs/README.txt diff --git a/ftc-library/libs/RobotCore-release.aar b/libs/RobotCore-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/RobotCore-release.aar rename to libs/RobotCore-release.aar diff --git a/ftc-library/libs/WirelessP2p-release.aar b/libs/WirelessP2p-release.aar old mode 100755 new mode 100644 similarity index 100% rename from ftc-library/libs/WirelessP2p-release.aar rename to libs/WirelessP2p-release.aar diff --git a/sample/build.gradle b/sample/build.gradle index d0d2242..bc83e1f 100644 --- a/sample/build.gradle +++ b/sample/build.gradle @@ -22,13 +22,11 @@ repositories { dependencies { compile files('libs/android-support-v4.jar') - compile(name: 'RobotCore-release', ext: 'aar') compile(name: 'Hardware-release', ext: 'aar') compile(name: 'FtcCommon-release', ext: 'aar') compile(name: 'ModernRobotics-release', ext: 'aar') compile(name: 'Analytics-release', ext: 'aar') compile(name: 'WirelessP2p-release', ext: 'aar') - compile project(':ftc-library') } diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java new file mode 100644 index 0000000..ff7c79d --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java @@ -0,0 +1,190 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.qualcomm.ftccommon.DbgLog; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.Range; + +import java.util.Iterator; +import java.util.TreeMap; + +/** + * Automatically tunes PID parameters + */ +public class AutoTunePID extends LinearOpMode { + protected static DcMotor motorFR; + protected static DcMotor motorFL; + protected static DcMotor motorBR; + protected static DcMotor motorBL; + protected static volatile double[] rollAngle = new double[2], pitchAngle = new double[2], yawAngle = new double[2]; + protected double kP = 0.06; + protected double kI = 0.02; + protected double kD = 0.005; + double dt; + double prevError; + double error = 0; + double iError = 0; + double dError; + double time = getRuntime(); + double target_heading; + TreeMap time_displacement = new TreeMap(); + NavXDevice navx; + + public void resetPID() throws InterruptedException { + if (!opModeIsActive()) + return; + if (Math.abs(target_heading - getGyroYaw() + 360) < Math.abs(target_heading - getGyroYaw())) + error = target_heading - getGyroYaw() + 360; + else + error = target_heading - getGyroYaw(); + time = getRuntime(); + iError = 0; + } + + public double tune_PID(double timeout) throws InterruptedException { + long timer = (long) (timeout * Math.pow(10, 3)) + System.currentTimeMillis(); + double kP = 0.01; // start value. It IS too small. + //double max; + target_heading = 10; + resetPID(); + while (!isTuned(kP, 5) && System.currentTimeMillis() < timer) { + kP *= 2; + target_heading = (target_heading == 10 ? 0 : 10); + } + kP /= 2; + while (!isTuned(kP, 5) && System.currentTimeMillis() < timer) { + kP *= 1.1; + target_heading = (target_heading == 10 ? 0 : 10); + } + kP /= 1.1; + while (!isTuned(kP, 5) && System.currentTimeMillis() < timer) { + kP += .0005; + target_heading = (target_heading == 10 ? 0 : 10); + } + kP -= .0005; + Iterator it = time_displacement.keySet().iterator(); + double Tu = 0; // Tu is the period of the oscillation. T meaning period in physics. + double previous = it.next(); + //finding the average period + //note: next-previous is one half period... so needs to be doubled at the end + while (it.hasNext()) { + double next = it.next(); + Tu += next - previous; + previous = next; + } + Tu = Tu * 2 / (time_displacement.size() - 1); // calculate period based off of some number of half-periods + this.kP = 0.6 * kP; + this.kI = 2 * this.kP / Tu; + this.kD = this.kP * Tu / 8; + return time_displacement.get(previous); // returns the amplitude of the + } + + public double getGyroYaw() throws InterruptedException { + return navx.getHeading(); + } + + public double get_PID() throws InterruptedException { + return get_PID(getGyroYaw(), kP, kI, kD); + } + + public double get_PID(double gyro) throws InterruptedException { + return get_PID(gyro, kP, kI, kD); + } + + public double get_PID(double kP, double kI, double kD) throws InterruptedException { + return get_PID(getGyroYaw(), kP, kI, kD); + } + + public double get_PID(double gyro, double kP, double kI, double kD) throws InterruptedException { + if (!opModeIsActive()) + return 0; + dt = getRuntime() - time; + time = getRuntime(); + prevError = error; + if (Math.abs(target_heading - gyro + 360) < Math.abs(target_heading - gyro)) + error = target_heading - gyro + 360; + else + error = target_heading - gyro; + dError = (error - prevError) / dt; + //make this a reimann right sum if needed to improve speed at the cost of accuracy + iError = Range.clip(iError + 0.5 * (prevError + error) * dt, -125, 125) * 0.99; // a trapezoidal approximation of the integral. + return kP * error + kD * dError + kI * iError; + } + + public boolean isTuned(double kP, double timeout) throws InterruptedException { + //TODO: add debug log info for debugging + time_displacement = new TreeMap(); + long timer = (long) (timeout * Math.pow(10, 3)) + System.currentTimeMillis(); + resetStartTime(); + double PID_change; + double right; + double left; + double new_yaw = getGyroYaw(); + resetPID(); + double sign = Math.signum(get_PID(kP, 0, 0)); + while (System.currentTimeMillis() < timer) { + while (new_yaw == getGyroYaw()) { + waitOneFullHardwareCycle(); + } + new_yaw = getGyroYaw(); + PID_change = get_PID(new_yaw, kP, 0, 0); + if (sign != Math.signum(PID_change)) { + time_displacement.put(getRuntime(), error); + sign *= -1; + } + //keep the absolute value of the motors above 0.3 and less than 0.7 + right = Range.clip(PID_change, -1, 1); + left = -right; + setRightPower(right); + setLeftPower(left); + DbgLog.error(String.format("kP: %.4f", kP)); + DbgLog.error(time_displacement.toString()); + DbgLog.error(String.format("k*error: %.2f, error: %.2f", kP * error, error)); + DbgLog.error(String.format("gyro:%.2f, target:%.2f, PID (right):%.2f", getGyroYaw(), target_heading, PID_change)); + } + DbgLog.error(String.format("time_displacement.size(): %d", time_displacement.size())); + DbgLog.error(time_displacement.toString()); + if (time_displacement.size() < 3) + return false; + double max = Double.MIN_VALUE; + double min = Double.MAX_VALUE; + for (Double aDouble : time_displacement.keySet()) { + double next = Math.abs(time_displacement.get(aDouble)); // measuring amplitude differentials. + max = Math.max(max, next); + min = Math.min(min, next); + } + DbgLog.error(String.format("max-min: %.2f", max - min)); + return max - min <= 2; + } + + void setLeftPower(double power) { + if (!opModeIsActive()) + return; + motorBL.setPower(Range.clip(-power, -1, 1)); + motorFL.setPower(Range.clip(-power, -1, 1)); + } + + void setRightPower(double power) { + if (!opModeIsActive()) + return; + motorBR.setPower(Range.clip(power, -1, 1)); + motorFR.setPower(Range.clip(power, -1, 1)); + } + + public void runOpMode() throws InterruptedException { + motorFR = hardwareMap.dcMotor.get("rf"); + motorBR = hardwareMap.dcMotor.get("rb"); + motorFL = hardwareMap.dcMotor.get("lf"); + motorBL = hardwareMap.dcMotor.get("lb"); + navx = new NavXDevice(hardwareMap, "dim", 0); + waitForStart(); + tune_PID(120); + + DbgLog.error("kP: %.5f, kI: %.5f, kD: %.5f", kP, kI, kD); + telemetry.addData("hi", String.format(" kP: %.5f, kI: %.5f, kD: %.5f", kP, kI, kD)); + //kP = 0.01061 + //kI = 0.02116 + //kD = 0.00133 + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java new file mode 100644 index 0000000..05acddb --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java @@ -0,0 +1,156 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.MotorInfo; +import com.lasarobotics.library.nav.PID; +import com.lasarobotics.library.util.Log; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * Basic navigational test - driving forward and rotation + */ +public class BasicEncoderTest extends OpMode { + private static final double WHEEL_RADIUS = 4.0 / 2.0; + private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 1; //2; + + //private static final double PID_PU = 0.001; //0.006 //decrease to increase smoothness + //private static final double PID_TU = 346.0; //0.108 //increase to decrease oscillation + + //lower P to increase smoothness of approach + //do NOT mess with I - you don't want this term at all + private static final double PID_P = 0.0009;//0.0009 PID_PU * 0.5; //0.05 + private static final double PID_I = 0.0;//1.2 * PID_P / PID_TU; //0.01 + private static final double PID_D = 0.0;//PID_P * PID_TU / 8.0; //0.005 + private static final double DISTANCE = 1.5; //distance in feet + private static final Units.Distance DISTANCE_UNIT = Units.Distance.FEET; + private static final double MIN_POWER = 0; + + EncodedMotor frontLeft, frontRight, backLeft, backRight; + PID pidLeft, pidRight; + Log log; + + long lastTime = 0; + long timeStart = 0; + int phase = 0; + + private double coerce(double power) { + return MathUtil.deadband(MIN_POWER, MathUtil.coerce(-1, 1, power)); + } + + @Override + public void init() { + log = new Log("/FTC_Logs/", "PID"); + + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("lf"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("rf"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("lb"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); //set wheel radius for distance calculations + backRight = new EncodedMotor(hardwareMap.dcMotor.get("rb"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); + + backLeft.setDirection(DcMotor.Direction.FORWARD); + frontLeft.setDirection(DcMotor.Direction.FORWARD); + backRight.setDirection(DcMotor.Direction.REVERSE); + frontRight.setDirection(DcMotor.Direction.REVERSE); + + backLeft.setTargetPosition(DISTANCE, DISTANCE_UNIT); + backRight.setTargetPosition(DISTANCE, DISTANCE_UNIT); + frontLeft.setTargetPosition(DISTANCE, DISTANCE_UNIT); + frontRight.setTargetPosition(DISTANCE, DISTANCE_UNIT); + + //Create PID looper + pidLeft = new PID(); + pidLeft.setSetpoint(Units.Distance.convertToAngle(DISTANCE, + WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, + WHEEL_RADIUS_UNIT, DISTANCE_UNIT, Units.Angle.ENCODER_COUNTS)); + //pidLeft.setMaxAcceleration(PID_MAX_ACCEL); + pidLeft.setCoefficients(PID_P, PID_I, PID_D); + + pidRight = new PID(); + pidRight.setSetpoint(Units.Distance.convertToAngle(DISTANCE, + WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, + WHEEL_RADIUS_UNIT, DISTANCE_UNIT, Units.Angle.ENCODER_COUNTS)); + //pidRight.setMaxAcceleration(PID_MAX_ACCEL); + pidRight.setCoefficients(PID_P, PID_I, PID_D); + } + + @Override + public void init_loop() { + lastTime = System.nanoTime(); + timeStart = lastTime; + phase = 0; + } + + @Override + public void loop() { + //Because OpMode is asynchoronous, you have to call .update() on every EncodedMotor + backLeft.update(); + frontLeft.update(); + backRight.update(); + frontRight.update(); + + switch (phase) { + case 0: + long time = System.nanoTime(); + double timeDelta = (time - lastTime) / 1000000000.0; + + double leftPos = (backLeft.getCurrentPosition() + + frontLeft.getCurrentPosition()) / 2; + double rightPos = (backRight.getCurrentPosition() + + frontRight.getCurrentPosition()) / 2; + + telemetry.addData("Pos", leftPos + ", " + rightPos); + + pidLeft.addMeasurement(leftPos, timeDelta); + pidRight.addMeasurement(rightPos, timeDelta); + + telemetry.addData("PID", pidLeft.getOutputValue() + ", " + pidRight.getOutputValue()); + telemetry.addData("PID P", PID_P); + telemetry.addData("PID I", PID_I); + telemetry.addData("PID D", PID_D); + telemetry.addData("Target", backLeft.getTargetPosition()); + telemetry.addData("Current", (leftPos + rightPos) / 2); + telemetry.addData("Target Feet", backLeft.getTargetPosition(Units.Distance.FEET)); + + double power = (pidLeft.getOutputValue() + pidRight.getOutputValue()) / 2; + //double powerCompensation = 0; + + telemetry.addData("PID Power", power); + log.add("Time", time - timeStart); + log.add("PID Power", power); + log.add("Position Left", leftPos); + + double left = power; + double right = power; + + left = coerce(left); + right = coerce(right); + + telemetry.addData("Power", left + ", " + right); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); + + //if (left == 0 && right == 0) + // phase = 1; + return; + case 1: + backLeft.setPowerFloat(); + frontLeft.setPowerFloat(); + backRight.setPowerFloat(); + frontRight.setPowerFloat(); + return; + } + } + + @Override + public void stop() { + Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0, 0); + log.saveAs(Log.FileType.CSV, true); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java index 6e29758..d8b59e6 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,49 +1,253 @@ package com.qualcomm.ftcrobotcontroller.opmodes; +import com.kauailabs.navx.ftc.navXPIDController; import com.lasarobotics.library.controller.Controller; -import com.lasarobotics.library.skynet.EncodedMotor; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.MotorInfo; +import com.lasarobotics.library.nav.PID; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; +import com.qualcomm.ftccommon.DbgLog; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; -public class EncoderTest extends OpMode { +import java.text.DecimalFormat; + +public class EncoderTest extends OpMode implements NavXDataReceiver { + + //Figure out how many encoder counts you need to go one unit of distance + private static final double ENCODER_COUNTS_PER_UNIT_DISTANCE = 1010.0; + + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 0; //port on device interface module + + private static final double NAVX_TARGET_ANGLE_DEGREES = 90.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.005; //0.009; //.01 + private static final double NAVX_YAW_PID_I = 0.0; //0.0002; //.0005 + private static final double NAVX_YAW_PID_D = 0.0; //0.11; //.05 + + private static final double PID_P = 0.0009; + private static final double PID_I = 0.0; + private static final double PID_D = 0.0; + private static final double PID_MAX_ACCEL = 0; //3 + private static final double PID_MAX_DECEL = 0; + + private static final double DISTANCE_FEET = 1; //distance in feet + private static final double MIN_POWER = 0; //distance in feet + + private static final double FORWARD_ROTATION_FEROCITY = 4; //how ferociously should we rotate? - lower if oscillating + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + NavXDevice navx; + NavXPIDController yawPIDController; + NavXPIDController.PIDState yawPIDState; EncodedMotor frontLeft, frontRight, backLeft, backRight; Controller one; + PID pidLeft; + PID pidRight; + + long lastTime = 0; + int phase = 0; public void init() { - frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft")); - frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight")); - backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft")); - backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight")); + MotorInfo motorInfo = new MotorInfo(ENCODER_COUNTS_PER_UNIT_DISTANCE, Units.Distance.FEET); + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("lf"), motorInfo); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("rf"), motorInfo); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("lb"), motorInfo); //set wheel radius for distance calculations + backRight = new EncodedMotor(hardwareMap.dcMotor.get("rb"), motorInfo); + + backLeft.setDirection(DcMotor.Direction.FORWARD); + frontLeft.setDirection(DcMotor.Direction.FORWARD); + backRight.setDirection(DcMotor.Direction.REVERSE); + frontRight.setDirection(DcMotor.Direction.REVERSE); one = new Controller(gamepad1); //TODO these are all currently ASYNCHRONOUS //TODO meaning that they can only really be called from init() or a state machine - frontLeft.moveDistance(1000); - frontRight.moveDistance(1000); - backLeft.moveDistance(1000); - backRight.moveDistance(1000); - - frontLeft.setPower(50); - frontRight.setPower(50); - backLeft.setPower(50); - backRight.setPower(50); + backLeft.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + backRight.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + frontLeft.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + frontRight.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + + //Create PID looper + pidLeft = new PID(); + pidLeft.setSetpointDistance(backLeft.getMotorInfo(), DISTANCE_FEET, Units.Distance.FEET); + pidLeft.setMaxAcceleration(PID_MAX_ACCEL); + pidLeft.setMaxDeceleration(PID_MAX_DECEL); + pidLeft.setCoefficients(PID_P, PID_I, PID_D); + + pidRight = new PID(); + pidRight.setSetpointDistance(backRight.getMotorInfo(), DISTANCE_FEET, Units.Distance.FEET); + pidRight.setMaxAcceleration(PID_MAX_ACCEL); + pidRight.setMaxDeceleration(PID_MAX_DECEL); + pidRight.setCoefficients(PID_P, PID_I, PID_D); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.reset(); + navx.registerCallback(this); + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + yawPIDController.setSetpoint(NAVX_TARGET_ANGLE_DEGREES); + //Allow crossing over the bounds (see setContinuous() documentation) + yawPIDController.setContinuous(true); + //Set angle tolerance + //yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + //Set P,I,D coefficients + yawPIDController.setPID(NAVX_YAW_PID_P, NAVX_YAW_PID_I, NAVX_YAW_PID_D); + //Disable antistall (more accurate, and since this is only used for compensation, we can ignore the stalls) + yawPIDController.disableAntistall(); + //Making the tolerance very small makes the robot work hard to get to get to a very close estimate + yawPIDController.setTolerance(navXPIDController.ToleranceType.NONE, 0); + //Start data collection + yawPIDController.start(); + + //Initiate blank PID state + yawPIDState = new NavXPIDController.PIDState(); + } + + public void init_loop() { + lastTime = System.nanoTime(); + } + + private double coerce(double power) { + return MathUtil.deadband(MIN_POWER, MathUtil.coerce(-1, 1, power)); } public void loop() { + long time = System.nanoTime(); + double timeDelta = (time - lastTime) / 1000000000.0; one.update(gamepad1); + backLeft.update(); + backRight.update(); + frontLeft.update(); + frontRight.update(); + + double leftPos = (backLeft.getCurrentPosition() + + frontLeft.getCurrentPosition()) / 2; + double rightPos = (backRight.getCurrentPosition() + + frontRight.getCurrentPosition()) / 2; + + pidLeft.addMeasurement(leftPos, timeDelta); + pidRight.addMeasurement(rightPos, timeDelta); + + double power = (pidLeft.getOutputValue() + pidRight.getOutputValue()) / 2; + double powerCompensation = 0; + + navx.displayTelemetry(telemetry); + if (yawPIDController.isUpdateAvailable(yawPIDState)) { + telemetry.addData("NavX Debug Coefficients", yawPIDController.getCoefficientDebug()); + DbgLog.error(yawPIDController.getCoefficientDebug()); + if (!yawPIDState.isOnTarget()) { + powerCompensation = yawPIDState.getOutput(); + } + } + + //Divide test into 3 segments + //1 Rotate to angle + //2 Move and rotate to angle + //3 Rotate to angle + + double left = 0; + double right = 0; + + + /*switch (phase) { + case 0: //rotate to angle + case 2: + left = coerce(-powerCompensation); + right = coerce(powerCompensation); - if (frontLeft.hasReachedPosition(1000)) - frontLeft.setPower(0); - if (frontRight.hasReachedPosition(1000)) - frontRight.setPower(0); - if (backLeft.hasReachedPosition(1000)) - backLeft.setPower(0); - if (backRight.hasReachedPosition(1000)) - backRight.setPower(0); + if (MathUtil.equal(powerCompensation, 0)) { + if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) + phase = 3; + else + phase = 1; + } else + break; + case 1: //move and rotate + if (phase != 3) { + left = MathUtil.coerce(-1, 1, power) + + Math.sin(MathUtil.coerce(-1, 1, powerCompensation) * Math.PI / 2); + right = MathUtil.coerce(-1, 1, power) - + Math.sin(MathUtil.coerce(-1, 1, powerCompensation) * Math.PI / 2); + left = coerce(left); + right = coerce(right); + + if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { + phase = 2; + break; + } + //TODO fix this - must go back to 2 immediately + else + break; + } + case 3: //coast + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + backLeft.setPowerFloat(); + backRight.setPowerFloat(); + }*/ + telemetry.addData("Original Power: ", power); + telemetry.addData("Target: ", pidLeft.getSetpoint()); + telemetry.addData("Wheel Radius (m): ", frontLeft.getMotorInfo().getEffectiveWheelRadius(Units.Distance.METERS)); + telemetry.addData("Wheel Radius (in): ", frontLeft.getMotorInfo().getEffectiveWheelRadius(Units.Distance.INCHES)); + + double avgDist = Math.abs(pidLeft.getOutputValue() - pidLeft.getSetpoint()) + Math.abs(pidRight.getOutputValue() - pidRight.getSetpoint()); + avgDist /= 2; + telemetry.addData("avgDist", avgDist); + if (false && avgDist < 500) { + telemetry.addData("DONE", "COASTING..."); + /*backLeft.setPowerFloat(); + backRight.setPowerFloat(); + frontLeft.setPowerFloat(); + frontRight.setPowerFloat();*/ + } else { + left = MathUtil.coerce(-1, 1, power) - + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * FORWARD_ROTATION_FEROCITY) * Math.PI / 2); + right = MathUtil.coerce(-1, 1, power) + + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * FORWARD_ROTATION_FEROCITY) * Math.PI / 2); + + left = coerce(left); + right = coerce(right); + + //if (phase != 3) + Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); + } + + telemetry.addData("Left (counts): ", leftPos); + telemetry.addData("Right (counts): ", rightPos); + telemetry.addData("Motor Power: ", power); + telemetry.addData("Actual Power: ", left + ", " + right); + telemetry.addData("Time Delta: ", timeDelta); + telemetry.addData("Power Compensation", df.format(powerCompensation)); + + lastTime = time; } - public void stop() { + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("navX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("navX Jerk", df.format(navx.getJerk())); + } + public void stop() { + navx.stop(); + yawPIDController.stop(); + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + backLeft.setPowerFloat(); + backRight.setPowerFloat(); } } diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java index 0055cc2..cb79b74 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -48,6 +48,11 @@ public class FtcOpModeRegister implements OpModeRegister { public void register(OpModeManager manager) { //Custom op modes manager.register("Null", NullOp.class); + manager.register("Test PurpleBot", TestPurpleBot.class); + manager.register("Teleop PurpleBot", TeleopPurpleBot.class); + manager.register("Basic Encoder Test", BasicEncoderTest.class); + manager.register("Encoder Test", EncoderTest.class); + manager.register("Navigation Test", NavigationTest.class); //manager.register("Mecanum Prototype", MecanumPrototypeTeleop.class); manager.register("MonkeyC Do", MonkeyCDo.class); manager.register("MonkeyC Write", MonkeyCWrite.class); diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java new file mode 100644 index 0000000..4a8c337 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java @@ -0,0 +1,264 @@ +/* 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. */ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import android.util.Log; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.RollingAverage; +import com.lasarobotics.library.util.Timers; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.DcMotorController; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/* + * An example linear op mode where the robot will drive in + * a straight line (where the driving direction is guided by + * the Yaw angle from a navX-Model device). + * + * This example uses a simple PID controller configuration + * with a P coefficient, and will likely need tuning in order + * to achieve optimal performance. + * + * Note that for the best accuracy, a reasonably high update rate + * for the navX-Model sensor should be used. This example uses + * the default update rate (50Hz), which may be lowered in order + * to reduce the frequency of the updates to the drive system. + */ + +public class NavXAutoTest extends LinearOpMode { + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Model Device is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 1; + private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50; + private final double TOLERANCE_DEGREES = 1.0; + private final double MIN_MOTOR_OUTPUT_VALUE = -1; + private final double MAX_MOTOR_OUTPUT_VALUE = 1; + private final double MIN_DRIVE_POWER = 0.0; + private final double YAW_PID_P = 0.05; + private final double YAW_PID_I = 0; + private final double YAW_PID_D = 0; + DcMotor frontLeft, frontRight, backLeft, backRight, intake; + int DEVICE_TIMEOUT_MS = 500; + + Timers mTimers = new Timers(); + DecimalFormat df = new DecimalFormat("#.##"); + private NavXDevice navx; + private ElapsedTime runtime = new ElapsedTime(); + + public double limit(double a) { + return MathUtil.deadband(MIN_DRIVE_POWER, MathUtil.coerce(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE, a)); + } + + @Override + public void runOpMode() throws InterruptedException { + frontLeft = hardwareMap.dcMotor.get("frontLeft"); + frontRight = hardwareMap.dcMotor.get("frontRight"); + backLeft = hardwareMap.dcMotor.get("backLeft"); + backRight = hardwareMap.dcMotor.get("backRight"); + intake = hardwareMap.dcMotor.get("intake"); + + frontLeft.setDirection(DcMotor.Direction.REVERSE); + backLeft.setDirection(DcMotor.Direction.REVERSE); + navx = new NavXDevice(hardwareMap, "dim", NAVX_DIM_I2C_PORT); + navx.reset(); + + waitForStart(); + //driveForEncoderCounts(4500, .5); + turnToDeg(40, 1); + blockForMs(500); + driveForEncoderCounts(3900, -.5); + //turnToDeg(90); + //blockForMs(500); + //driveForEncoderCounts(500, .5); + navx.stop(); + frontLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + backLeft.setPower(0); + } + + private void blockForMs(int ms) throws InterruptedException { + mTimers.startClock("delay"); + while (mTimers.getClockValue("delay") < ms && opModeIsActive()) { + waitOneFullHardwareCycle(); + } + waitOneFullHardwareCycle(); + } + + private void driveForEncoderCounts(int encoderCounts, double drive_speed) throws InterruptedException { + + Log.d("navx", "started drive for " + encoderCounts); + /* Create a PID Controller which uses the Yaw Angle as input. */ + while (backLeft.getCurrentPosition() != 0) + backLeft.setMode(DcMotorController.RunMode.RESET_ENCODERS); + backLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + NavXPIDController yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + navx.reset(); //requestReset the NavX raw + + /* Configure the PID controller */ + yawPIDController.setSetpoint(0.0); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + yawPIDController.enable(true); + navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); + + try { + while (backLeft.getCurrentPosition() < encoderCounts && + !Thread.currentThread().isInterrupted() && opModeIsActive()) { + telemetry.addData("Encoder", backLeft.getCurrentPosition()); + if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { + if (yawPIDResult.isOnTarget()) { + frontLeft.setPower(drive_speed); + frontRight.setPower(drive_speed); + backLeft.setPower(drive_speed); + backRight.setPower(drive_speed); + } else { + double output = yawPIDResult.getOutput(); + backLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - (output * Math.signum(drive_speed)))); + frontLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - (output * Math.signum(drive_speed)))); + backRight.setPower(MathUtil.coerce(-1, 1, drive_speed + (output * Math.signum(drive_speed)))); + frontRight.setPower(MathUtil.coerce(-1, 1, drive_speed + (output * Math.signum(drive_speed)))); + } + telemetry.addData("Yaw", df.format(navx.getRotation().x)); + } else { + /* A timeout occurred */ + Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT."); + } + } + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } + yawPIDController.stop(); + frontLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + backLeft.setPower(0); + Log.d("navx", "ended drive for " + encoderCounts); + } + + private void turnToDeg(int deg, double power) throws InterruptedException { + Log.d("navx", "started turn for " + deg); + + NavXPIDController yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + navx.reset(); //requestReset the NavX yaw + + /* Configure the PID controller */ + yawPIDController.setSetpoint(deg); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + yawPIDController.enable(true); + navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); + + RollingAverage average = new RollingAverage<>(100); + double lastValue = 0.0; + + while ((Math.abs(navx.getRotation().x - deg) > TOLERANCE_DEGREES || + !(average.getAverage() < 0.01 * power && average.getSize() >= 20)) //&& abs(power) < some value + && opModeIsActive()) { + Log.d("navx", "current yaw " + navx.getRotation().x); + + if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { + double output = yawPIDResult.getOutput(); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, limit(-output * power), limit(output * power)); + telemetry.addData("Yaw", df.format(navx.getRotation().x)); + telemetry.addData("PID Power", df.format(output)); + telemetry.addData("PID Average", df.format(average.getAverage())); + + average.addValue(Math.abs(output - lastValue)); + lastValue = output; + } else { + /* A timeout occurred */ + Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT."); + } + } + Log.d("navx", "ended turn for " + deg); + frontLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + backLeft.setPower(0); + } +// private void turnToDeg(int deg) throws InterruptedException { +// Log.d("navx","started turn for " +deg); +// navx.zeroYaw(); +// /* Create a PID Controller which uses the Yaw Angle as input. */ +// navXPIDController yawPIDController = new navXPIDController( navx, +// navXPIDController.navXTimestampedDataSource.YAW); +// +// /* Configure the PID controller */ +// yawPIDController.setSetpoint(deg); +// yawPIDController.setContinuous(false); +// yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); +// yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); +// yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); +// yawPIDController.enable(true); +// navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); +// DecimalFormat df = new DecimalFormat("#.##"); +// +// while ( Math.abs(navx.getYaw() - deg) > TOLERANCE_DEGREES && !Thread.currentThread().isInterrupted()) { +// Log.d("navx", "current yaw " + navx.getYaw()); +// if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { +// if (yawPIDResult.isOnTarget()) { +// frontLeft.setPowerFloat(); +// frontRight.setPowerFloat(); +// backLeft.setPowerFloat(); +// backRight.setPowerFloat(); +// telemetry.addData("Motor Power", df.format(0.00)); +// telemetry.addData("turning",navx.getYaw()); +// } else { +// double output = yawPIDResult.getOutput(); +// Tank.motor4(frontLeft, frontRight, backLeft, backRight, +// MathUtil.coerce(-1, 1, output + .1), MathUtil.coerce(-1, 1, -output - .1)); +// telemetry.addData("Motor Power", df.format(output)); +// } +// } else { +// /* A timeout occurred */ +// Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT."); +// } +// telemetry.addData("Yaw", df.format(navx.getYaw())); +// } +// yawPIDController.close(); +// Log.d("navx", "ended turn for " + deg); +// } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavigationTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavigationTest.java new file mode 100644 index 0000000..ec7faae --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavigationTest.java @@ -0,0 +1,56 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.Navigator; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import java.text.DecimalFormat; + +public class NavigationTest extends OpMode { + + //NavX setup + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 1; //port on device interface module + + //PID setup + private static final double ROT_KP = 0.005; + private static final double ROT_KI = 0; + private static final double ROT_KD = 0; + private static final double TRANS_KP = 0.0009; + private static final double TRANS_KI = 0.0; + private static final double TRANS_KD = 0.0; + private static final double PID_MAX_ACCEL = 3; //Maximum acceleration + private static final double PID_MAX_DECEL = 0; //Maximum deceleration + private static final double ROTATION_BIAS = 4; //how ferociously should we rotate? - lower if oscillating + + //Anti-stall protection + private static final double ANTISTALL_MIN_POWER = 0; //minimum motor power + private static final double ANTISTALL_POWER_BOOST = 0; //power boost when we're not quite at the target + + //Formatting + private static final DecimalFormat df = new DecimalFormat("#.##"); + + //Navigation + NavXDevice navx; + Navigator navigator; + EncodedMotor frontLeft, frontRight, backLeft, backRight; + + long lastTime = 0; + + public void init() { + + } + + public void init_loop() { + lastTime = System.nanoTime(); + } + + public void loop() { + + } + + public void stop() { + + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorsTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorsTest.java new file mode 100644 index 0000000..0b840bc --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorsTest.java @@ -0,0 +1,29 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.android.Sensors; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +public class SensorsTest extends OpMode { + + Sensors sensors; + + public void init() { + sensors = new Sensors(); + //sensors.stop(); + } + + @Override + public void init_loop() { + loop(); + } + + public void loop() { + telemetry.addData("Linear Acceleration", sensors.getLinearAcceleration().toString()); + telemetry.addData("Integrated Velocity", sensors.getIntegratedVelocity().toString()); + telemetry.addData("Integrated Position", sensors.getIntegratedPosition().toString()); + } + + public void stop() { + sensors.stop(); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java new file mode 100644 index 0000000..97ff142 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java @@ -0,0 +1,146 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.controller.ButtonState; +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Timers; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.hardware.Servo; + +public class Teleop5998 extends OpMode { + + DcMotor frontLeft, frontRight, backLeft, backRight; + DcMotor intake, lift; + Servo slide, dump, carabiner, climber; + Controller firstController; + Controller secondController; + DcMotor goalOne, goalTwo; + double modfier = 0.25; + LiftStatus status; + Timers robotTimer; + + public void init() { + gamepad1.setJoystickDeadzone(.1F); + gamepad2.setJoystickDeadzone(.1F); + frontLeft = hardwareMap.dcMotor.get("frontLeft"); + frontRight = hardwareMap.dcMotor.get("frontRight"); + backLeft = hardwareMap.dcMotor.get("backLeft"); + backRight = hardwareMap.dcMotor.get("backRight"); + intake = hardwareMap.dcMotor.get("intake"); + goalOne = hardwareMap.dcMotor.get("goalOne"); + goalTwo = hardwareMap.dcMotor.get("goalTwo"); + lift = hardwareMap.dcMotor.get("lift"); + slide = hardwareMap.servo.get("slide"); + dump = hardwareMap.servo.get("dump"); + carabiner = hardwareMap.servo.get("carabiner"); + climber = hardwareMap.servo.get("climber"); + robotTimer = new Timers(); + firstController = new Controller(gamepad1); + secondController = new Controller(gamepad2); + slide.setPosition(.5); + dump.setPosition(.5); + climber.setPosition(1); + carabiner.setPosition(.85); + status = LiftStatus.CENTER; + lift.setDirection(DcMotor.Direction.REVERSE); + } + + public void loop() { + firstController.update(gamepad1); + secondController.update(gamepad2); + //Drive + if (firstController.x == ButtonState.PRESSED) { + if (modfier == 1) + modfier = .25; + else + modfier = 1; + } + telemetry.addData("mod", modfier); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, -1 * firstController.left_stick_y * modfier, + firstController.right_stick_y * modfier); + + //Hanging + if (firstController.right_bumper == ButtonState.HELD) { + goalOne.setPower(1); + } else if (firstController.right_trigger > 0.2) { + goalOne.setPower(-1); + } else { + goalOne.setPower(0); + } + + if (firstController.left_bumper == ButtonState.HELD) { + goalTwo.setPower(1); + } else if (firstController.left_trigger > 0.2) { + goalTwo.setPower(-1); + } else { + goalTwo.setPower(0); + } + + if (firstController.a == ButtonState.HELD) { + carabiner.setPosition(MathUtil.coerce(0, 1, carabiner.getPosition() + .005)); + } else if (firstController.y == ButtonState.HELD) { + carabiner.setPosition(MathUtil.coerce(0, 1, carabiner.getPosition() - .005)); + } + + //Intake + if (secondController.y == ButtonState.PRESSED) { + intake.setPower(1); + } else if (secondController.a == ButtonState.PRESSED) { + intake.setPower(-1); + } else if (secondController.b == ButtonState.PRESSED) { + intake.setPower(0); + } + + //Lift + if (secondController.dpad_up == ButtonState.HELD) { + lift.setPower(.6); + } else if (secondController.dpad_down == ButtonState.HELD) { + lift.setPower(-.6); + } else { + lift.setPower(0); + } + + if (secondController.right_stick_x > .5) { + slide.setPosition(MathUtil.coerce(0, 1, slide.getPosition() + .005)); + } else if (secondController.right_stick_x < -.5) { + slide.setPosition(MathUtil.coerce(0, 1, slide.getPosition() - .005)); + } else if (secondController.right_stick_y > .5) { + slide.setPosition(.5); + } + + if (secondController.left_stick_x > .5) { + dump.setPosition(MathUtil.coerce(0, 1, dump.getPosition() + .005)); + } else if (secondController.left_stick_x < -.5) { + dump.setPosition(MathUtil.coerce(0, 1, dump.getPosition() - .005)); + } else if (secondController.left_stick_y > .5) { + dump.setPosition(.5); + } + //Climbersq + if (secondController.start == ButtonState.PRESSED) { + climber.setPosition(0); + } else if (secondController.back == ButtonState.PRESSED) { + climber.setPosition(1); + } + } + + public void stop() { // make sure nothing moves after the end of the match + if (intake != null) { + intake.setPower(0); + frontLeft.setPower(0); + frontRight.setPower(0); + backLeft.setPower(0); + backRight.setPower(0); + goalOne.setPower(0); + goalTwo.setPower(0); + lift.setPower(0); + } + } + + private enum LiftStatus { + LEFT, + RIGHT, + CENTER + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TeleopPurpleBot.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TeleopPurpleBot.java new file mode 100644 index 0000000..26e737f --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TeleopPurpleBot.java @@ -0,0 +1,36 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * PurpleBot (TM) Test Teleop + */ +public class TeleopPurpleBot extends OpMode { + + DcMotor leftFront, rightFront, leftBack, rightBack; + Controller one; + + @Override + public void init() { + leftFront = hardwareMap.dcMotor.get("lf"); + rightFront = hardwareMap.dcMotor.get("rf"); + leftBack = hardwareMap.dcMotor.get("lb"); + rightBack = hardwareMap.dcMotor.get("rb"); + + leftFront.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.REVERSE); + + one = new Controller(gamepad1); + } + + @Override + public void loop() { + one.update(gamepad1); + telemetry.addData("Controller One Connected", one.isConnected()); + + Tank.motor4(leftFront, rightFront, leftBack, rightBack, one.left_stick_y, one.right_stick_y); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TestPurpleBot.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TestPurpleBot.java new file mode 100644 index 0000000..b017d55 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TestPurpleBot.java @@ -0,0 +1,39 @@ +package com.qualcomm.ftcrobotcontroller.opmodes; + +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * PurpleBot (TM) Test Teleop + */ +public class TestPurpleBot extends OpMode { + + DcMotor leftFront, rightFront, leftBack, rightBack; + Controller one; + + @Override + public void init() { + leftFront = hardwareMap.dcMotor.get("lf"); + rightFront = hardwareMap.dcMotor.get("rf"); + leftBack = hardwareMap.dcMotor.get("lb"); + rightBack = hardwareMap.dcMotor.get("rb"); + + leftFront.setDirection(DcMotor.Direction.REVERSE); + leftBack.setDirection(DcMotor.Direction.REVERSE); + } + + @Override + public void loop() { + //leftFront.setPower(0.3); + //rightFront.setPower(0.3); + + Tank.motor4(leftFront, rightFront, leftBack, rightBack, 1, 1); + } + + @Override + public void stop() { + Tank.motor4(leftFront, rightFront, leftBack, rightBack, 0, 0); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotAutoSensors.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotAutoSensors.java index 5890485..660020a 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotAutoSensors.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotAutoSensors.java @@ -119,7 +119,7 @@ public void loop() // case 0: // - // Wait for the encoders to reset. This might take multiple cycles. + // Wait for the encoders to requestReset. This might take multiple cycles. // if (have_drive_encoders_reset()) { // @@ -167,7 +167,7 @@ public void loop() // case 2: // - // Wait for the encoders to reset. This might take multiple cycles. + // Wait for the encoders to requestReset. This might take multiple cycles. // if (have_drive_encoders_reset()) { // @@ -209,7 +209,7 @@ public void loop() // case 4: // - // As soon as the drive encoders reset, begin the next state. + // As soon as the drive encoders requestReset, begin the next state. // if (have_drive_encoders_reset()) { // diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotHardware.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotHardware.java index 6084fec..082191f 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotHardware.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/PushBotHardware.java @@ -824,7 +824,7 @@ boolean has_right_drive_encoder_reached(double p_count) // /** - * Indicate whether the left drive encoder has been completely reset. + * Indicate whether the left drive encoder has been completely requestReset. */ boolean has_left_drive_encoder_reset() { // @@ -855,7 +855,7 @@ boolean has_left_drive_encoder_reset() { // /** - * Indicate whether the left drive encoder has been completely reset. + * Indicate whether the left drive encoder has been completely requestReset. */ boolean has_right_drive_encoder_reset() { // @@ -886,7 +886,7 @@ boolean has_right_drive_encoder_reset() { // /** - * Indicate whether the encoders have been completely reset. + * Indicate whether the encoders have been completely requestReset. */ boolean have_drive_encoders_reset() { // diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java new file mode 100644 index 0000000..ddd0ae4 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java @@ -0,0 +1,122 @@ +package com.qualcomm.ftcrobotcontroller.opmodes.navx; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.MotorInfo; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import java.text.DecimalFormat; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXDriveForwardPID extends OpMode implements NavXDataReceiver { + + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 1; //port on device interface module + + private static final double NAVX_TOLERANCE_DEGREES = 2.0; //degrees of tolerance for PID controllers + private static final double NAVX_TARGET_ANGLE_DEGREES = 180.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.005; + private static final double NAVX_YAW_PID_I = 0.0; + private static final double NAVX_YAW_PID_D = 0.0; + + private static final double WHEEL_RADIUS = 2; + private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + EncodedMotor frontLeft, frontRight, backLeft, backRight; //make sure these have encoders! + Controller one; + NavXDevice navx; + NavXPIDController yawPIDController; + NavXPIDController.PIDState yawPIDState; + + public void init() { + //Create motors WITH ENCODERS (highly preferable) + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + + //Instantiate controllers + one = new Controller(gamepad1); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.registerCallback(this); + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + yawPIDController.setSetpoint(NAVX_TARGET_ANGLE_DEGREES); + //Allow crossing over the bounds (see setContinuous() documentation) + yawPIDController.setContinuous(true); + //Set angle tolerance + yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + //Set P,I,D coefficients + yawPIDController.setPID(NAVX_YAW_PID_P, NAVX_YAW_PID_I, NAVX_YAW_PID_D); + //Enable antistall (less accurate, but prevents motors from stalling) + yawPIDController.enableAntistall(); + yawPIDController.setAntistallDeadband(0.02); + //Making the tolerance very small makes the robot work hard to get to get to a very close estimate + yawPIDController.setTolerance(navXPIDController.ToleranceType.NONE, 0); + //Start data collection + yawPIDController.start(); + + //Initiate blank PID state + yawPIDState = new NavXPIDController.PIDState(); + } + + @Override + public void start() { + navx.reset(); + } + + public void loop() { + one.update(gamepad1); + navx.displayTelemetry(telemetry); + + /* Drive straight forward at 1/2 of full drive speed */ + final double driveSpeed = 0.5; + + if (yawPIDController.isUpdateAvailable(yawPIDState)) { + if (yawPIDState.isOnTarget()) { + backLeft.setPower(driveSpeed); + frontLeft.setPower(driveSpeed); + backRight.setPower(driveSpeed); + frontRight.setPower(driveSpeed); + telemetry.addData("Motor Output", df.format(driveSpeed) + ", " + + df.format(driveSpeed)); + } else { + double output = yawPIDState.getOutput(); + backLeft.setPower(MathUtil.coerce(-1, 1, driveSpeed + output)); + frontLeft.setPower(MathUtil.coerce(-1, 1, driveSpeed + output)); + backRight.setPower(MathUtil.coerce(-1, 1, driveSpeed - output)); + frontRight.setPower(MathUtil.coerce(-1, 1, driveSpeed - output)); + telemetry.addData("Motor Power", df.format(MathUtil.coerce(-1, 1, driveSpeed + output)) + ", " + + df.format(MathUtil.coerce(-1, 1, driveSpeed - output))); + } + } + } + + public void stop() { + navx.stop(); + yawPIDController.stop(); + } + + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("navX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("navX Jerk", df.format(navx.getJerk())); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java new file mode 100644 index 0000000..99ed457 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java @@ -0,0 +1,101 @@ +package com.qualcomm.ftcrobotcontroller.opmodes.navx; + +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +import java.text.DecimalFormat; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXNavigationTest extends OpMode implements NavXDataReceiver { + + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 1; //port on device interface module + + private static final double NAVX_TOLERANCE_DEGREES = 2.0; //degrees of tolerance for PID controllers + private static final double NAVX_TARGET_ANGLE_DEGREES = 0.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.005; + private static final double NAVX_YAW_PID_I = 0.0; + private static final double NAVX_YAW_PID_D = 0.0; + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + DcMotor frontLeft, frontRight, backLeft, backRight; //make sure these have encoders! + Controller one; + NavXDevice navx; + NavXPIDController yawPIDController; + NavXPIDController.PIDState yawPIDState; + + public void init() { + //Create motors PREFERABLY WITH ENCODERS (important!) + frontLeft = hardwareMap.dcMotor.get("frontLeft"); + frontRight = hardwareMap.dcMotor.get("frontRight"); + backLeft = hardwareMap.dcMotor.get("backLeft"); + backRight = hardwareMap.dcMotor.get("backRight"); + + //Instantiate controllers + one = new Controller(gamepad1); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.registerCallback(this); + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + yawPIDController.setSetpoint(NAVX_TARGET_ANGLE_DEGREES); + //Allow crossing over the bounds (see setContinuous() documentation) + yawPIDController.setContinuous(true); + //Set angle tolerance + yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + //Set P,I,D coefficients + yawPIDController.setPID(NAVX_YAW_PID_P, NAVX_YAW_PID_I, NAVX_YAW_PID_D); + //Start data collection + yawPIDController.start(); + } + + @Override + public void start() { + navx.reset(); + } + + public void loop() { + one.update(gamepad1); + navx.displayTelemetry(telemetry); + + if (yawPIDController.isUpdateAvailable(yawPIDState)) { + if (yawPIDState.isOnTarget()) { + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + backLeft.setPowerFloat(); + backRight.setPowerFloat(); + telemetry.addData("Motor Power", df.format(0.00)); + } else { + double power = yawPIDState.getOutput(); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, + power, power); + telemetry.addData("Motor Power", df.format(power)); + } + } + } + + public void stop() { + navx.stop(); + yawPIDController.stop(); + } + + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("NavX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("NavX Jerk", navx.getJerk()); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java new file mode 100644 index 0000000..09433b5 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -0,0 +1,115 @@ +package com.qualcomm.ftcrobotcontroller.opmodes.navx; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.MotorInfo; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.Units; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import java.text.DecimalFormat; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXRotatePID extends OpMode implements NavXDataReceiver { + + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 1; //port on device interface module + + private static final double NAVX_TOLERANCE_DEGREES = 2.0; //degrees of tolerance for PID controllers + private static final double NAVX_TARGET_ANGLE_DEGREES = 600.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.005; + private static final double NAVX_YAW_PID_I = 0.0; + private static final double NAVX_YAW_PID_D = 0.0; + + private static final double WHEEL_RADIUS = 2; + private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + EncodedMotor frontLeft, frontRight, backLeft, backRight; //make sure these have encoders! + Controller one; + NavXDevice navx; + NavXPIDController yawPIDController; + NavXPIDController.PIDState yawPIDState; + + public void init() { + //Create motors WITH ENCODERS (highly preferable) + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT)); + + //Instantiate controllers + one = new Controller(gamepad1); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.registerCallback(this); + + //Initialize the navX PID controller + //Using the yaw axis, we can find out how far we move forward + yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + //Set the target location + yawPIDController.setSetpoint(NAVX_TARGET_ANGLE_DEGREES); + //Allow crossing over the bounds (see setContinuous() documentation) + yawPIDController.setContinuous(false); + //Set angle tolerance + yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + //Set P,I,D coefficients + yawPIDController.setPID(NAVX_YAW_PID_P, NAVX_YAW_PID_I, NAVX_YAW_PID_D); + //Enable antistall (less accurate, but prevents motors from stalling) + yawPIDController.enableAntistall(); + yawPIDController.setAntistallDeadband(0.02); + //Making the tolerance very small makes the robot work hard to get to get to a very close estimate + yawPIDController.setTolerance(navXPIDController.ToleranceType.NONE, 0); + //Start data collection + yawPIDController.start(); + + //Initiate blank PID state + yawPIDState = new NavXPIDController.PIDState(); + } + + @Override + public void start() { + navx.reset(); + } + + public void loop() { + one.update(gamepad1); + navx.displayTelemetry(telemetry); + + if (yawPIDController.isUpdateAvailable(yawPIDState)) { + if (yawPIDState.isOnTarget()) { + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + backLeft.setPowerFloat(); + backRight.setPowerFloat(); + telemetry.addData("Motor Power", df.format(0.00)); + } else { + double power = yawPIDState.getOutput(); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, + power, -power); + telemetry.addData("Motor Power", df.format(power)); + } + } + } + + public void stop() { + navx.stop(); + yawPIDController.stop(); + } + + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("navX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("navX Jerk", df.format(navx.getJerk())); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java new file mode 100644 index 0000000..ca1b4bd --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java @@ -0,0 +1,59 @@ +package com.qualcomm.ftcrobotcontroller.opmodes.navx; + +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; + +import java.text.DecimalFormat; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXSensorTest extends OpMode implements NavXDataReceiver { + + private static final String NAVX_DIM = "dim"; //device interface module name + private static final int NAVX_PORT = 0; //port on device interface module + + private static final double NAVX_TOLERANCE_DEGREES = 2.0; //degrees of tolerance for PID controllers + private static final double NAVX_TARGET_ANGLE_DEGREES = 90.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.005; + private static final double NAVX_YAW_PID_I = 0.0; + private static final double NAVX_YAW_PID_D = 0.0; + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + Controller one; + NavXDevice navx; + + public void init() { + //Instantiate controllers + one = new Controller(gamepad1); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.registerCallback(this); + } + + @Override + public void start() { + navx.reset(); + } + + public void loop() { + one.update(gamepad1); + navx.displayTelemetry(telemetry); + } + + public void stop() { + navx.stop(); + } + + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("NavX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("NavX Jerk", df.format(navx.getJerk())); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java new file mode 100644 index 0000000..f00f990 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java @@ -0,0 +1,195 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.IDataArrivalSubscriber; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navX-Micro Collision Detection Op + *

    + * This is a demo program showing the use of the navX-Micro to implement + * a collision detection feature, which can be used to detect events + * while driving a robot, such as bumping into a wall or being hit + * by another robot. + *

    + * The basic principle used within the Collision Detection example + * is the calculation of Jerk (which is defined as the change in + * acceleration). In the sample code shown below, both the X axis and + * the Y axis jerk are calculated, and if either exceeds a threshold, + * then a collision has occurred. + *

    + * The 'collision threshold' used in these samples will likely need to + * be tuned for your robot, since the amount of jerk which constitutes + * a collision will be dependent upon the robot mass and expected + * maximum velocity of either the robot, or any object which may strike + * the robot. + *

    + * Note that this example uses the "callback" mechanism to be informed + * precisely when new data is received from the navX-Micro. + */ +public class navXCollisionDetectionOp extends OpMode implements IDataArrivalSubscriber { + + /* This is the port on the Core Device Interace Module */ + /* in which the navX-Micro is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + + /* Tune this threshold to adjust the sensitivy of the */ + /* Collision detection. */ + private final double COLLISION_THRESHOLD_DELTA_G = 0.5; + private final String COLLISION = "Collision"; + private final String NO_COLLISION = "--------"; + double last_world_linear_accel_x; + double last_world_linear_accel_y; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + private boolean collision_state; + private long last_system_timestamp = 0; + private long last_sensor_timestamp = 0; + + private long sensor_timestamp_delta = 0; + private long system_timestamp_delta = 0; + + @Override + public void init() { + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData); + last_world_linear_accel_x = 0.0; + last_world_linear_accel_y = 0.0; + setCollisionState(false); + } + + @Override + public void start() { + navx_device.registerCallback(this); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device", connected ? + "Connected" : "Disconnected"); + String gyrocal, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + if (connected) { + gyrocal = (navx_device.isCalibrating() ? + "CALIBRATING" : "Calibration Complete"); + motion = (navx_device.isMoving() ? "Moving" : "Not Moving"); + if (navx_device.isRotating()) { + motion += ", Rotating"; + } + } else { + gyrocal = + motion = "-------"; + } + telemetry.addData("2 GyroAccel", gyrocal); + telemetry.addData("3 Motion", motion); + telemetry.addData("4 Collision", getCollisionString()); + telemetry.addData("5 Timing", Long.toString(sensor_timestamp_delta) + ", " + + Long.toString(system_timestamp_delta)); + telemetry.addData("6 Events", Double.toString(navx_device.getUpdateCount())); + } + + private String getCollisionString() { + return (this.collision_state ? COLLISION : NO_COLLISION); + } + + private void setCollisionState(boolean newValue) { + this.collision_state = newValue; + } + + /* This callback is invoked by the AHRS class whenever new data is + received from the sensor. Note that this callback is occurring + within the context of the AHRS class IO thread, and it may + interrupt the thread running this opMode. Therefore, it is + very important to use thread synchronization techniques when + communicating between this callback and the rest of the + code in this opMode. + + The difference between the current linear acceleration data in + the X and Y axes and that in the last sample is compared. If + the absolute value of that difference is greater than the + "Collision Detection Threshold", a collision event is declared. + */ + + @Override + public void timestampedDataReceived(long curr_system_timestamp, long curr_sensor_timestamp, Object o) { + boolean collisionDetected = false; + + sensor_timestamp_delta = curr_sensor_timestamp - last_sensor_timestamp; + system_timestamp_delta = curr_system_timestamp - last_system_timestamp; + double curr_world_linear_accel_x = navx_device.getWorldLinearAccelX(); + double currentJerkX = curr_world_linear_accel_x - last_world_linear_accel_x; + last_world_linear_accel_x = curr_world_linear_accel_x; + double curr_world_linear_accel_y = navx_device.getWorldLinearAccelY(); + double currentJerkY = curr_world_linear_accel_y - last_world_linear_accel_y; + last_world_linear_accel_y = curr_world_linear_accel_y; + + if ((Math.abs(currentJerkX) > COLLISION_THRESHOLD_DELTA_G) || + (Math.abs(currentJerkY) > COLLISION_THRESHOLD_DELTA_G)) { + collisionDetected = true; + } + + setCollisionState(collisionDetected); + } + + @Override + public void untimestampedDataReceived(long l, Object o) { + + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java new file mode 100644 index 0000000..1271fa0 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java @@ -0,0 +1,158 @@ +/* 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. */ +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import android.util.Log; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.navXPIDController; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/* + * An example linear op mode where the robot will drive in + * a straight line (where the driving direction is guided by + * the Yaw angle from a navX-Model device). + * + * This example uses a simple PID controller configuration + * with a P coefficient, and will likely need tuning in order + * to achieve optimal performance. + * + * Note that for the best accuracy, a reasonably high update rate + * for the navX-Model sensor should be used. This example uses + * the default update rate (50Hz), which may be lowered in order + * to reduce the frequency of the updates to the drive system. + */ + +public class navXDriveStraightPIDLinearOp extends LinearOpMode { + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Model Device is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50; + private final double TARGET_ANGLE_DEGREES = 0.0; + private final double TOLERANCE_DEGREES = 2.0; + private final double MIN_MOTOR_OUTPUT_VALUE = -1.0; + private final double MAX_MOTOR_OUTPUT_VALUE = 1.0; + private final double YAW_PID_P = 0.005; + private final double YAW_PID_I = 0.0; + private final double YAW_PID_D = 0.0; + DcMotor leftMotor; + DcMotor rightMotor; + private AHRS navx_device; + private navXPIDController yawPIDController; + private ElapsedTime runtime = new ElapsedTime(); + + public double limit(double a) { + return Math.min(Math.max(a, MIN_MOTOR_OUTPUT_VALUE), MAX_MOTOR_OUTPUT_VALUE); + } + + @Override + public void runOpMode() throws InterruptedException { + leftMotor = hardwareMap.dcMotor.get("left_drive"); + rightMotor = hardwareMap.dcMotor.get("right_drive"); + + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData, + NAVX_DEVICE_UPDATE_RATE_HZ); + + rightMotor.setDirection(DcMotor.Direction.REVERSE); + + /* If possible, use encoders when driving, as it results in more */ + /* predictable drive system response. */ + //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + /* Create a PID Controller which uses the Yaw Angle as input. */ + yawPIDController = new navXPIDController(navx_device, + navXPIDController.navXTimestampedDataSource.YAW); + + /* Configure the PID controller */ + yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + yawPIDController.enable(true); + + waitForStart(); + + /* requestReset the navX-Model device yaw angle so that whatever direction */ + /* it is currently pointing will be zero degrees. */ + + navx_device.zeroYaw(); + + /* Wait for new Yaw PID output values, then update the motors + with the new PID value with each new output value. + */ + + final double TOTAL_RUN_TIME_SECONDS = 10.0; + int DEVICE_TIMEOUT_MS = 500; + navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); + + /* Drive straight forward at 1/2 of full drive speed */ + double drive_speed = 0.5; + + DecimalFormat df = new DecimalFormat("#.##"); + + try { + while ((runtime.time() < TOTAL_RUN_TIME_SECONDS) && + !Thread.currentThread().isInterrupted()) { + if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { + if (yawPIDResult.isOnTarget()) { + leftMotor.setPower(drive_speed); + rightMotor.setPower(drive_speed); + telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + + df.format(drive_speed)); + } else { + double output = yawPIDResult.getOutput(); + leftMotor.setPower(drive_speed + output); + rightMotor.setPower(drive_speed - output); + telemetry.addData("PIDOutput", df.format(limit(drive_speed + output)) + ", " + + df.format(limit(drive_speed - output))); + } + telemetry.addData("Yaw", df.format(navx_device.getYaw())); + } else { + /* A timeout occurred */ + Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT."); + } + } + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } finally { + navx_device.close(); + } + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java new file mode 100644 index 0000000..45144a0 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java @@ -0,0 +1,155 @@ +/* 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. */ +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.navXPIDController; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/* + * An example loop op mode where the robot will drive in + * a straight line (where the driving direction is guided by + * the Yaw angle from a navX-Model device). + * + * This example uses a simple PID controller configuration + * with a P coefficient, and will likely need tuning in order + * to achieve optimal performance. + * + * Note that for the best accuracy, a reasonably high update rate + * for the navX-Model sensor should be used. This example uses + * the default update rate (50Hz), which may be lowered in order + * to reduce the frequency of the updates to the drive system. + */ + +public class navXDriveStraightPIDLoopOp extends OpMode { + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Model Device is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50; + private final double TARGET_ANGLE_DEGREES = 0.0; + private final double TOLERANCE_DEGREES = 2.0; + private final double MIN_MOTOR_OUTPUT_VALUE = -1.0; + private final double MAX_MOTOR_OUTPUT_VALUE = 1.0; + private final double YAW_PID_P = 0.005; + private final double YAW_PID_I = 0.0; + private final double YAW_PID_D = 0.0; + DcMotor leftMotor; + DcMotor rightMotor; + navXPIDController.PIDResult yawPIDResult; + DecimalFormat df; + private AHRS navx_device; + private navXPIDController yawPIDController; + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void init() { + leftMotor = hardwareMap.dcMotor.get("left_drive"); + rightMotor = hardwareMap.dcMotor.get("right_drive"); + + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData, + NAVX_DEVICE_UPDATE_RATE_HZ); + + rightMotor.setDirection(DcMotor.Direction.REVERSE); + + /* If possible, use encoders when driving, as it results in more */ + /* predictable drive system response. */ + //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + /* Create a PID Controller which uses the Yaw Angle as input. */ + yawPIDController = new navXPIDController(navx_device, + navXPIDController.navXTimestampedDataSource.YAW); + + /* Configure the PID controller */ + yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + yawPIDController.enable(true); + + df = new DecimalFormat("#.##"); + } + + public double limit(double a) { + return Math.min(Math.max(a, MIN_MOTOR_OUTPUT_VALUE), MAX_MOTOR_OUTPUT_VALUE); + } + + @Override + public void start() { + /* requestReset the navX-Model device yaw angle so that whatever direction */ + /* it is currently pointing will be zero degrees. */ + navx_device.zeroYaw(); + yawPIDResult = new navXPIDController.PIDResult(); + } + + @Override + public void loop() { + /* Wait for new Yaw PID output values, then update the motors + with the new PID value with each new output value. + */ + + /* Drive straight forward at 1/2 of full drive speed */ + double drive_speed = 0.5; + + if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) { + if (yawPIDResult.isOnTarget()) { + leftMotor.setPower(drive_speed); + rightMotor.setPower(drive_speed); + telemetry.addData("Motor Output", df.format(drive_speed) + ", " + + df.format(drive_speed)); + } else { + double output = yawPIDResult.getOutput(); + leftMotor.setPower(limit(drive_speed + output)); + rightMotor.setPower(limit(drive_speed - output)); + telemetry.addData("Motor Output", df.format(limit(drive_speed + output)) + ", " + + df.format(limit(drive_speed - output))); + } + } else { + /* No sensor update has been received since the last time */ + /* the loop() function was invoked. Therefore, there's no */ + /* need to update the motors at this time. */ + } + telemetry.addData("Yaw", df.format(navx_device.getYaw())); + } + + @Override + public void stop() { + navx_device.close(); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java new file mode 100644 index 0000000..667aa57 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java @@ -0,0 +1,107 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navX-Micro Processed Data Mode Op + *

    + * Acquires motion-in-progress indicator from navX-Micro + * and displays it in the Robot DriveStation + * as telemetry data. + */ +public class navXMotionDetectionOp extends OpMode { + + /* This is the port on the Core Device Interace Module */ + /* in which the navX-Micro is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + + private String startDate; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + + @Override + public void init() { + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device", connected ? + "Connected" : "Disconnected"); + String gyrocal, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + if (connected) { + gyrocal = (navx_device.isCalibrating() ? + "CALIBRATING" : "Calibration Complete"); + motion = (navx_device.isMoving() ? "Moving" : "Not Moving"); + if (navx_device.isRotating()) { + motion += ", Rotating"; + } + } else { + gyrocal = + motion = "-------"; + } + telemetry.addData("2 GyroAccel", gyrocal); + telemetry.addData("3 Motion", motion); + } + +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java new file mode 100644 index 0000000..85546b5 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java @@ -0,0 +1,126 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.navXPerformanceMonitor; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navX-Micro Performance Tuning Op + *

    + * This opmode provides insight into the peformance of the communication + * with the navX-Model sensor over the I2C bus via the Core Device Interface + * Module. Since the Android operating system is not a real-time + * operating system, and since communication w/the navX-Model sensor is + * occurring over a wifi-direct network which can be prone to interference, + * the actual performance (update rate) achieved may be less than + * one might expect. + *

    + * Since the navX-Model devices integrate sensor data onboard, to achieve + * the best performance for device control methods like a PID controller + * that work best with constant-time updates, the strategy is to: + *

    + * 1) Configure the navX-Model device to a high update rate (e.g., 50Hz) + * 2) Using this performance-tuning Op-Mode (with all other + * sensors connected, just as your robot will be configured for normal + * use) observe the "effective" update rate, and the number of missed + * samples over the last minute. + * 3) Lower the navX-Model device update rate until the number of missed + * samples over the last minute reaches zero. + */ +public class navXPerformanceTuningOp extends OpMode { + + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Micro is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + + private AHRS navx_device; + private navXPerformanceMonitor navx_perfmon; + private byte sensor_update_rate_hz = 40; + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void init() { + AHRS.setLogging(true); + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData, + sensor_update_rate_hz); + navx_perfmon = new navXPerformanceMonitor(navx_device); + } + + @Override + public void start() { + navx_device.registerCallback(navx_perfmon); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device...............:", connected ? + "Connected" : "Disconnected"); + String gyrocal, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + telemetry.addData("2 Sensor Rate (Hz)...", Byte.toString(navx_device.getActualUpdateRate())); + telemetry.addData("3 Transfer Rate (Hz).", Integer.toString(navx_device.getCurrentTransferRate())); + telemetry.addData("4 Delivvered Rate (Hz)", Integer.toString(navx_perfmon.getDeliveredRateHz())); + telemetry.addData("5 Missed Samples.....", Integer.toString(navx_perfmon.getNumMissedSensorTimestampedSamples())); + telemetry.addData("6 Duplicate Samples..", Integer.toString(navx_device.getDuplicateDataCount())); + telemetry.addData("7 Sensor deltaT (ms).", Long.toString(navx_perfmon.getLastSensorTimestampDeltaMS())); + telemetry.addData("8 System deltaT (ms).", Long.toString(navx_perfmon.getLastSystemTimestampDeltaMS())); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java new file mode 100644 index 0000000..f805b03 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java @@ -0,0 +1,132 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navX-Micro Processed Data Mode Op + *

    + * Acquires processed data from navX-Micro + * and displays it in the Robot DriveStation + * as telemetry data. This processed data includes + * Yaw, Pitch, Roll, Compass Heading, Fused (9-axis) Heading, + * Sensor Status and Timestamp, and World-Frame Linear + * Acceleration data. + */ +public class navXProcessedOp extends OpMode { + + /* This is the port on the Core Device Interace Module */ + /* in which the navX-Micro is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + + private String startDate; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + + @Override + public void init() { + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device", connected ? + "Connected" : "Disconnected"); + String gyrocal, magcal, yaw, pitch, roll, compass_heading; + String fused_heading, ypr, cf, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + if (connected) { + gyrocal = (navx_device.isCalibrating() ? + "CALIBRATING" : "Calibration Complete"); + magcal = (navx_device.isMagnetometerCalibrated() ? + "Calibrated" : "UNCALIBRATED"); + yaw = df.format(navx_device.getYaw()); + pitch = df.format(navx_device.getPitch()); + roll = df.format(navx_device.getRoll()); + ypr = yaw + ", " + pitch + ", " + roll; + compass_heading = df.format(navx_device.getCompassHeading()); + fused_heading = df.format(navx_device.getFusedHeading()); + if (!navx_device.isMagnetometerCalibrated()) { + compass_heading = "-------"; + } + cf = compass_heading + ", " + fused_heading; + if (navx_device.isMagneticDisturbance()) { + cf += " (Mag. Disturbance)"; + } + motion = (navx_device.isMoving() ? "Moving" : "Not Moving"); + if (navx_device.isRotating()) { + motion += ", Rotating"; + } + } else { + gyrocal = + magcal = + ypr = + cf = + motion = "-------"; + } + telemetry.addData("2 GyroAccel", gyrocal); + telemetry.addData("3 Y,P,R", ypr); + telemetry.addData("4 Magnetometer", magcal); + telemetry.addData("5 Compass,9Axis", cf); + telemetry.addData("6 Motion", motion); + } + +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java new file mode 100644 index 0000000..a543113 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java @@ -0,0 +1,130 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navXRawOp + *

    + * This sample demonstrates how to acquire the raw + * Gyroscope, Accelerometer and Magnetometer data. This raw + * data is typically not as useful as the "processed" data + * (see the navXProcessedOp for details), however is provided + * for those interested in accessing the raw data. + *

    + * Gyroscope data is units of Degrees/second. + * Accelerometer data is in units of G. + * Magnetometer data is in units if microTorr (uT) + *

    + * Magnetometer data is not valid unless magnetometer calibration + * has been performed. Without calibration, the magnetometer + * data will be all zeros. For details on how to calibrate the + * magnetometer, please see the Magnetometer Calibration documentation: + * http://navx-micro.kauailabs.com/guidance/magnetometer-calibration/ + *

    + * Note that due to limitations imposed by the Core Device + * Interface Module's I2C interface mechanisms, the acquisition + * of both processed data and raw data requires two separate + * I2C bus transfers, and thus takes longer than acquiring + * only the raw or only the processed data. + */ +public class navXRawOp extends OpMode { + + private final int NAVX_DIM_I2C_PORT = 0; + private String startDate; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + + @Override + public void init() { + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kQuatAndRawData); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device", connected ? "Connected" : "Disconnected"); + String gyrocal, gyro_raw, accel_raw, mag_raw; + boolean magnetometer_calibrated; + if (connected) { + DecimalFormat df = new DecimalFormat("#.##"); + magnetometer_calibrated = navx_device.isMagnetometerCalibrated(); + gyro_raw = df.format(navx_device.getRawGyroX()) + ", " + + df.format(navx_device.getRawGyroY()) + ", " + + df.format(navx_device.getRawGyroZ()); + accel_raw = df.format(navx_device.getRawAccelX()) + ", " + + df.format(navx_device.getRawAccelY()) + ", " + + df.format(navx_device.getRawAccelZ()); + if (magnetometer_calibrated) { + mag_raw = df.format(navx_device.getRawMagX()) + ", " + + df.format(navx_device.getRawMagY()) + ", " + + df.format(navx_device.getRawMagZ()); + } else { + mag_raw = "Uncalibrated"; + } + } else { + gyro_raw = + accel_raw = + mag_raw = "-------"; + } + telemetry.addData("2 Gyros (Degrees/Sec):", gyro_raw); + telemetry.addData("3 Accelerometers (G):", accel_raw); + telemetry.addData("4 Magnetometers (uT):", mag_raw); + } + +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java new file mode 100644 index 0000000..427cad1 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java @@ -0,0 +1,142 @@ +/* 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. */ +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import android.util.Log; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.navXPIDController; +import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/* + * An example linear op mode where the robot will rotate + * to a specified angle an then stop. + * + * This example uses a simple PID controller configuration + * with a P coefficient, and will likely need tuning in order + * to achieve optimal performance. + * + * Note that for the best accuracy, a reasonably high update rate + * for the navX-Model sensor should be used. + */ +public class navXRotateToAnglePIDLinearOp extends LinearOpMode { + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Model Device is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50; + private final double TARGET_ANGLE_DEGREES = 90.0; + private final double TOLERANCE_DEGREES = 2.0; + private final double MIN_MOTOR_OUTPUT_VALUE = -1.0; + private final double MAX_MOTOR_OUTPUT_VALUE = 1.0; + private final double YAW_PID_P = 0.005; + private final double YAW_PID_I = 0.0; + private final double YAW_PID_D = 0.0; + DcMotor leftMotor; + DcMotor rightMotor; + private AHRS navx_device; + private navXPIDController yawPIDController; + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void runOpMode() throws InterruptedException { + leftMotor = hardwareMap.dcMotor.get("left_drive"); + rightMotor = hardwareMap.dcMotor.get("right_drive"); + + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData, + NAVX_DEVICE_UPDATE_RATE_HZ); + + rightMotor.setDirection(DcMotor.Direction.REVERSE); + + /* If possible, use encoders when driving, as it results in more */ + /* predictable drive system response. */ + //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + /* Create a PID Controller which uses the Yaw Angle as input. */ + yawPIDController = new navXPIDController(navx_device, + navXPIDController.navXTimestampedDataSource.YAW); + + /* Configure the PID controller */ + yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + + waitForStart(); + + try { + yawPIDController.enable(true); + + /* Wait for new Yaw PID output values, then update the motors + with the new PID value with each new output value. + */ + + final double TOTAL_RUN_TIME_SECONDS = 30.0; + int DEVICE_TIMEOUT_MS = 500; + navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); + + DecimalFormat df = new DecimalFormat("#.##"); + + while ((runtime.time() < TOTAL_RUN_TIME_SECONDS) && + !Thread.currentThread().isInterrupted()) { + if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { + if (yawPIDResult.isOnTarget()) { + leftMotor.setPowerFloat(); + rightMotor.setPowerFloat(); + telemetry.addData("PIDOutput", df.format(0.00)); + } else { + double output = yawPIDResult.getOutput(); + leftMotor.setPower(output); + rightMotor.setPower(-output); + telemetry.addData("PIDOutput", df.format(output) + ", " + + df.format(-output)); + } + } else { + /* A timeout occurred */ + Log.w("navXRotateOp", "Yaw PID waitForNewUpdate() TIMEOUT."); + } + telemetry.addData("Yaw", df.format(navx_device.getYaw())); + } + } catch (InterruptedException ex) { + Thread.currentThread().interrupt(); + } finally { + navx_device.close(); + } + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java new file mode 100644 index 0000000..52a73f8 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java @@ -0,0 +1,143 @@ +/* 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. */ +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.navXPIDController; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/* + * An example loop op mode where the robot will rotate + * to a specified angle an then stop. + * + * This example uses a simple PID controller configuration + * with a P coefficient, and will likely need tuning in order + * to achieve optimal performance. + * + * Note that for the best accuracy, a reasonably high update rate + * for the navX-Model sensor should be used. + */ +public class navXRotateToAnglePIDLoopOp extends OpMode { + /* This is the port on the Core Device Interface Module */ + /* in which the navX-Model Device is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + private final byte NAVX_DEVICE_UPDATE_RATE_HZ = 50; + private final double TARGET_ANGLE_DEGREES = 90.0; + private final double TOLERANCE_DEGREES = 2.0; + private final double MIN_MOTOR_OUTPUT_VALUE = -1.0; + private final double MAX_MOTOR_OUTPUT_VALUE = 1.0; + private final double YAW_PID_P = 0.005; + private final double YAW_PID_I = 0.0; + private final double YAW_PID_D = 0.0; + DcMotor leftMotor; + DcMotor rightMotor; + navXPIDController.PIDResult yawPIDResult; + DecimalFormat df; + private AHRS navx_device; + private navXPIDController yawPIDController; + private ElapsedTime runtime = new ElapsedTime(); + + @Override + public void init() { + leftMotor = hardwareMap.dcMotor.get("left_drive"); + rightMotor = hardwareMap.dcMotor.get("right_drive"); + + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData, + NAVX_DEVICE_UPDATE_RATE_HZ); + + rightMotor.setDirection(DcMotor.Direction.REVERSE); + + /* If possible, use encoders when driving, as it results in more */ + /* predictable drive system response. */ + //leftMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + //rightMotor.setChannelMode(DcMotorController.RunMode.RUN_USING_ENCODERS); + + /* Create a PID Controller which uses the Yaw Angle as input. */ + yawPIDController = new navXPIDController(navx_device, + navXPIDController.navXTimestampedDataSource.YAW); + + /* Configure the PID controller */ + yawPIDController.setSetpoint(TARGET_ANGLE_DEGREES); + yawPIDController.setContinuous(true); + yawPIDController.setOutputRange(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE); + yawPIDController.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, TOLERANCE_DEGREES); + yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); + yawPIDController.enable(true); + + df = new DecimalFormat("#.##"); + } + + @Override + public void start() { + /* requestReset the navX-Model device yaw angle so that whatever direction */ + /* it is currently pointing will be zero degrees. */ + navx_device.zeroYaw(); + yawPIDResult = new navXPIDController.PIDResult(); + } + + @Override + public void loop() { + /* Wait for new Yaw PID output values, then update the motors + with the new PID value with each new output value. + */ + + if (yawPIDController.isNewUpdateAvailable(yawPIDResult)) { + if (yawPIDResult.isOnTarget()) { + leftMotor.setPowerFloat(); + rightMotor.setPowerFloat(); + telemetry.addData("Motor Output", df.format(0.00)); + } else { + double output = yawPIDResult.getOutput(); + leftMotor.setPower(output); + rightMotor.setPower(-output); + telemetry.addData("Motor Output", df.format(output) + ", " + + df.format(-output)); + } + } else { + /* No sensor update has been received since the last time */ + /* the loop() function was invoked. Therefore, there's no */ + /* need to update the motors at this time. */ + } + telemetry.addData("Yaw", df.format(navx_device.getYaw())); + } + + @Override + public void stop() { + navx_device.close(); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java new file mode 100644 index 0000000..ab75619 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java @@ -0,0 +1,146 @@ +/* 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. */ + +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; + +import com.kauailabs.navx.ftc.AHRS; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.util.ElapsedTime; + +import java.text.DecimalFormat; + +/** + * navX-Micro Zero Yaw Op + *

    + * Acquires processed data from navX-Micro + * and displays it in the Robot DriveStation + * as telemetry data. This opmode demonstrates how to "zero" + * (requestReset to zero degrees) the yaw. This causes whatever + * direction the navX-Model device is currently pointing to + * now be zero degrees, and is an effective method for dealing + * with accumulating Yaw Drift. + *

    + * For more information on Yaw drift, please see the online help at: + * http://navx-micro.kauailabs.com/guidance/yaw-drift/ + */ +public class navXZeroYawOp extends OpMode { + + /* This is the port on the Core Device Interace Module */ + /* in which the navX-Micro is connected. Modify this */ + /* depending upon which I2C port you are using. */ + private final int NAVX_DIM_I2C_PORT = 0; + + private String startDate; + private ElapsedTime runtime = new ElapsedTime(); + private AHRS navx_device; + + @Override + public void init() { + navx_device = AHRS.getInstance(hardwareMap.deviceInterfaceModule.get("dim"), + NAVX_DIM_I2C_PORT, + AHRS.DeviceDataType.kProcessedData); + } + + @Override + public void stop() { + navx_device.close(); + } + + /* + * Code to run when the op mode is first enabled goes here + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#start() + */ + @Override + public void init_loop() { + telemetry.addData("navX Op Init Loop", runtime.toString()); + } + + /* + * This method will be called repeatedly in a loop + * @see com.qualcomm.robotcore.eventloop.opmode.OpMode#loop() + */ + @Override + public void loop() { + + boolean connected = navx_device.isConnected(); + telemetry.addData("1 navX-Device", connected ? + "Connected" : "Disconnected"); + String gyrocal, magcal, yaw, pitch, roll, compass_heading; + String fused_heading, ypr, cf, motion; + DecimalFormat df = new DecimalFormat("#.##"); + + if (connected) { + gyrocal = (navx_device.isCalibrating() ? + "CALIBRATING" : "Calibration Complete"); + magcal = (navx_device.isMagnetometerCalibrated() ? + "Calibrated" : "UNCALIBRATED"); + yaw = df.format(navx_device.getYaw()); + pitch = df.format(navx_device.getPitch()); + roll = df.format(navx_device.getRoll()); + ypr = yaw + ", " + pitch + ", " + roll; + compass_heading = df.format(navx_device.getCompassHeading()); + fused_heading = df.format(navx_device.getFusedHeading()); + if (!navx_device.isMagnetometerCalibrated()) { + compass_heading = "-------"; + } + cf = compass_heading + ", " + fused_heading; + if (navx_device.isMagneticDisturbance()) { + cf += " (Mag. Disturbance)"; + } + motion = (navx_device.isMoving() ? "Moving" : "Not Moving"); + if (navx_device.isRotating()) { + motion += ", Rotating"; + } + } else { + gyrocal = + magcal = + ypr = + cf = + motion = "-------"; + } + telemetry.addData("2 GyroAccel", gyrocal); + telemetry.addData("3 Y,P,R", ypr); + telemetry.addData("4 Magnetometer", magcal); + telemetry.addData("5 Compass,9Axis", cf); + telemetry.addData("6 Motion", motion); + + /* If the left 'bumper' button pressed, + requestReset (zero) the current yaw angle. This causes whatever + direction the navX-Model device is currently pointing to + now be zero degrees. + */ + //if ( gamepad1.left_bumper ) { + if ((navx_device.getUpdateCount() % 500) == 0) { + navx_device.zeroYaw(); + } + } + +} diff --git a/settings.gradle b/settings.gradle index 221abf0..588d5a0 100644 --- a/settings.gradle +++ b/settings.gradle @@ -1 +1,3 @@ -include ':ftc-library','sample' +include ':ftc-library' +include ':sample' +include ':lib-navx'

     Ccom.lasarobotics.library.controller.ButtonState
     Ccom.lasarobotics.library.util.Constants
     Ccom.lasarobotics.library.controller.Controller
     Ccom.lasarobotics.library.util.DistanceUnit
     Ccom.lasarobotics.library.util.Units +
     Ccom.lasarobotics.library.doodle.DoodleDo
     Ccom.lasarobotics.library.doodle.DoodleMap
     Ccom.lasarobotics.library.doodle.maps.DoodleMap
    KalmanKalman
    Kalman filter implementation Takes in multiple sensors' data to produce highly-accurate position and velocity vector fields
    diff --git a/docs/javadoc/com/lasarobotics/library/skynet/package-tree.html b/docs/javadoc/com/lasarobotics/library/skynet/package-tree.html index 117ba87..9341200 100644 --- a/docs/javadoc/com/lasarobotics/library/skynet/package-tree.html +++ b/docs/javadoc/com/lasarobotics/library/skynet/package-tree.html @@ -3,14 +3,14 @@ -com.lasarobotics.library.skynet Class Hierarchy + com.lasarobotics.library.nav Class Hierarchy @@ -62,7 +62,7 @@
    -

    Hierarchy For Package com.lasarobotics.library.skynet

    +

    Hierarchy For Package com.lasarobotics.library.nav

    Package Hierarchies:
    • All Packages
      • @@ -73,7 +73,10 @@

      Class Hierarchy

      • java.lang.Object
          -
        • com.lasarobotics.library.skynet.Kalman
          • +
        • com.lasarobotics.library.skynet.Kalman +
      diff --git a/docs/javadoc/com/lasarobotics/library/util/DistanceUnit.html b/docs/javadoc/com/lasarobotics/library/util/DistanceUnit.html index 3e038e4..f13287e 100644 --- a/docs/javadoc/com/lasarobotics/library/util/DistanceUnit.html +++ b/docs/javadoc/com/lasarobotics/library/util/DistanceUnit.html @@ -89,7 +89,7 @@

      Enum DistanceUnit

    • java.lang.Enum<DistanceUnit>
      • -
      • com.lasarobotics.library.util.DistanceUnit
        • +
      • com.lasarobotics.library.util.Units
    diff --git a/docs/javadoc/index-files/index-10.html b/docs/javadoc/index-files/index-10.html index df72920..5614d70 100644 --- a/docs/javadoc/index-files/index-10.html +++ b/docs/javadoc/index-files/index-10.html @@ -66,12 +66,20 @@

    K

    -
    Kalman - Class in com.lasarobotics.library.skynet
    +
    Kalman - Class + in + com.lasarobotics.library.nav +
    Kalman filter implementation Takes in multiple sensors' data to produce highly-accurate position and velocity vector fields
    -
    Kalman() - Constructor for class com.lasarobotics.library.skynet.Kalman
    +
    Kalman() - + Constructor for class com.lasarobotics.library.skynet.Kalman
     
    A B C D E F G H I K L M N O P R S T U V W X Y Z 
    diff --git a/docs/javadoc/index-files/index-3.html b/docs/javadoc/index-files/index-3.html index 9ac23f4..393c7d3 100644 --- a/docs/javadoc/index-files/index-3.html +++ b/docs/javadoc/index-files/index-3.html @@ -103,7 +103,10 @@

    C

     
    com.lasarobotics.library.sensor.modernrobotics - package com.lasarobotics.library.sensor.modernrobotics
     
    -
    com.lasarobotics.library.skynet - package com.lasarobotics.library.skynet
    +
    + com.lasarobotics.library.nav + - package com.lasarobotics.library.skynet +
     
    com.lasarobotics.library.util - package com.lasarobotics.library.util
     
    diff --git a/docs/javadoc/overview-frame.html b/docs/javadoc/overview-frame.html index f4b91af..81c41a9 100644 --- a/docs/javadoc/overview-frame.html +++ b/docs/javadoc/overview-frame.html @@ -26,7 +26,8 @@

    Packages

  • com.lasarobotics.library.sensor.legacy.hitechnic
  • com.lasarobotics.library.sensor.legacy.lego
  • com.lasarobotics.library.sensor.modernrobotics
    • -
  • com.lasarobotics.library.skynet
    • +
  • com.lasarobotics.library.nav +
  • com.lasarobotics.library.util
    • diff --git a/docs/javadoc/overview-summary.html b/docs/javadoc/overview-summary.html index 50a4386..7f9c262 100644 --- a/docs/javadoc/overview-summary.html +++ b/docs/javadoc/overview-summary.html @@ -126,7 +126,8 @@     		 
    com.lasarobotics.library.skynetcom.lasarobotics.library.nav +