From 94281bc68aaf83a02eaac73db2c5ebb7ba7c6708 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Mon, 4 Jan 2016 11:27:43 -0600 Subject: [PATCH 01/43] Complete raw sensor integration -> result: inaccurate --- docs/javadoc/allclasses-frame.html | 3 +- docs/javadoc/allclasses-noframe.html | 3 +- .../lasarobotics/library/skynet/Kalman.html | 4 +- .../library/skynet/package-frame.html | 8 +- .../library/skynet/package-summary.html | 7 +- .../library/skynet/package-tree.html | 11 +- docs/javadoc/index-files/index-10.html | 12 +- docs/javadoc/index-files/index-3.html | 5 +- docs/javadoc/overview-frame.html | 3 +- docs/javadoc/overview-summary.html | 3 +- docs/javadoc/overview-tree.html | 8 +- .../lasarobotics/library/android/Sensors.java | 193 ++++++++++++++++++ .../library/{skynet => nav}/EncodedMotor.java | 2 +- .../library/util/ScreenOrientation.java | 63 ++++++ .../lasarobotics/library/util/Vector3.java | 4 +- .../opmodes/EncoderTest.java | 2 +- .../opmodes/FtcOpModeRegister.java | 7 +- .../opmodes/SensorsTest.java | 29 +++ 18 files changed, 339 insertions(+), 28 deletions(-) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/android/Sensors.java rename ftc-library/src/main/java/com/lasarobotics/library/{skynet => nav}/EncodedMotor.java (98%) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/util/ScreenOrientation.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/SensorsTest.java 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..7880b0a 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.Kalman
    • 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

    -

    Hierarchy For Package com.lasarobotics.library.skynet

    +

    Hierarchy For Package com.lasarobotics.library.nav

    Package Hierarchies:
    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.skynet + com.lasarobotics.library.nav +   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/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/skynet/EncodedMotor.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java similarity index 98% rename from ftc-library/src/main/java/com/lasarobotics/library/skynet/EncodedMotor.java rename to ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java index 352a9ba..55e02be 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/skynet/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -1,4 +1,4 @@ -package com.lasarobotics.library.skynet; +package com.lasarobotics.library.nav; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorController; 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/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/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java index 6e29758..6e459d1 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,7 +1,7 @@ package com.qualcomm.ftcrobotcontroller.opmodes; import com.lasarobotics.library.controller.Controller; -import com.lasarobotics.library.skynet.EncodedMotor; +import com.lasarobotics.library.nav.EncodedMotor; import com.qualcomm.robotcore.eventloop.opmode.OpMode; public class EncoderTest extends OpMode { 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 a0c1d31..0a36f49 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -47,11 +47,12 @@ public class FtcOpModeRegister implements OpModeRegister { */ public void register(OpModeManager manager) { //Custom op modes - manager.register("Null", NullOp.class); + manager.register("Null Op", NullOp.class); manager.register("Mecanum Prototype", MecanumPrototypeTeleop.class); manager.register("MonkeyC Do", MonkeyCDo.class); manager.register("MonkeyC Write", MonkeyCWrite.class); - manager.register("LoggingSample", LoggingSample.class); - manager.register("OptionsSample", OptionsSample.class); + manager.register("Logging Sample", LoggingSample.class); + manager.register("Options Sample", OptionsSample.class); + manager.register("Sensors Test", SensorsTest.class); } } 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(); + } +} From 662e150fea897cf6da4b5915f6d66bebd934cc63 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 7 Jan 2016 19:44:53 -0600 Subject: [PATCH 02/43] Add navX library --- ftc-library/build.gradle | 5 +- lib-navx/.gitignore | 1 + lib-navx/build.gradle | 47 + lib-navx/src/main/AndroidManifest.xml | 10 + .../java/com/kauailabs/navx/AHRSProtocol.java | 786 ++++++++ .../java/com/kauailabs/navx/IMUProtocol.java | 355 ++++ .../java/com/kauailabs/navx/IMURegisters.java | 213 ++ .../java/com/kauailabs/navx/ftc/AHRS.java | 1761 +++++++++++++++++ .../navx/ftc/IDataArrivalSubscriber.java | 79 + .../kauailabs/navx/ftc/navXPIDController.java | 647 ++++++ .../navx/ftc/navXPerformanceMonitor.java | 211 ++ lib-navx/src/main/java/overview.htm | 70 + lib-navx/src/main/res/values/strings.xml | 3 + .../libs => libs}/Analytics-release.aar | Bin .../libs => libs}/FtcCommon-release.aar | Bin .../libs => libs}/Hardware-release.aar | Bin .../libs => libs}/ModernRobotics-release.aar | Bin {ftc-library/libs => libs}/README.txt | 0 .../libs => libs}/RobotCore-release.aar | Bin .../libs => libs}/WirelessP2p-release.aar | Bin settings.gradle | 4 +- 21 files changed, 4190 insertions(+), 2 deletions(-) create mode 100644 lib-navx/.gitignore create mode 100644 lib-navx/build.gradle create mode 100644 lib-navx/src/main/AndroidManifest.xml create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/AHRSProtocol.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/IMUProtocol.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/IMURegisters.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/ftc/AHRS.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/ftc/IDataArrivalSubscriber.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java create mode 100644 lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPerformanceMonitor.java create mode 100644 lib-navx/src/main/java/overview.htm create mode 100644 lib-navx/src/main/res/values/strings.xml rename {ftc-library/libs => libs}/Analytics-release.aar (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/FtcCommon-release.aar (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/Hardware-release.aar (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/ModernRobotics-release.aar (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/README.txt (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/RobotCore-release.aar (100%) mode change 100755 => 100644 rename {ftc-library/libs => libs}/WirelessP2p-release.aar (100%) mode change 100755 => 100644 diff --git a/ftc-library/build.gradle b/ftc-library/build.gradle index 7b37499..bdafe87 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') + project ':lib-navx' + compile 'com.google.code.gson:gson:2.4' compile(name: 'RobotCore-release', ext: 'aar') 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..5c42b0d --- /dev/null +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java @@ -0,0 +1,647 @@ +/* ============================================ + 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 { + + 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; + /* Error statistics */ + private double error_current = 0.0; + private double error_previous = 0.0; + private double error_total = 0.0; + /* Coefficients */ + private double p; + private double i; + private double d; + private double ff; + /* Input/Output Clamps */ + private double max_input = 0.0; + private double min_input = 0.0; + private double max_output = 1.0; + 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 */ + 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/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' From e806f16697acfba64869bf5e305671986d86ee98 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 09:35:44 -0600 Subject: [PATCH 03/43] Add a basic NavX device - now to add PID controller overrides --- ftc-library/build.gradle | 2 +- .../lasarobotics/library/android/Util.java | 4 +- sample/build.gradle | 3 +- .../navx/navXCollisionDetectionOp.java | 195 ++++++++++++++++++ .../navx/navXDriveStraightPIDLinearOp.java | 158 ++++++++++++++ .../navx/navXDriveStraightPIDLoopOp.java | 155 ++++++++++++++ .../opmodes/navx/navXMotionDetectionOp.java | 107 ++++++++++ .../opmodes/navx/navXPerformanceTuningOp.java | 126 +++++++++++ .../opmodes/navx/navXProcessedOp.java | 132 ++++++++++++ .../opmodes/navx/navXRawOp.java | 130 ++++++++++++ .../navx/navXRotateToAnglePIDLinearOp.java | 142 +++++++++++++ .../navx/navXRotateToAnglePIDLoopOp.java | 143 +++++++++++++ .../opmodes/navx/navXZeroYawOp.java | 146 +++++++++++++ 13 files changed, 1438 insertions(+), 5 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLinearOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLoopOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXMotionDetectionOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXPerformanceTuningOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXProcessedOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRawOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLinearOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLoopOp.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXZeroYawOp.java diff --git a/ftc-library/build.gradle b/ftc-library/build.gradle index bdafe87..a98e826 100644 --- a/ftc-library/build.gradle +++ b/ftc-library/build.gradle @@ -30,7 +30,7 @@ repositories { dependencies { compile files('libs/android-support-v4.jar') - project ':lib-navx' + compile project(':lib-navx') compile 'com.google.code.gson:gson:2.4' 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/sample/build.gradle b/sample/build.gradle index d0d2242..58d35ce 100644 --- a/sample/build.gradle +++ b/sample/build.gradle @@ -22,13 +22,12 @@ 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') + compile project(':lib-navx') } diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java new file mode 100644 index 0000000..c50a6a4 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXDriveStraightPIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLinearOp.java new file mode 100644 index 0000000..984b6fe --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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(); + + /* reset 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/navXDriveStraightPIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLoopOp.java new file mode 100644 index 0000000..e1dd96d --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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() { + /* reset 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/navXMotionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXMotionDetectionOp.java new file mode 100644 index 0000000..3afb365 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXPerformanceTuningOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXPerformanceTuningOp.java new file mode 100644 index 0000000..4233e6f --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXProcessedOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXProcessedOp.java new file mode 100644 index 0000000..bc37a8b --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXRawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRawOp.java new file mode 100644 index 0000000..ff5291e --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXRotateToAnglePIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLinearOp.java new file mode 100644 index 0000000..b8014a9 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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/navXRotateToAnglePIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLoopOp.java new file mode 100644 index 0000000..d640f36 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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() { + /* reset 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/navXZeroYawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXZeroYawOp.java new file mode 100644 index 0000000..23a7c15 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/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; + +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" + * (reset 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, + reset (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(); + } + } + +} From 28ded799d7d255106949c977c177fd391acf0d73 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 09:38:36 -0600 Subject: [PATCH 04/43] Comment a thing or two --- .../sensor/kauailabs/navx/NavXDevice.java | 161 ++++++++++++++++++ 1 file changed, 161 insertions(+) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDevice.java 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..19cbf60 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDevice.java @@ -0,0 +1,161 @@ +package com.lasarobotics.library.sensor.kauailabs.navx; + +import com.kauailabs.navx.ftc.AHRS; +import com.lasarobotics.library.util.Vector3; +import com.qualcomm.robotcore.hardware.HardwareMap; + +/** + * NavX MXP controller + */ +public class NavXDevice { + + AHRS ahrs; + DataType dataType; + + /** + * 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 + */ + NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort) { + initialize(map, deviceInterfaceModuleName, i2cPort, SensorSpeed.NORMAL_FAST, DataType.PROCESSED_DATA); + } + + /** + * 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) + */ + 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.dataType = type; + } + + 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 isRotating() { + return ahrs.isRotating() && !ahrs.isMagneticDisturbance(); + } + + public void resetYaw() { + ahrs.zeroYaw(); + } + + /** + * Get update frequency in Hertz + * + * @return Update rate in Hertzs + */ + public float getUpdateRate() { + return (float) ahrs.getActualUpdateRate(); + } + + /** + * 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 destroy() { + ahrs.close(); + } + + + 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); + } + } +} From 997608610b221047a43c5584e0dd5b1cd0633a6a Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 11:25:05 -0600 Subject: [PATCH 05/43] Start sensor test --- .../sensor/kauailabs/navx/NavXDevice.java | 63 +++++++++++++++++-- .../FtcRobotControllerActivity.java | 30 ++++----- .../opmodes/navx/NavXSensorTest.java | 45 +++++++++++++ .../navXCollisionDetectionOp.java | 2 +- .../navXDriveStraightPIDLinearOp.java | 2 +- .../navXDriveStraightPIDLoopOp.java | 2 +- .../navXMotionDetectionOp.java | 2 +- .../navXPerformanceTuningOp.java | 2 +- .../navx/{ => kauailabs}/navXProcessedOp.java | 2 +- .../navx/{ => kauailabs}/navXRawOp.java | 2 +- .../navXRotateToAnglePIDLinearOp.java | 2 +- .../navXRotateToAnglePIDLoopOp.java | 2 +- .../navx/{ => kauailabs}/navXZeroYawOp.java | 2 +- 13 files changed, 125 insertions(+), 33 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXCollisionDetectionOp.java (99%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXDriveStraightPIDLinearOp.java (99%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXDriveStraightPIDLoopOp.java (99%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXMotionDetectionOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXPerformanceTuningOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXProcessedOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXRawOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXRotateToAnglePIDLinearOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXRotateToAnglePIDLoopOp.java (98%) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{ => kauailabs}/navXZeroYawOp.java (98%) 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 index 19cbf60..9895a10 100644 --- 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 @@ -3,6 +3,9 @@ import com.kauailabs.navx.ftc.AHRS; import com.lasarobotics.library.util.Vector3; import com.qualcomm.robotcore.hardware.HardwareMap; +import com.qualcomm.robotcore.robocol.Telemetry; + +import java.text.DecimalFormat; /** * NavX MXP controller @@ -19,7 +22,7 @@ public class NavXDevice { * @param deviceInterfaceModuleName String name of the device interface module the sensor is on * @param i2cPort The i2C port the sensor is currently on */ - NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort) { + public NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort) { initialize(map, deviceInterfaceModuleName, i2cPort, SensorSpeed.NORMAL_FAST, DataType.PROCESSED_DATA); } @@ -31,7 +34,7 @@ public class NavXDevice { * @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) */ - NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort, SensorSpeed speed) { + public NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort, SensorSpeed speed) { initialize(map, deviceInterfaceModuleName, i2cPort, speed, DataType.PROCESSED_DATA); } @@ -57,14 +60,57 @@ public boolean isMoving() { return ahrs.isMoving(); } + public boolean isMagneticDisturbance() { + return ahrs.isMagneticDisturbance(); + } + public boolean isRotating() { return ahrs.isRotating() && !ahrs.isMagneticDisturbance(); } - public void resetYaw() { + public void reset() { ahrs.zeroYaw(); } + 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 * @@ -74,6 +120,15 @@ 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 * @@ -105,7 +160,7 @@ public float getHeading() { return ahrs.getFusedHeading(); } - public void destroy() { + public void stop() { ahrs.close(); } diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/FtcRobotControllerActivity.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/FtcRobotControllerActivity.java index e790228..f20bbfb 100755 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/FtcRobotControllerActivity.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/FtcRobotControllerActivity.java @@ -74,19 +74,15 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE public class FtcRobotControllerActivity extends Activity { + public static final String CONFIGURE_FILENAME = "CONFIGURE_FILENAME"; private static final int REQUEST_CONFIG_WIFI_CHANNEL = 1; private static final boolean USE_DEVICE_EMULATION = false; private static final int NUM_GAMEPADS = 2; - - public static final String CONFIGURE_FILENAME = "CONFIGURE_FILENAME"; - protected SharedPreferences preferences; protected UpdateUI.Callback callback; protected Context context; - private Utility utility; protected ImageButton buttonMenu; - protected TextView textDeviceName; protected TextView textWifiDirectStatus; protected TextView textRobotStatus; @@ -94,23 +90,12 @@ public class FtcRobotControllerActivity extends Activity { protected TextView textOpMode; protected TextView textErrorMessage; protected ImmersiveMode immersion; - protected UpdateUI updateUI; protected Dimmer dimmer; protected LinearLayout entireScreenLayout; - protected FtcRobotControllerService controllerService; - protected FtcEventLoop eventLoop; - - protected class RobotRestarter implements Restarter { - - public void requestRestart() { - requestRobotRestart(); - } - - } - + private Utility utility; protected ServiceConnection connection = new ServiceConnection() { @Override public void onServiceConnected(ComponentName name, IBinder service) { @@ -235,7 +220,6 @@ public void onWindowFocusChanged(boolean hasFocus){ } } - @Override public boolean onCreateOptionsMenu(Menu menu) { getMenuInflater().inflate(R.menu.ftc_robot_controller, menu); @@ -277,7 +261,7 @@ public boolean onOptionsItemSelected(MenuItem item) { @Override public void onConfigurationChanged(Configuration newConfig) { super.onConfigurationChanged(newConfig); - // don't destroy assets on screen rotation + // don't stop assets on screen rotation } @Override @@ -381,4 +365,12 @@ public void run() { } }); } + + protected class RobotRestarter implements Restarter { + + public void requestRestart() { + requestRobotRestart(); + } + + } } 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..e1ea09b --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java @@ -0,0 +1,45 @@ +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.NavXDevice; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXSensorTest extends OpMode { + + DcMotor frontLeft, frontRight, backLeft, backRight; + Controller one; + NavXDevice navx; + + public void init() { + frontLeft = hardwareMap.dcMotor.get("frontLeft"); + frontRight = hardwareMap.dcMotor.get("frontRight"); + backLeft = hardwareMap.dcMotor.get("backLeft"); + backRight = hardwareMap.dcMotor.get("backRight"); + + one = new Controller(gamepad1); + navx = new NavXDevice(hardwareMap, "dim", 1); + } + + @Override + public void init_loop() { + navx.reset(); + } + + public void loop() { + one.update(gamepad1); + navx.displayTelemetry(telemetry); + + + Tank.motor4(frontLeft, frontRight, backLeft, backRight, one.left_stick_y, one.right_stick_y); + } + + public void stop() { + navx.stop(); + } +} diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java similarity index 99% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java index c50a6a4..f00f990 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXCollisionDetectionOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXCollisionDetectionOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.kauailabs.navx.ftc.IDataArrivalSubscriber; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java similarity index 99% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLinearOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java index 984b6fe..cec6d1c 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLinearOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java @@ -28,7 +28,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import android.util.Log; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java similarity index 99% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLoopOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java index e1dd96d..5303f37 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXDriveStraightPIDLoopOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLoopOp.java @@ -28,7 +28,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.kauailabs.navx.ftc.navXPIDController; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXMotionDetectionOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXMotionDetectionOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java index 3afb365..667aa57 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXMotionDetectionOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXMotionDetectionOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXPerformanceTuningOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXPerformanceTuningOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java index 4233e6f..85546b5 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXPerformanceTuningOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXPerformanceTuningOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.kauailabs.navx.ftc.navXPerformanceMonitor; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXProcessedOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXProcessedOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java index bc37a8b..f805b03 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXProcessedOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXProcessedOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRawOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java index ff5291e..a543113 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRawOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRawOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.qualcomm.robotcore.eventloop.opmode.OpMode; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLinearOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java index b8014a9..427cad1 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLinearOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLinearOp.java @@ -28,7 +28,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import android.util.Log; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLoopOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLoopOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java index d640f36..cb49829 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXRotateToAnglePIDLoopOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXRotateToAnglePIDLoopOp.java @@ -28,7 +28,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.kauailabs.navx.ftc.navXPIDController; diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXZeroYawOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java similarity index 98% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXZeroYawOp.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java index 23a7c15..2d44982 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/navXZeroYawOp.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXZeroYawOp.java @@ -29,7 +29,7 @@ are permitted (subject to the limitations in the disclaimer below) provided that 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; +package com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs; import com.kauailabs.navx.ftc.AHRS; import com.qualcomm.robotcore.eventloop.opmode.OpMode; From fa289aecb3ed99e7d313c0fff6f6def73ce76630 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 18:51:02 -0600 Subject: [PATCH 06/43] Add collision detection --- .../kauailabs/navx/NavXDataReceiver.java | 8 + .../sensor/kauailabs/navx/NavXDevice.java | 139 +++++++++++++++++- .../kauailabs/navx/NavXPIDController.java | 8 + sample/build.gradle | 1 - .../opmodes/navx/NavXSensorTest.java | 21 ++- 5 files changed, 168 insertions(+), 9 deletions(-) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXDataReceiver.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java 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 index 9895a10..f7d18c3 100644 --- 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 @@ -1,19 +1,29 @@ package com.lasarobotics.library.sensor.kauailabs.navx; import com.kauailabs.navx.ftc.AHRS; +import com.kauailabs.navx.ftc.IDataArrivalSubscriber; 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 { +public class NavXDevice implements IDataArrivalSubscriber { + static final DataType dataType = DataType.RAW_AND_PROCESSED; + static final int MAX_NUM_CALLBACKS = 3; AHRS ahrs; - DataType dataType; + 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 @@ -23,7 +33,7 @@ public class NavXDevice { * @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.PROCESSED_DATA); + initialize(map, deviceInterfaceModuleName, i2cPort, SensorSpeed.NORMAL_FAST, dataType); } /** @@ -41,7 +51,8 @@ public NavXDevice(HardwareMap map, String deviceInterfaceModuleName, int i2cPort 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.dataType = type; + this.callbacks = new NavXDataReceiver[MAX_NUM_CALLBACKS]; + ahrs.registerCallback(this); } public boolean isCalibrating() { @@ -162,6 +173,126 @@ public float getHeading() { 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); + } + } } 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..31e8a16 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java @@ -0,0 +1,8 @@ +package com.lasarobotics.library.sensor.kauailabs.navx; + +/** + * PID Controller designed for the navX MXP or micro + */ +public class NavXPIDController { + +} diff --git a/sample/build.gradle b/sample/build.gradle index 58d35ce..bc83e1f 100644 --- a/sample/build.gradle +++ b/sample/build.gradle @@ -29,5 +29,4 @@ dependencies { compile(name: 'Analytics-release', ext: 'aar') compile(name: 'WirelessP2p-release', ext: 'aar') compile project(':ftc-library') - compile project(':lib-navx') } 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 index e1ea09b..08712b9 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java @@ -2,6 +2,7 @@ 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.qualcomm.robotcore.eventloop.opmode.OpMode; import com.qualcomm.robotcore.hardware.DcMotor; @@ -10,7 +11,10 @@ * OpMode designed to test extended functionality of the NavX sensor * Requires four motors to test PID */ -public class NavXSensorTest extends OpMode { +public class NavXSensorTest extends OpMode implements NavXDataReceiver { + + final String NAVX_DIM = "dim"; //device interface module name + final int NAVX_PORT = 1; //port on device interface module DcMotor frontLeft, frontRight, backLeft, backRight; Controller one; @@ -23,11 +27,15 @@ public void init() { backRight = hardwareMap.dcMotor.get("backRight"); one = new Controller(gamepad1); - navx = new NavXDevice(hardwareMap, "dim", 1); + + //Initialize the navX controller + //Make sure to implement NavXDataReceiver + navx = new NavXDevice(hardwareMap, NAVX_DIM, NAVX_PORT); + navx.registerCallback(this); } @Override - public void init_loop() { + public void start() { navx.reset(); } @@ -35,11 +43,16 @@ public void loop() { one.update(gamepad1); navx.displayTelemetry(telemetry); - Tank.motor4(frontLeft, frontRight, backLeft, backRight, one.left_stick_y, one.right_stick_y); } public void stop() { navx.stop(); } + + @Override + public void dataReceived(long timeDelta) { + telemetry.addData("NavX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("NavX Jerk", navx.getJerk()); + } } From 996849ccb28ccfa0cfd067df945d4675b4735938 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 19:56:21 -0600 Subject: [PATCH 07/43] Add PID controller --- .../kauailabs/navx/NavXPIDController.java | 149 +++++++++++++++++- .../kauailabs/navx/ftc/navXPIDController.java | 10 +- .../opmodes/navx/NavXSensorTest.java | 61 +++++-- 3 files changed, 205 insertions(+), 15 deletions(-) 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 index 31e8a16..a59265a 100644 --- 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 @@ -1,8 +1,155 @@ package com.lasarobotics.library.sensor.kauailabs.navx; +import android.util.Log; + +import com.kauailabs.navx.ftc.navXPIDController; + /** * PID Controller designed for the navX MXP or micro */ -public class NavXPIDController { +public class NavXPIDController extends navXPIDController { + + private final static int waitTimeout = 50; //wait timeout in ms + + public NavXPIDController(NavXDevice navX, DataSource dataSource) { + super(navX.ahrs, dataSource.val); + setPID(0.5, 0, 0, 0); + setOutputRange(-1.0, 1.0); + } + + public double getCoefficientPotential() { + return p; + } + + public double getCoefficientIntegral() { + return i; + } + + public double getCoefficientDerivative() { + return d; + } + + public double getCoefficientFastForward() { + return ff; + } + + 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) { + return isNewUpdateAvailable(state); + } + + + /** + * Returns the latest output value, selected by the DataSource when creating the controller + * + * @return Latest output value as a double + */ + public double getOutputValue() { + return super.get(); + } + + 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/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java index 5c42b0d..dd52e1a 100644 --- a/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java @@ -59,6 +59,11 @@ of this software and associated documentation files (the "Software"), to deal public class navXPIDController implements IDataArrivalSubscriber { + /* Coefficients */ + protected double p; + protected double i; + protected double d; + protected double ff; navXTimestampedDataSource timestamped_src; navXUntimestampedDataSource untimestamped_src; AHRS navx_device; @@ -71,11 +76,6 @@ public class navXPIDController implements IDataArrivalSubscriber { private double error_current = 0.0; private double error_previous = 0.0; private double error_total = 0.0; - /* Coefficients */ - private double p; - private double i; - private double d; - private double ff; /* Input/Output Clamps */ private double max_input = 0.0; private double min_input = 0.0; 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 index 08712b9..1ace171 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java @@ -2,10 +2,13 @@ import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.nav.EncodedMotor; 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 @@ -13,25 +16,51 @@ */ public class NavXSensorTest extends OpMode implements NavXDataReceiver { - final String NAVX_DIM = "dim"; //device interface module name - final int NAVX_PORT = 1; //port on device interface module + 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 = 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; - DcMotor frontLeft, frontRight, backLeft, backRight; + 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() { - frontLeft = hardwareMap.dcMotor.get("frontLeft"); - frontRight = hardwareMap.dcMotor.get("frontRight"); - backLeft = hardwareMap.dcMotor.get("backLeft"); - backRight = hardwareMap.dcMotor.get("backRight"); + //Create motors WITH ENCODERS (important!) + 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")); + //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 @@ -43,11 +72,25 @@ public void loop() { one.update(gamepad1); navx.displayTelemetry(telemetry); - Tank.motor4(frontLeft, frontRight, backLeft, backRight, one.left_stick_y, one.right_stick_y); + 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 From d309ed2728c0df7c074b666f4cd3c02beae472ce Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 20:41:13 -0600 Subject: [PATCH 08/43] Add idea for a navigator --- .../lasarobotics/library/nav/Navigator.java | 85 +++++++++++++++ .../sensor/legacy/hitechnic/Gyroscope.java | 2 +- .../lasarobotics/library/util/Vector2.java | 38 +++++++ .../opmodes/navx/NavXFullNavigation.java | 101 ++++++++++++++++++ 4 files changed, 225 insertions(+), 1 deletion(-) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/util/Vector2.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java new file mode 100644 index 0000000..f15c6fe --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java @@ -0,0 +1,85 @@ +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.Vector2; +import com.lasarobotics.library.util.Vector3; + +/** + * 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. + */ +public class Navigator { + + /*** + * INITIALIZATION + ***/ + + Vector3 startLocation = new Vector3<>(0.0, 0.0, 0.0); + double startHeading = 0.0; + boolean omnidirectionalDrive = false; + /*** + * CONTROLLERS + ***/ + + NavXDevice navx; + NavXPIDController accelX; + NavXPIDController accelY; + NavXPIDController accelZ; + NavXPIDController rotX; + NavXPIDController rotY; + NavXPIDController rotZ; + + public Navigator(NavXDevice navx, boolean omnidirectionalDrive) { + this.omnidirectionalDrive = omnidirectionalDrive; + this.navx = navx; + reset(new Vector3<>(0.0, 0.0, 0.0), 0.0); + initializeControllers(); + } + + public Navigator(NavXDevice navx, Vector2 startLocation, double startHeading, boolean omnidirectionalDrive) { + this.omnidirectionalDrive = omnidirectionalDrive; + this.navx = navx; + reset(startLocation, startHeading); + initializeControllers(); + } + + public Navigator(NavXDevice navx, Vector3 startLocation, double startHeading, boolean omnidirectionalDrive) { + this.omnidirectionalDrive = omnidirectionalDrive; + this.navx = navx; + reset(startLocation, startHeading); + initializeControllers(); + } + + public void reset(Vector2 startLocation, double startHeading) { + reset(new Vector3<>(startLocation.x, startLocation.y, 0.0), startHeading); + } + + public void reset(Vector3 startLocation, double startHeading) { + this.startLocation = startLocation; + this.startHeading = startHeading; + navx.reset(); + } + + private void initializeControllers() { + accelX = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_X); + accelY = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Y); + accelZ = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Z); + + rotX = new NavXPIDController(navx, NavXPIDController.DataSource.PITCH); + rotY = new NavXPIDController(navx, NavXPIDController.DataSource.ROLL); + rotZ = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + } +} 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/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/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java new file mode 100644 index 0000000..0a6fd03 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.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.nav.EncodedMotor; +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 java.text.DecimalFormat; + +/** + * OpMode designed to test extended functionality of the NavX sensor + * Requires four motors to test PID + */ +public class NavXFullNavigation 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 = 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("#.##"); + + 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 (important!) + 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")); + + //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()); + } +} From d7dcdf2877d0d47b5c9304bb7472605d672c1656 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 8 Jan 2016 21:13:42 -0600 Subject: [PATCH 09/43] Write a matrix implementation --- .../library/nav/kalman/Matrix.java | 407 ++++++++++++++++++ 1 file changed, 407 insertions(+) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java new file mode 100644 index 0000000..d081d6e --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java @@ -0,0 +1,407 @@ +package com.lasarobotics.library.nav.kalman; + +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; + } +} From 3ed6f7b2d749d6a41f81d2ec8037f54e0cadcbb1 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 09:06:42 -0600 Subject: [PATCH 10/43] Start Kalman filter implementation --- .../library/nav/kalman/Kalman.java | 218 ++++++++++++++++++ 1 file changed, 218 insertions(+) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java new file mode 100644 index 0000000..3260374 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java @@ -0,0 +1,218 @@ +package com.lasarobotics.library.nav.kalman; + +/** + * 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 Kalman { + //Last gain determinant, useful for debug + public double lastGain; + /** + * 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(); + + /** + * 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.rows; i++) { + double m = 1; + m_f.set(i, i, m); + for (int j = i + 1; j < m_f.columns; 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.rows, m_x.rows); + 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.columns, kh.rows); + 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 From afb4c9688893ea1912b39dd7f103b3bcb7a3a65e Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 15:04:42 -0600 Subject: [PATCH 11/43] Test a robot and confirm PID! --- .../library/nav/kalman/Kalman.java | 2 +- .../opmodes/FtcOpModeRegister.java | 7 + .../opmodes/Teleop5998.java | 123 ++++++++++++++++++ .../opmodes/navx/NavXRotatePID.java | 103 +++++++++++++++ .../opmodes/navx/NavXSensorTest.java | 46 +------ 5 files changed, 236 insertions(+), 45 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java index 3260374..a4666c5 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java @@ -157,7 +157,7 @@ public void reset(Matrix q, Matrix r, Matrix p, Matrix x) { * @param dt Time delta since last measuremment * @return Latest estimate */ - public double Update(Matrix measurement, double dt) { + public double update(Matrix measurement, double dt) { // Predict to now, then update. // Predict: // X = F*X + H*U 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 0a36f49..3c4f6e5 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -31,6 +31,9 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXRotatePID; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXSensorTest; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXProcessedOp; import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; @@ -54,5 +57,9 @@ public void register(OpModeManager manager) { manager.register("Logging Sample", LoggingSample.class); manager.register("Options Sample", OptionsSample.class); manager.register("Sensors Test", SensorsTest.class); + manager.register("NavX Official Test", navXProcessedOp.class); + manager.register("NavX Sensor Test", NavXSensorTest.class); + manager.register("NavX Move forward PID", NavXRotatePID.class); + manager.register("Teleop 5998", Teleop5998.class); } } 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..3d28b4d --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java @@ -0,0 +1,123 @@ +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.Timers; +import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; + +public class Teleop5998 extends OpMode { + + DcMotor frontLeft, frontRight, backLeft, backRight; + DcMotor intake, lift; + //Servo slide, dump; + Controller firstController; + Controller secondController; + DcMotor goalOne, goalTwo; + 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"); + robotTimer = new Timers(); + firstController = new Controller(gamepad1); + secondController = new Controller(gamepad2); + //slide.setPosition(.53); + //dump.setPosition(.53); + status = LiftStatus.CENTER; + } + + public void loop() { + firstController.update(gamepad1); + secondController.update(gamepad2); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, -firstController.left_stick_y, + firstController.right_stick_y); + + if (firstController.right_bumper == ButtonState.PRESSED) { + goalOne.setPower(1); + } else if (firstController.right_trigger > 0.2) { + goalOne.setPower(-1); + } else { + goalOne.setPower(0); + } + + if (firstController.left_bumper == ButtonState.PRESSED) { + goalTwo.setPower(1); + } else if (firstController.left_trigger > 0.2) { + goalTwo.setPower(-1); + } else { + goalTwo.setPower(0); + } + + if (secondController.dpad_up == ButtonState.PRESSED) { + intake.setPower(1); + } else if (secondController.dpad_down == ButtonState.PRESSED) { + intake.setPower(-1); + } else if (secondController.dpad_left == ButtonState.PRESSED) { + intake.setPower(0); + } else if (secondController.dpad_right == ButtonState.PRESSED) { + intake.setPower(0); + } + + if (secondController.left_bumper == ButtonState.HELD) { + lift.setPower(.3); + } else if (secondController.right_bumper == ButtonState.HELD) { + lift.setPower(-.3); + } else { + lift.setPower(0); + } + + /*if (secondController.x == ButtonState.HELD) { + status = LiftStatus.LEFT; + robotTimer.startClock("dumpTimer"); + slide.setPosition(1); + } else if (secondController.b == ButtonState.HELD) { + status = LiftStatus.RIGHT; + robotTimer.startClock("dumpTimer"); + slide.setPosition(0); + } else if (secondController.a == ButtonState.HELD) { + status = LiftStatus.CENTER; + slide.setPosition(0.5); + dump.setPosition(0.5); + } + + if ((status == LiftStatus.LEFT) && (robotTimer.getClockValue("dumpTimer") >= 100)) { + dump.setPosition(0); + } else if ((status == LiftStatus.RIGHT) && (robotTimer.getClockValue("dumpTimer") >= 100)) { + dump.setPosition(1); + }*/ + + + } + + public void stop() { // make sure nothing moves after the end of the match + intake.setPower(0); + frontLeft.setPower(0); + frontRight.setPower(0); + backLeft.setPower(0); + backRight.setPower(0); + goalOne.setPower(0); + goalTwo.setPower(0); + lift.setPower(0); + //slide.setPosition(.53); + //dump.setPosition(.53); + } + + private enum LiftStatus { + LEFT, + RIGHT, + CENTER + } +} 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..6a70583 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -0,0 +1,103 @@ +package com.qualcomm.ftcrobotcontroller.opmodes.navx; + +import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; +import com.lasarobotics.library.nav.EncodedMotor; +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 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 = 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("#.##"); + + 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 (important!) + 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")); + + //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(); + + 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", 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 index 1ace171..ca1b4bd 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXSensorTest.java @@ -1,11 +1,8 @@ package com.qualcomm.ftcrobotcontroller.opmodes.navx; import com.lasarobotics.library.controller.Controller; -import com.lasarobotics.library.drive.Tank; -import com.lasarobotics.library.nav.EncodedMotor; 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 java.text.DecimalFormat; @@ -17,7 +14,7 @@ public class NavXSensorTest 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 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 @@ -27,19 +24,10 @@ public class NavXSensorTest extends OpMode implements NavXDataReceiver { 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 (important!) - 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")); - //Instantiate controllers one = new Controller(gamepad1); @@ -47,20 +35,6 @@ public void init() { //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 @@ -71,31 +45,15 @@ public void start() { 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()); + telemetry.addData("NavX Jerk", df.format(navx.getJerk())); } } From efc92f10c19d0c7c26ee61c501a7c4a5790a960a Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 19:22:16 -0600 Subject: [PATCH 12/43] Add antistall protection --- .../lasarobotics/library/nav/Navigator.java | 47 ++++++++++++++++--- .../kauailabs/navx/NavXPIDController.java | 32 +++++++++++-- .../opmodes/FtcOpModeRegister.java | 2 +- .../opmodes/navx/NavXRotatePID.java | 14 +++++- 4 files changed, 83 insertions(+), 12 deletions(-) diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java index f15c6fe..31a1e65 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java @@ -1,5 +1,7 @@ package com.lasarobotics.library.nav; +import android.util.Log; + import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; import com.lasarobotics.library.util.Vector2; @@ -23,6 +25,12 @@ */ public class Navigator { + static NavXPIDController accelX; + static NavXPIDController accelY; + static NavXPIDController accelZ; + static NavXPIDController rotX; + static NavXPIDController rotY; + static NavXPIDController rotZ; /*** * INITIALIZATION ***/ @@ -35,12 +43,6 @@ public class Navigator { ***/ NavXDevice navx; - NavXPIDController accelX; - NavXPIDController accelY; - NavXPIDController accelZ; - NavXPIDController rotX; - NavXPIDController rotY; - NavXPIDController rotZ; public Navigator(NavXDevice navx, boolean omnidirectionalDrive) { this.omnidirectionalDrive = omnidirectionalDrive; @@ -82,4 +84,37 @@ private void initializeControllers() { rotY = new NavXPIDController(navx, NavXPIDController.DataSource.ROLL); rotZ = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); } + + public void setPID(Controller controller, double p, double i, double d) { + controller.getController().setPID(p, i, d); + Log.d("Navigator PID", controller.getController().getCoefficientString()); + } + + public enum Controller { + ACCEL_X, + ACCEL_Y, + ACCEL_Z, + GYRO_X, + GYRO_Y, + GYRO_Z; + + public NavXPIDController getController() { + switch (this) { + case ACCEL_X: + return accelX; + case ACCEL_Y: + return accelY; + case ACCEL_Z: + return accelZ; + case GYRO_X: + return rotX; + case GYRO_Y: + return rotY; + case GYRO_Z: + return rotZ; + default: + throw new RuntimeException("No such instance!"); + } + } + } } 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 index a59265a..da513fd 100644 --- 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 @@ -3,13 +3,16 @@ import android.util.Log; import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.util.MathUtil; /** * 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 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); @@ -33,6 +36,10 @@ public double getCoefficientFastForward() { return ff; } + public String getCoefficientString() { + return "p: " + p + ", i: " + i + ", d: " + d + ", ff: " + ff; + } + public void reset() { super.reset(); } @@ -59,9 +66,28 @@ public void waitForUpdate(PIDState state) { } public boolean isUpdateAvailable(PIDState state) { - return isNewUpdateAvailable(state); + boolean isAvailable = isNewUpdateAvailable(state); + if (isAvailable) { + state.output = MathUtil.deadband(antistallDeadband, state.output); + } + return isAvailable; + } + + public double getAntistallDeadband() { + return antistallDeadband; } + public void setAntistallDeadband(double minMotorPower) { + antistallDeadband = minMotorPower; + } + + public void enableAntistall() { + enableAntistall = true; + } + + public void disableAntistall() { + enableAntistall = false; + } /** * Returns the latest output value, selected by the DataSource when creating the controller @@ -69,7 +95,7 @@ public boolean isUpdateAvailable(PIDState state) { * @return Latest output value as a double */ public double getOutputValue() { - return super.get(); + return enableAntistall ? MathUtil.deadband(antistallDeadband, super.get()) : super.get(); } public void setTolerance(ToleranceType toleranceType, double tolerance) { 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 3c4f6e5..ebc4f74 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -59,7 +59,7 @@ public void register(OpModeManager manager) { manager.register("Sensors Test", SensorsTest.class); manager.register("NavX Official Test", navXProcessedOp.class); manager.register("NavX Sensor Test", NavXSensorTest.class); - manager.register("NavX Move forward PID", NavXRotatePID.class); + manager.register("NavX Rotate PID Test", NavXRotatePID.class); manager.register("Teleop 5998", Teleop5998.class); } } 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 index 6a70583..a5bd2e9 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -1,5 +1,6 @@ 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; @@ -59,9 +60,18 @@ public void init() { 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); + //Set PID variables + //Incresing P increases "jerkiness", D can make the device under/over-shoot, and I would not mess with I + yawPIDController.setPID(0.005, 0, 0); //Start data collection yawPIDController.start(); + //Initiate blank PID state yawPIDState = new NavXPIDController.PIDState(); } @@ -97,7 +107,7 @@ public void stop() { @Override public void dataReceived(long timeDelta) { - telemetry.addData("NavX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); - telemetry.addData("NavX Jerk", navx.getJerk()); + telemetry.addData("navX Collision", navx.hasCollided() ? "COLLIDED!" : "No collision"); + telemetry.addData("navX Jerk", df.format(navx.getJerk())); } } From bb74002ddc47487a98704d8fb616687b45ced3d6 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 22:36:10 -0600 Subject: [PATCH 13/43] Do a lot... for Navigator, try using encoders first --- ...ary_1_1util_1_1_distance_unit-members.html | 67 +++++- ..._1_1library_1_1util_1_1_distance_unit.html | 5 +- docs/doxygen/html/hierarchy.html | 9 +- .../lasarobotics/library/skynet/Kalman.html | 2 +- .../library/util/DistanceUnit.html | 2 +- .../library/drive/DriveSpecification.java | 46 ++++ .../library/drive/Drivetrain.java | 30 +++ .../{kalman/Kalman.java => KalmanFilter.java} | 25 +- .../lasarobotics/library/nav/Navigator.java | 213 +++++++++++++----- .../kauailabs/navx/NavXPIDController.java | 22 +- .../library/util/DistanceUnit.java | 13 -- .../library/{nav/kalman => util}/Matrix.java | 2 +- .../com/lasarobotics/library/util/Units.java | 123 ++++++++++ .../opmodes/FtcOpModeRegister.java | 4 + .../opmodes/navx/NavXDriveForwardPID.java | 117 ++++++++++ .../opmodes/navx/NavXRotatePID.java | 7 +- 16 files changed, 586 insertions(+), 101 deletions(-) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/drive/Drivetrain.java rename ftc-library/src/main/java/com/lasarobotics/library/nav/{kalman/Kalman.java => KalmanFilter.java} (91%) delete mode 100644 ftc-library/src/main/java/com/lasarobotics/library/util/DistanceUnit.java rename ftc-library/src/main/java/com/lasarobotics/library/{nav/kalman => util}/Matrix.java (99%) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/util/Units.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java 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/com/lasarobotics/library/skynet/Kalman.html b/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html index 7880b0a..d46d513 100644 --- a/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html +++ b/docs/javadoc/com/lasarobotics/library/skynet/Kalman.html @@ -86,7 +86,7 @@

    Class Kalman

  • java.lang.Object
    • -
    • com.lasarobotics.library.nav.Kalman
      • +
    • com.lasarobotics.library.nav.KalmanFilter
    • 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/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java new file mode 100644 index 0000000..014e35d --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java @@ -0,0 +1,46 @@ +package com.lasarobotics.library.drive; + +import com.lasarobotics.library.util.Units; + +/** + * Drivetrain specification used by a variety of classes + */ +public class DriveSpecification { + Drivetrain drive; //Drive train type + double wheelRadius; //wheel radius in meters + + /** + * Instantiate a drIve train specIfication + * + * @param drive Current drive train + * @param wheelRadius Wheel radius + * @param radiusUnit Unit of whell radius + */ + DriveSpecification(Drivetrain drive, double wheelRadius, Units.Distance radiusUnit) { + this.drive = drive; + try { + this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); + } catch (Units.DistanceManualConversionException e) { + throw new RuntimeException("Cannot convert from this unit!"); + } + } + + public double getWheelRadius() { + return wheelRadius; + } + + public double convertToDistance(double value, Units.Angle convertFrom, Units.Distance convertTo) { + try { + //1 radian = 1 arc length = 1 Circumference / 2PI + //Distance Traveled = radians * wheel radius in meters * conversion unit + return convertFrom.convertTo(Units.Angle.RADIANS, value) * wheelRadius * + Units.Distance.METERS.convertTo(convertTo, 1); + } catch (Units.DistanceManualConversionException e) { + throw new RuntimeException("Cannot convert to this unit!"); + } + } + + public Drivetrain getDrivetrain() { + return drive; + } +} 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/kalman/Kalman.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java similarity index 91% rename from ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java rename to ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java index a4666c5..60cf452 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Kalman.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/KalmanFilter.java @@ -1,4 +1,6 @@ -package com.lasarobotics.library.nav.kalman; +package com.lasarobotics.library.nav; + +import com.lasarobotics.library.util.Matrix; /** * Kalman @@ -18,9 +20,7 @@ * Derived classes need to perhaps hide certain matrixes to simplify the * interface. Also perhaps override: setupFMatrix, predict, Reset or Update. */ -public class Kalman { - //Last gain determinant, useful for debug - public double lastGain; +public class KalmanFilter { /** * State matrix (X) */ @@ -61,6 +61,15 @@ public class Kalman { * 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 @@ -102,10 +111,10 @@ public double variance(int index) { public void setupFMatrix(double dt) { m_f.zero(); - for (int i = 0; i < m_f.rows; i++) { + 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.columns; j++) { + for (int j = i + 1; j < m_f.getColumns(); j++) { m *= dt; m_f.set(i, i, m); } @@ -194,7 +203,7 @@ public double update(Matrix measurement, double dt) { Matrix ht = Matrix.transpose(m_h); Matrix tmp = Matrix.multiply(m_p, ht); Matrix sinv = Matrix.invert(s); - Matrix k = new Matrix(y.rows, m_x.rows); + Matrix k = new Matrix(y.getRows(), m_x.getRows()); if (sinv != null) { k = Matrix.multiply(tmp, sinv); } @@ -206,7 +215,7 @@ public double update(Matrix measurement, double dt) { // P = (I – K * H) * P Matrix kh = Matrix.multiply(k, m_h); - Matrix id = new Matrix(kh.columns, kh.rows); + Matrix id = new Matrix(kh.getColumns(), kh.getRows()); id.makeIdentity(); id.subtract(kh); id.multiply(m_p); diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java index 31a1e65..d75aea1 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java @@ -2,6 +2,7 @@ import android.util.Log; +import com.lasarobotics.library.drive.DriveSpecification; import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; import com.lasarobotics.library.util.Vector2; @@ -25,77 +26,111 @@ */ public class Navigator { + static NavXDevice navx; + + /** + * Controllers - we track all axis for finding out when we are on a platform, + * but because robots only have really 1 DoF, we only track the acceleration + * relative to the field and the yaw rotation + */ static NavXPIDController accelX; static NavXPIDController accelY; - static NavXPIDController accelZ; - static NavXPIDController rotX; - static NavXPIDController rotY; + //static NavXPIDController accelZ; + //static NavXPIDController rotX; + //static NavXPIDController rotY; static NavXPIDController rotZ; - /*** - * INITIALIZATION - ***/ - - Vector3 startLocation = new Vector3<>(0.0, 0.0, 0.0); - double startHeading = 0.0; - boolean omnidirectionalDrive = false; - /*** - * CONTROLLERS - ***/ - - NavXDevice navx; - - public Navigator(NavXDevice navx, boolean omnidirectionalDrive) { - this.omnidirectionalDrive = omnidirectionalDrive; - this.navx = navx; - reset(new Vector3<>(0.0, 0.0, 0.0), 0.0); - initializeControllers(); - } - public Navigator(NavXDevice navx, Vector2 startLocation, double startHeading, boolean omnidirectionalDrive) { - this.omnidirectionalDrive = omnidirectionalDrive; - this.navx = navx; - reset(startLocation, startHeading); - initializeControllers(); - } + Vector3 startLocation = new Vector3<>(0.0, 0.0, 0.0); //robot start location in field coordinates + double startHeading = 0.0; //robot start heading relative to field + DriveSpecification driveSpec; //drive specifications + + Vector3 position = new Vector3<>(0.0, 0.0, 0.0); //current position + Vector3 velocity = new Vector3<>(0.0, 0.0, 0.0); //current velocity + Vector3 acceleration = new Vector3<>(0.0, 0.0, 0.0); //current acceleration + + NavigatorTarget target = null; //current target + double toleranceRot = 2; //rotational tolerance in degrees + double toleranceLoc = 0.01; //directional tolerance in meters + + // + // INITIALIZATION + // - public Navigator(NavXDevice navx, Vector3 startLocation, double startHeading, boolean omnidirectionalDrive) { - this.omnidirectionalDrive = omnidirectionalDrive; - this.navx = navx; - reset(startLocation, startHeading); - initializeControllers(); + public Navigator(NavXDevice navx, DriveSpecification driveSpec) { + Navigator.navx = navx; + zero(new Vector3<>(0.0, 0.0, 0.0), 0.0); + initialize(driveSpec); } - public void reset(Vector2 startLocation, double startHeading) { - reset(new Vector3<>(startLocation.x, startLocation.y, 0.0), startHeading); + public Navigator(NavXDevice navx, DriveSpecification driveSpec, Vector2 startLocation, double startHeading) { + Navigator.navx = navx; + zero(startLocation, startHeading); + initialize(driveSpec); } - public void reset(Vector3 startLocation, double startHeading) { - this.startLocation = startLocation; - this.startHeading = startHeading; - navx.reset(); + public Navigator(NavXDevice navx, DriveSpecification driveSpec, Vector3 startLocation, double startHeading) { + Navigator.navx = navx; + zero(startLocation, startHeading); + initialize(driveSpec); } - private void initializeControllers() { + private void initialize(DriveSpecification driveSpec) { + zero(); + this.driveSpec = driveSpec; + accelX = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_X); accelY = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Y); - accelZ = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Z); + //accelZ = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Z); - rotX = new NavXPIDController(navx, NavXPIDController.DataSource.PITCH); - rotY = new NavXPIDController(navx, NavXPIDController.DataSource.ROLL); + //rotX = new NavXPIDController(navx, NavXPIDController.DataSource.PITCH); + //rotY = new NavXPIDController(navx, NavXPIDController.DataSource.ROLL); rotZ = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); } + public void zero() { + zero(new Vector3<>(0.0, 0.0, 0.0), 0.0); + } + + public void zero(Vector2 startLocation, double startHeading) { + zero(new Vector3<>(startLocation.x, startLocation.y, 0.0), startHeading); + } + + public void zero(Vector3 startLocation, double startHeading) { + this.startLocation = startLocation; + this.startHeading = startHeading; + navx.reset(); + + position = new Vector3<>(0.0, 0.0, 0.0); + velocity = new Vector3<>(0.0, 0.0, 0.0); + acceleration = new Vector3<>(0.0, 0.0, 0.0); + } + public void setPID(Controller controller, double p, double i, double d) { controller.getController().setPID(p, i, d); Log.d("Navigator PID", controller.getController().getCoefficientString()); } + public void setTarget(NavigatorTarget target) { + boolean start = this.target == null; //if true, we should start tasks + this.target = target; + updateParameters(); + } + + // + // TARGETING + // + + private void updateParameters() { + accelX.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceLoc); + accelY.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceLoc); + rotZ.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceRot); + + //accelX.setSetpoint(); + } + public enum Controller { ACCEL_X, ACCEL_Y, - ACCEL_Z, - GYRO_X, - GYRO_Y, GYRO_Z; public NavXPIDController getController() { @@ -104,12 +139,6 @@ public NavXPIDController getController() { return accelX; case ACCEL_Y: return accelY; - case ACCEL_Z: - return accelZ; - case GYRO_X: - return rotX; - case GYRO_Y: - return rotY; case GYRO_Z: return rotZ; default: @@ -117,4 +146,84 @@ public NavXPIDController getController() { } } } + + //TODO: implement Bezier path-based targeting ? + + // + // RESULTS + // + + public static class NavigatorResult { + Vector2 power; + + NavigatorResult() { + power = new Vector2(0.0f, 0.0f); + } + + /** + * Get a power vector suitable for arcade control + * + * @return A vector with the power returning as an X and Y magnitude + */ + public Vector2 getPowerVector() { + return power; + } + + /** + * Get a power vector suitable for tank drive control + * + * @return A vector with the first index being the left side motors and the second index the right motors + */ + public Vector2 getPowerTankDrive() { + //FIXME implement + throw new RuntimeException("Not yet implemented!"); + } + + /** + * Get a power vector suitable for arcade control or an omnidirectional drive train + * + * @return A vector with the first index being the magnitude and the second a heading, + * which can can be from -360 (counterclockwise rotation) to 360 (clockwise) + */ + public Vector2 getPowerMagnitudeDirection() { + //FIXME implement + throw new RuntimeException("Not yet implemented!"); + } + + } + + public static class NavigatorTarget { + Vector2 targetPosition; + double heading; + + /** + * Instantiate a navigator target with a target position in field coordinates and a target heading + * + * @param targetPosition Target position, in field coordinates. These coordinates can be reset(). + * @param heading Target heading, or direction to face. 0 is North, -90 or 270 is West, etc. Relative to robot. + */ + public NavigatorTarget(Vector2 targetPosition, double heading) { + this.targetPosition = targetPosition; + this.heading = heading; + } + + /** + * Instantiate a navigator target with a 2D target vector, either in field coordinates + * or a distance relative to the robot's current position + * If robotRelative is true, the target position is a vector representing the + * distance away from the current location + * The direction of the vector represents the heading. + * Otherwise, targetPosition is a location in field coordinates, defined by startLocation. + * + * @param targetPosition Robot's target position, in either field or inertial coordinates + * @param heading Robot's target heading, either relative to the field (as with field oriented + * drive) or to the robot's current rotation + * @param robotRelative True if targetPosition is in inertial (robot-relative) coordianates, + * False if targetPosition is in field coordinates + */ + public NavigatorTarget(Vector2 targetPosition, double heading, boolean robotRelative) { + //FIXME implement + throw new RuntimeException("Not yet implemented!"); + } + } } 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 index da513fd..25493eb 100644 --- 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 @@ -73,6 +73,15 @@ public boolean isUpdateAvailable(PIDState state) { 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 getAntistallDeadband() { return antistallDeadband; } @@ -81,6 +90,10 @@ public void setAntistallDeadband(double minMotorPower) { antistallDeadband = minMotorPower; } + public void setAntistall(boolean enableAntistall) { + this.enableAntistall = enableAntistall; + } + public void enableAntistall() { enableAntistall = true; } @@ -89,15 +102,6 @@ public void disableAntistall() { enableAntistall = false; } - /** - * 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 void setTolerance(ToleranceType toleranceType, double tolerance) { super.setTolerance(toleranceType.val, tolerance); } 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/nav/kalman/Matrix.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java similarity index 99% rename from ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java rename to ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java index d081d6e..4b9b78e 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/kalman/Matrix.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Matrix.java @@ -1,4 +1,4 @@ -package com.lasarobotics.library.nav.kalman; +package com.lasarobotics.library.util; public class Matrix { /** 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..ba39843 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java @@ -0,0 +1,123 @@ +package com.lasarobotics.library.util; + +/** + * Units for conversion + */ +public class Units { + + public enum Distance implements Unit { + ENCODER_COUNTS(Double.NaN), + WHEEL_REVOLUTIONS(Double.NaN), + WHEEL_DEGREES(Double.NaN), + INCHES(0.0254), + FEET(0.3048), + CENTIMETERS(1.0 / 100.0), + METERS(1); //default unit + + protected double conversionFactor; + + Distance(double conversionFactor) { + this.conversionFactor = conversionFactor; + } + + @Override + public double getConversionFactor() { + return conversionFactor; + } + + @Override + public double convertTo(Unit other, double value) throws DistanceManualConversionException { + if (other.getConversionFactor() == Double.NaN || this.getConversionFactor() == Double.NaN) + throw new DistanceManualConversionException(); + value *= conversionFactor; + value /= other.getConversionFactor(); + return value; + } + + @Override + public Unit getDefaultUnit() { + return METERS; + } + + @Override + public boolean isConvertable() { + return conversionFactor != Double.NaN; + } + } + + public enum Angle implements Unit { + DEGREES(1), + RADIANS(180 / Math.PI); + + protected double conversionFactor; + + Angle(double conversionFactor) { + this.conversionFactor = conversionFactor; + } + + @Override + public double getConversionFactor() { + return conversionFactor; + } + + @Override + public double convertTo(Unit other, double value) throws DistanceManualConversionException { + value *= conversionFactor; + value /= other.getConversionFactor(); + return value; + } + + @Override + public boolean isConvertable() { + return true; + } + + public Angle getDefaultUnit() { + return DEGREES; + } + } + + 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 + * @throws DistanceManualConversionException Method will throw exception if the unit + * requested or the current unit are not inter-convertible because some information + * is lacking. e.g. we need wheel radius to convert inches into wheel revolutions. + */ + double convertTo(Unit other, double value) throws DistanceManualConversionException; + + /** + * Test whether a unit is convertable and therefore will not throw an error if converted from/to + * + * @return True if convertable, false otherwise + */ + boolean isConvertable(); + } + + /** + * Exception that throws when more data is necessary to convert from one + */ + public static class DistanceManualConversionException extends Exception { + DistanceManualConversionException() { + super("Unable to convert to this unit due to unknown information! Manually convert."); + } + } +} 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 ebc4f74..4422795 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -31,8 +31,10 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXDriveForwardPID; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXRotatePID; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXSensorTest; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXDriveStraightPIDLoopOp; import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXProcessedOp; import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; @@ -58,8 +60,10 @@ public void register(OpModeManager manager) { manager.register("Options Sample", OptionsSample.class); manager.register("Sensors Test", SensorsTest.class); manager.register("NavX Official Test", navXProcessedOp.class); + manager.register("NavX Official Forward PID Test", navXDriveStraightPIDLoopOp.class); manager.register("NavX Sensor Test", NavXSensorTest.class); manager.register("NavX Rotate PID Test", NavXRotatePID.class); + manager.register("NavX Forward PID Test", NavXDriveForwardPID.class); manager.register("Teleop 5998", Teleop5998.class); } } 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..ebb22c4 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java @@ -0,0 +1,117 @@ +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.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.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 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 (important!) + 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")); + + //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/NavXRotatePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java index a5bd2e9..b86020e 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -21,7 +21,7 @@ public class NavXRotatePID extends OpMode implements NavXDataReceiver { 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 = 90.0; //target angle for PID + 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; @@ -55,7 +55,7 @@ public void init() { //Set the target location yawPIDController.setSetpoint(NAVX_TARGET_ANGLE_DEGREES); //Allow crossing over the bounds (see setContinuous() documentation) - yawPIDController.setContinuous(true); + yawPIDController.setContinuous(false); //Set angle tolerance yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); //Set P,I,D coefficients @@ -65,9 +65,6 @@ public void init() { 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); - //Set PID variables - //Incresing P increases "jerkiness", D can make the device under/over-shoot, and I would not mess with I - yawPIDController.setPID(0.005, 0, 0); //Start data collection yawPIDController.start(); From d48a758784173518d8715e3592eb31124c726c04 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 23:23:02 -0600 Subject: [PATCH 14/43] Fix encoder class --- .../library/nav/EncodedMotor.java | 10 +++---- .../lasarobotics/library/nav/Navigator.java | 7 +++++ .../opmodes/EncoderTest.java | 26 +++++++------------ .../opmodes/FtcOpModeRegister.java | 3 +++ ...avigation.java => NavXNavigationTest.java} | 6 ++--- 5 files changed, 28 insertions(+), 24 deletions(-) rename sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/{NavXFullNavigation.java => NavXNavigationTest.java} (95%) 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 index 55e02be..f984b87 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -49,7 +49,7 @@ public void moveDistance(int distance) { else super.setDirection(Direction.FORWARD); - setTargetPosition(distance); + setTargetPosition(Math.abs(distance)); } /** @@ -60,19 +60,19 @@ public void moveDistance(int distance) { * @return True if the robot has reached the position */ public boolean hasReachedPosition(double position) { - return Math.abs(getCurrentPosition()) > Math.abs(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); + 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); + setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS); isEncoderEnabled = false; } @@ -97,7 +97,7 @@ public void resetEncoder() { return; //This command requires (at least ?) one execution loop to reset the encoders - super.setMode(DcMotorController.RunMode.RESET_ENCODERS); + setMode(DcMotorController.RunMode.RESET_ENCODERS); encodersResetting = true; encoderOffset = -super.getCurrentPosition(); diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java index d75aea1..0efdbc4 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java @@ -23,6 +23,13 @@ * 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 */ public class Navigator { 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 6e459d1..0a10fba 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,6 +1,7 @@ package com.qualcomm.ftcrobotcontroller.opmodes; import com.lasarobotics.library.controller.Controller; +import com.lasarobotics.library.drive.Tank; import com.lasarobotics.library.nav.EncodedMotor; import com.qualcomm.robotcore.eventloop.opmode.OpMode; @@ -19,28 +20,21 @@ public void init() { //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(1000); } public void loop() { one.update(gamepad1); - 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); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0, 0); + else + Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0.5, -0.5); + + telemetry.addData("Front Left: ", frontLeft.getCurrentPosition()); + telemetry.addData("Front Right: ", frontRight.getCurrentPosition()); + telemetry.addData("Back Left: ", backLeft.getCurrentPosition()); + telemetry.addData("Back Right: ", backRight.getCurrentPosition()); } public void stop() { 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 4422795..d1c2ffc 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -32,6 +32,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXDriveForwardPID; +import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXNavigationTest; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXRotatePID; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXSensorTest; import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXDriveStraightPIDLoopOp; @@ -59,11 +60,13 @@ public void register(OpModeManager manager) { manager.register("Logging Sample", LoggingSample.class); manager.register("Options Sample", OptionsSample.class); manager.register("Sensors Test", SensorsTest.class); + manager.register("Encoder Test", EncoderTest.class); manager.register("NavX Official Test", navXProcessedOp.class); manager.register("NavX Official Forward PID Test", navXDriveStraightPIDLoopOp.class); manager.register("NavX Sensor Test", NavXSensorTest.class); manager.register("NavX Rotate PID Test", NavXRotatePID.class); manager.register("NavX Forward PID Test", NavXDriveForwardPID.class); + manager.register("NavX Navigation Test", NavXNavigationTest.class); manager.register("Teleop 5998", Teleop5998.class); } } diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java similarity index 95% rename from sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java rename to sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java index 0a6fd03..1b2f6fb 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXFullNavigation.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java @@ -14,13 +14,13 @@ * OpMode designed to test extended functionality of the NavX sensor * Requires four motors to test PID */ -public class NavXFullNavigation extends OpMode implements NavXDataReceiver { +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 = 90.0; //target angle for PID + 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; From ac70dbbfd5a5cc7f61b2906a508209c8f5efcdff Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 9 Jan 2016 23:36:03 -0600 Subject: [PATCH 15/43] Start encoder improvements --- .../library/nav/EncodedMotor.java | 25 +++++++++++++++---- .../com/lasarobotics/library/util/Units.java | 3 ++- 2 files changed, 22 insertions(+), 6 deletions(-) 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 index f984b87..a1316d9 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -1,5 +1,6 @@ package com.lasarobotics.library.nav; +import com.lasarobotics.library.util.Units; import com.qualcomm.robotcore.hardware.DcMotor; import com.qualcomm.robotcore.hardware.DcMotorController; @@ -7,6 +8,7 @@ * Drive encoder support */ public class EncodedMotor extends DcMotor { + protected double wheelRadius; //wheel radius in meters private boolean isEncoderEnabled = true; private boolean encodersResetting = false; private int encoderOffset = 0; @@ -17,28 +19,41 @@ public class EncodedMotor extends DcMotor { * * @param motor DcMotor instance. */ - public EncodedMotor(DcMotor motor) { + public EncodedMotor(DcMotor motor, double wheelRadius, Units.Distance radiusUnits) { super(motor.getController(), motor.getPortNumber(), motor.getDirection()); enableEncoder(); + calculateRadius(wheelRadius, radiusUnits); } - protected EncodedMotor(DcMotorController controller, int portNumber) { + protected EncodedMotor(DcMotorController controller, int portNumber, double wheelRadius, Units.Distance radiusUnits) { super(controller, portNumber); enableEncoder(); + calculateRadius(wheelRadius, radiusUnits); } - protected EncodedMotor(DcMotorController controller, int portNumber, Direction direction) { + protected EncodedMotor(DcMotorController controller, int portNumber, Direction direction, double wheelRadius, Units.Distance radiusUnits) { super(controller, portNumber, direction); enableEncoder(); + calculateRadius(wheelRadius, radiusUnits); + } + + private void calculateRadius(double wheelRadius, Units.Distance radiusUnit) { + try { + this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); + } catch (Units.DistanceManualConversionException e) { + throw new RuntimeException("Cannot convert from this unit!"); + } } /** * 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(int distance) { + public void moveDistance(double distance) { if (distance == 0) return; @@ -49,7 +64,7 @@ public void moveDistance(int distance) { else super.setDirection(Direction.FORWARD); - setTargetPosition(Math.abs(distance)); + setTargetPosition(Math.abs((int) distance)); } /** 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 index ba39843..44c7d7c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java @@ -47,7 +47,8 @@ public boolean isConvertable() { public enum Angle implements Unit { DEGREES(1), - RADIANS(180 / Math.PI); + RADIANS(180 / Math.PI), + ENCODER_COUNTS(4); //encoders have 1440 resolutions / rotation, so 1 count = 1/4 degree protected double conversionFactor; From 81ea0f76ce80bf26e7c3b4f315a23cbb6080de1b Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 11:33:14 -0600 Subject: [PATCH 16/43] Write enocder distance API --- .../library/drive/DriveSpecification.java | 17 +--- .../library/nav/EncodedMotor.java | 41 ++++++-- .../com/lasarobotics/library/util/Units.java | 96 +++++++++++-------- .../opmodes/EncoderTest.java | 28 +++--- .../opmodes/navx/NavXDriveForwardPID.java | 14 ++- .../opmodes/navx/NavXNavigationTest.java | 14 +-- .../opmodes/navx/NavXRotatePID.java | 14 ++- 7 files changed, 132 insertions(+), 92 deletions(-) diff --git a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java index 014e35d..9413a8b 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java @@ -18,28 +18,13 @@ public class DriveSpecification { */ DriveSpecification(Drivetrain drive, double wheelRadius, Units.Distance radiusUnit) { this.drive = drive; - try { - this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); - } catch (Units.DistanceManualConversionException e) { - throw new RuntimeException("Cannot convert from this unit!"); - } + this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); } public double getWheelRadius() { return wheelRadius; } - public double convertToDistance(double value, Units.Angle convertFrom, Units.Distance convertTo) { - try { - //1 radian = 1 arc length = 1 Circumference / 2PI - //Distance Traveled = radians * wheel radius in meters * conversion unit - return convertFrom.convertTo(Units.Angle.RADIANS, value) * wheelRadius * - Units.Distance.METERS.convertTo(convertTo, 1); - } catch (Units.DistanceManualConversionException e) { - throw new RuntimeException("Cannot convert to this unit!"); - } - } - public Drivetrain getDrivetrain() { return drive; } 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 index a1316d9..45d986c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -38,11 +38,7 @@ protected EncodedMotor(DcMotorController controller, int portNumber, Direction d } private void calculateRadius(double wheelRadius, Units.Distance radiusUnit) { - try { - this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); - } catch (Units.DistanceManualConversionException e) { - throw new RuntimeException("Cannot convert from this unit!"); - } + this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); } /** @@ -57,7 +53,7 @@ public void moveDistance(double distance) { if (distance == 0) return; - resetEncoder(); + reset(); if (distance < 0) super.setDirection(Direction.REVERSE); @@ -78,6 +74,18 @@ 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 reset 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 reset the encoders setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); @@ -101,13 +109,14 @@ public void update() { if (encodersResetting && super.getCurrentPosition() == 0) { encodersResetting = false; encoderOffset = 0; + enableEncoder(); } } /** * Reset encoders. */ - public void resetEncoder() { + public void reset() { if (encodersResetting) return; @@ -123,6 +132,20 @@ public void setTargetPosition(int position) { super.setTargetPosition(position + encoderOffset); } + public void setTargetPosition(double position, Units.Distance unit) { + setTargetPosition((int) convertDistanceToEncoderCounts(position, 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 reset. * Note that encoders take at least one execution loop to fully reset. @@ -139,4 +162,8 @@ public int getCurrentPosition() { 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/util/Units.java b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java index 44c7d7c..7c0c1ce 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java @@ -6,9 +6,6 @@ public class Units { public enum Distance implements Unit { - ENCODER_COUNTS(Double.NaN), - WHEEL_REVOLUTIONS(Double.NaN), - WHEEL_DEGREES(Double.NaN), INCHES(0.0254), FEET(0.3048), CENTIMETERS(1.0 / 100.0), @@ -20,15 +17,37 @@ public enum Distance implements Unit { 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; } - @Override - public double convertTo(Unit other, double value) throws DistanceManualConversionException { - if (other.getConversionFactor() == Double.NaN || this.getConversionFactor() == Double.NaN) - throw new DistanceManualConversionException(); + public double convertTo(Distance other, double value) { value *= conversionFactor; value /= other.getConversionFactor(); return value; @@ -38,17 +57,13 @@ public double convertTo(Unit other, double value) throws DistanceManualConversio public Unit getDefaultUnit() { return METERS; } - - @Override - public boolean isConvertable() { - return conversionFactor != Double.NaN; - } } public enum Angle implements Unit { DEGREES(1), - RADIANS(180 / Math.PI), - ENCODER_COUNTS(4); //encoders have 1440 resolutions / rotation, so 1 count = 1/4 degree + RADIANS(180.0 / Math.PI), + REVOLUTIONS(360.0), + ENCODER_COUNTS(4.0); //encoders have 1440 resolutions / rotation, so 1 count = 1/4 degree protected double conversionFactor; @@ -56,23 +71,41 @@ public enum Angle implements Unit { 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; } - @Override - public double convertTo(Unit other, double value) throws DistanceManualConversionException { + public double convertTo(Angle other, double value) { value *= conversionFactor; value /= other.getConversionFactor(); return value; } - @Override - public boolean isConvertable() { - return true; - } - public Angle getDefaultUnit() { return DEGREES; } @@ -99,26 +132,7 @@ interface Unit { * @param other Another units * @param value Value to convert * @return Converted value, if possible - * @throws DistanceManualConversionException Method will throw exception if the unit - * requested or the current unit are not inter-convertible because some information - * is lacking. e.g. we need wheel radius to convert inches into wheel revolutions. - */ - double convertTo(Unit other, double value) throws DistanceManualConversionException; - - /** - * Test whether a unit is convertable and therefore will not throw an error if converted from/to - * - * @return True if convertable, false otherwise */ - boolean isConvertable(); - } - - /** - * Exception that throws when more data is necessary to convert from one - */ - public static class DistanceManualConversionException extends Exception { - DistanceManualConversionException() { - super("Unable to convert to this unit due to unknown information! Manually convert."); - } + //double convertTo(Unit other, double value); } } 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 0a10fba..be99a82 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -3,38 +3,44 @@ import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.util.Units; import com.qualcomm.robotcore.eventloop.opmode.OpMode; +import com.qualcomm.robotcore.hardware.DcMotor; public class EncoderTest extends OpMode { - EncodedMotor frontLeft, frontRight, backLeft, backRight; + private static final double WHEEL_RADIUS = 2; + private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + DcMotor frontLeft, frontRight, backRight; + EncodedMotor backLeft; Controller one; 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")); + frontLeft = hardwareMap.dcMotor.get("frontLeft"); + frontRight = hardwareMap.dcMotor.get("frontRight"); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), + WHEEL_RADIUS, WHEEL_RADIUS_UNIT); //set wheel radius for distance calculations + backRight = hardwareMap.dcMotor.get("backRight"); 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 - backLeft.setTargetPosition(1000); + backLeft.setTargetPosition(1, Units.Distance.FEET); + backLeft.reset(); } public void loop() { one.update(gamepad1); + backLeft.update(); - if (backLeft.hasReachedPosition(1000)) + if (backLeft.hasReachedPosition(1, Units.Distance.FEET)) Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0, 0); else Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0.5, -0.5); - telemetry.addData("Front Left: ", frontLeft.getCurrentPosition()); - telemetry.addData("Front Right: ", frontRight.getCurrentPosition()); - telemetry.addData("Back Left: ", backLeft.getCurrentPosition()); - telemetry.addData("Back Right: ", backRight.getCurrentPosition()); + telemetry.addData("Back Left (counts): ", backLeft.getCurrentPosition()); + telemetry.addData("Back Left (feet): ", backLeft.getCurrentPosition(Units.Distance.FEET)); } public void stop() { 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 index ebb22c4..b727cad 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java @@ -7,6 +7,7 @@ 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; @@ -26,6 +27,9 @@ public class NavXDriveForwardPID extends OpMode implements NavXDataReceiver { 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! @@ -35,11 +39,11 @@ public class NavXDriveForwardPID extends OpMode implements NavXDataReceiver { NavXPIDController.PIDState yawPIDState; public void init() { - //Create motors WITH ENCODERS (important!) - 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")); + //Create motors WITH ENCODERS (highly preferable) + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); //Instantiate controllers one = new Controller(gamepad1); 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 index 1b2f6fb..99ed457 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXNavigationTest.java @@ -2,11 +2,11 @@ import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; -import com.lasarobotics.library.nav.EncodedMotor; 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; @@ -27,18 +27,18 @@ public class NavXNavigationTest extends OpMode implements NavXDataReceiver { private static final DecimalFormat df = new DecimalFormat("#.##"); - EncodedMotor frontLeft, frontRight, backLeft, backRight; //make sure these have encoders! + DcMotor 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 (important!) - 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")); + //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); 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 index b86020e..17a4cf3 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -7,6 +7,7 @@ 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; @@ -26,6 +27,9 @@ public class NavXRotatePID extends OpMode implements NavXDataReceiver { 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! @@ -35,11 +39,11 @@ public class NavXRotatePID extends OpMode implements NavXDataReceiver { NavXPIDController.PIDState yawPIDState; public void init() { - //Create motors WITH ENCODERS (important!) - 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")); + //Create motors WITH ENCODERS (highly preferable) + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); //Instantiate controllers one = new Controller(gamepad1); From 8faf2b0e95428c3c550799f60cfa08c5a1daa286 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 12:04:19 -0600 Subject: [PATCH 17/43] [INCOMPLETE] Start PID measurement --- .../com/lasarobotics/library/nav/PID.java | 145 ++++++++++++++++++ 1 file changed, 145 insertions(+) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java 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..e95534e --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -0,0 +1,145 @@ +package com.lasarobotics.library.nav; + +/** + * PID Targeting + */ +public class PID { + protected double setpoint = 0; + protected double processValue = 0; // actual possition (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 Kp = 0.2, Ki = 0.01, Kd = 1; // PID constant multipliers + protected double dt = 100.0; // delta time + protected double output = 0; // the drive amount that effects the PV. + + public PID(double p, double i, double d) { + this.Kp = p; + this.Ki = i; + this.Kd = d; + } + + public void setSetpoint(double setpoint) { + this.setpoint = setpoint; + } + + public void addMeasurement(double measuredValue, double timeDelta) { + this.processValue = measuredValue; + this.dt = timeDelta; + } + + /** + * 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 double getCoefficientProportional() { + return Kp; + } + + public double getCoefficientIntegral() { + return Ki; + } + + public double getCoefficientDerivative() { + return Kd; + } + + public double getLastTimeDelta() { + return dt; + } + + public double getOutputValue() { + return output; + } + + /*This represents the speed at which electronics could actualy + sample the process values.. and do any work on them. + * [most industrial batching processes are SLOW, on the order of minutes. + * but were going to deal in times 10ms to 1 second. + * Most PLC's have relativly slow data busses, and would sample + * temperatures on the order of 100's of milliseconds. So our + * starting time interval is 100ms] + */ + private void tmrPID_Ctrl_Tick(object sender, EventArgs e) { /* + * Pseudocode from Wikipedia + * + previous_error = 0 + integral = 0 + start: + error = setpoint - PV(actual_position) + integral = integral + error*dt + derivative = (error - previous_error)/dt + output = Kp*error + Ki*integral + Kd*derivative + previous_error = error + wait(dt) + goto start + */ + // calculate the difference between the desired value and the actual value + error = setpoint - PV; + // 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 = (Kp * error) + (Ki * integral) + (Kd * derivative); + + // remember the error for the next time around. + previousError = error; + + } + + //This timer updates the process data. it needs to be the fastest + // interval in the system. + private void tmrPV_Tick(object sender, EventArgs e) { + /* this my version of cruise control. + * PV = PV + (output * .2) - (PV*.10); + * The equation contains values for speed, efficiency, + * and wind resistance. + Here 'PV' is the speed of the car. + 'output' is the amount of gas supplied to the engine. + * (It is only 20% efficient in this example) + And it looses energy at 10% of the speed. (The faster the + car moves the more PV will be reduced.) + * Noise is added randomly if checked, otherwise noise is 0.0 + * (Noise doesn't relate to the cruise control, but can be useful + * when modeling other processes.) + */ + PV = PV + (output * 0.20) - (PV * 0.10) + noise; + // change the above equation to fit your aplication + } +} From 3674b7d3052eaf03e2a41ea1050c66d2593730b2 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 14:37:18 -0600 Subject: [PATCH 18/43] Complete PID loop structure --- .../com/lasarobotics/library/nav/PID.java | 51 ++----------------- 1 file changed, 5 insertions(+), 46 deletions(-) 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 index e95534e..ea15d5c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -27,6 +27,7 @@ public void setSetpoint(double setpoint) { public void addMeasurement(double measuredValue, double timeDelta) { this.processValue = measuredValue; this.dt = timeDelta; + update(); } /** @@ -85,34 +86,13 @@ public double getOutputValue() { return output; } - /*This represents the speed at which electronics could actualy - sample the process values.. and do any work on them. - * [most industrial batching processes are SLOW, on the order of minutes. - * but were going to deal in times 10ms to 1 second. - * Most PLC's have relativly slow data busses, and would sample - * temperatures on the order of 100's of milliseconds. So our - * starting time interval is 100ms] - */ - private void tmrPID_Ctrl_Tick(object sender, EventArgs e) { /* - * Pseudocode from Wikipedia - * - previous_error = 0 - integral = 0 - start: - error = setpoint - PV(actual_position) - integral = integral + error*dt - derivative = (error - previous_error)/dt - output = Kp*error + Ki*integral + Kd*derivative - previous_error = error - wait(dt) - goto start - */ + private void update() { // calculate the difference between the desired value and the actual value - error = setpoint - PV; + error = setpoint - processValue; // track error over time, scaled to the timer interval - integral = integral + (error * Dt); + integral = integral + (error * dt); // determin the amount of change from the last time checked - derivative = (error - previousError) / Dt; + derivative = (error - previousError) / dt; // calculate how much drive the output in order to get to the // desired setpoint. @@ -120,26 +100,5 @@ private void tmrPID_Ctrl_Tick(object sender, EventArgs e) { /* // remember the error for the next time around. previousError = error; - - } - - //This timer updates the process data. it needs to be the fastest - // interval in the system. - private void tmrPV_Tick(object sender, EventArgs e) { - /* this my version of cruise control. - * PV = PV + (output * .2) - (PV*.10); - * The equation contains values for speed, efficiency, - * and wind resistance. - Here 'PV' is the speed of the car. - 'output' is the amount of gas supplied to the engine. - * (It is only 20% efficient in this example) - And it looses energy at 10% of the speed. (The faster the - car moves the more PV will be reduced.) - * Noise is added randomly if checked, otherwise noise is 0.0 - * (Noise doesn't relate to the cruise control, but can be useful - * when modeling other processes.) - */ - PV = PV + (output * 0.20) - (PV * 0.10) + noise; - // change the above equation to fit your aplication } } From 0bf2860880ec4c24b1411d4bda6bb277674cc1de Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 15:05:14 -0600 Subject: [PATCH 19/43] Complete a successful PID test! Next: tolerance and antistall --- .../library/nav/EncodedMotor.java | 17 ++++++++++-- .../com/lasarobotics/library/nav/PID.java | 7 ++++- .../opmodes/EncoderTest.java | 27 ++++++++++++++++--- 3 files changed, 44 insertions(+), 7 deletions(-) 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 index 45d986c..c9ae8bd 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -63,6 +63,10 @@ public void moveDistance(double distance) { 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 reset the encoders. @@ -127,13 +131,22 @@ public void reset() { encoderOffset = -super.getCurrentPosition(); } + public void setTargetPosition(double position, Units.Distance unit) { + setTargetPosition((int) convertDistanceToEncoderCounts(position, unit)); + } + + @Override + public int getTargetPosition() { + return super.getTargetPosition() - encoderOffset; + } + @Override public void setTargetPosition(int position) { super.setTargetPosition(position + encoderOffset); } - public void setTargetPosition(double position, Units.Distance unit) { - setTargetPosition((int) convertDistanceToEncoderCounts(position, unit)); + public double getTargetPosition(Units.Distance unit) { + return convertEncoderCountsToDistance(getTargetPosition(), unit); } public double convertDistanceToEncoderCounts(double distance, Units.Distance unit) { 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 index ea15d5c..79921d6 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -5,7 +5,7 @@ */ public class PID { protected double setpoint = 0; - protected double processValue = 0; // actual possition (Process Value) + 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 @@ -14,6 +14,11 @@ public class PID { protected double dt = 100.0; // delta time protected double output = 0; // the drive amount that effects the PV. + public PID() { + this.Kp = 0.005; + this.Ki = 0; + this.Kd = 0; + } public PID(double p, double i, double d) { this.Kp = p; this.Ki = i; 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 be99a82..50748d6 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -3,6 +3,8 @@ import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.PID; +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; @@ -14,6 +16,9 @@ public class EncoderTest extends OpMode { DcMotor frontLeft, frontRight, backRight; EncodedMotor backLeft; Controller one; + PID pidBackLeft; + + long lastTime = 0; public void init() { frontLeft = hardwareMap.dcMotor.get("frontLeft"); @@ -28,19 +33,33 @@ public void init() { //TODO meaning that they can only really be called from init() or a state machine backLeft.setTargetPosition(1, Units.Distance.FEET); backLeft.reset(); + + //Create PID looper + pidBackLeft = new PID(); + pidBackLeft.setSetpoint(1000); + } + + public void init_loop() { + lastTime = System.nanoTime(); } public void loop() { + long time = System.nanoTime(); + double timeDelta = (time - lastTime) / 1000000000.0; one.update(gamepad1); backLeft.update(); + pidBackLeft.addMeasurement(Math.abs(backLeft.getCurrentPosition()), timeDelta); + double power = pidBackLeft.getOutputValue(); - if (backLeft.hasReachedPosition(1, Units.Distance.FEET)) - Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0, 0); - else - Tank.motor4(frontLeft, frontRight, backLeft, backRight, 0.5, -0.5); + Tank.motor4(frontLeft, frontRight, backLeft, backRight, + MathUtil.coerce(-1, 1, power), MathUtil.coerce(-1, 1, -power)); telemetry.addData("Back Left (counts): ", backLeft.getCurrentPosition()); telemetry.addData("Back Left (feet): ", backLeft.getCurrentPosition(Units.Distance.FEET)); + telemetry.addData("Motor Power: ", power); + telemetry.addData("Time Delta: ", timeDelta); + + lastTime = time; } public void stop() { From 85dd25a60ab414a32df0a7a8216356134576ccbb Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 16:54:20 -0600 Subject: [PATCH 20/43] Simplify motor information and add future drivetrain config class --- .../library/drive/DriveConfiguration.java | 29 ++++++++++++++++ .../library/nav/EncodedMotor.java | 16 ++++----- .../lasarobotics/library/nav/MotorInfo.java | 33 +++++++++++++++++++ .../opmodes/EncoderTest.java | 8 +++-- 4 files changed, 74 insertions(+), 12 deletions(-) create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java 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..22068cc --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java @@ -0,0 +1,29 @@ +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; + +/** + * 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/nav/EncodedMotor.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java index c9ae8bd..88ef0ac 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -19,26 +19,22 @@ public class EncodedMotor extends DcMotor { * * @param motor DcMotor instance. */ - public EncodedMotor(DcMotor motor, double wheelRadius, Units.Distance radiusUnits) { + public EncodedMotor(DcMotor motor, MotorInfo info) { super(motor.getController(), motor.getPortNumber(), motor.getDirection()); enableEncoder(); - calculateRadius(wheelRadius, radiusUnits); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } - protected EncodedMotor(DcMotorController controller, int portNumber, double wheelRadius, Units.Distance radiusUnits) { + protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info) { super(controller, portNumber); enableEncoder(); - calculateRadius(wheelRadius, radiusUnits); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } - protected EncodedMotor(DcMotorController controller, int portNumber, Direction direction, double wheelRadius, Units.Distance radiusUnits) { + protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info, Direction direction) { super(controller, portNumber, direction); enableEncoder(); - calculateRadius(wheelRadius, radiusUnits); - } - - private void calculateRadius(double wheelRadius, Units.Distance radiusUnit) { - this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); + this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } /** 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..e93af14 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java @@ -0,0 +1,33 @@ +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 wheelRadius, Units.Distance radiusUnits) { + this.wheelRadius = radiusUnits.convertTo(Units.Distance.METERS, wheelRadius); + this.mechanicalAdvantage = 1; + } + + public MotorInfo(double wheelRadius, Units.Distance radiusUnits, double mechanicalAdvantage) { + this.wheelRadius = radiusUnits.convertTo(Units.Distance.METERS, wheelRadius); + this.mechanicalAdvantage = mechanicalAdvantage; + } + + 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/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java index 50748d6..2eb7125 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -3,6 +3,7 @@ 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.nav.PID; import com.lasarobotics.library.util.MathUtil; import com.lasarobotics.library.util.Units; @@ -13,6 +14,8 @@ public class EncoderTest extends OpMode { private static final double WHEEL_RADIUS = 2; private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; + DcMotor frontLeft, frontRight, backRight; EncodedMotor backLeft; Controller one; @@ -24,7 +27,7 @@ public void init() { frontLeft = hardwareMap.dcMotor.get("frontLeft"); frontRight = hardwareMap.dcMotor.get("frontRight"); backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), - WHEEL_RADIUS, WHEEL_RADIUS_UNIT); //set wheel radius for distance calculations + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); //set wheel radius for distance calculations backRight = hardwareMap.dcMotor.get("backRight"); one = new Controller(gamepad1); @@ -36,7 +39,8 @@ public void init() { //Create PID looper pidBackLeft = new PID(); - pidBackLeft.setSetpoint(1000); + pidBackLeft.setSetpoint(Units.Distance.convertToAngle(1, WHEEL_RADIUS, WHEEL_RADIUS_UNIT, + Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); } public void init_loop() { From b3d65c9f7e60d06a0001b42bb38c5bf54cc6795d Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 10 Jan 2016 21:56:16 -0600 Subject: [PATCH 21/43] Start testing... potential problem with part of new PID functionality --- .../library/drive/DriveSpecification.java | 2 +- .../lasarobotics/library/nav/MotorInfo.java | 2 +- .../com/lasarobotics/library/nav/PID.java | 31 ++++- .../lasarobotics/library/util/MathUtil.java | 48 +++++++ .../opmodes/EncoderTest.java | 126 ++++++++++++++++-- .../opmodes/navx/NavXDriveForwardPID.java | 9 +- .../opmodes/navx/NavXRotatePID.java | 9 +- 7 files changed, 208 insertions(+), 19 deletions(-) diff --git a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java index 9413a8b..606611c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java @@ -14,7 +14,7 @@ public class DriveSpecification { * * @param drive Current drive train * @param wheelRadius Wheel radius - * @param radiusUnit Unit of whell radius + * @param radiusUnit Unit of wheel radius */ DriveSpecification(Drivetrain drive, double wheelRadius, Units.Distance radiusUnit) { this.drive = drive; 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 index e93af14..1798168 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java @@ -28,6 +28,6 @@ public double getMechanicalAdvantage() { } public double getEffectiveWheelRadius(Units.Distance unit) { - return getWheelRadius(unit) * mechanicalAdvantage; + 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 index 79921d6..3740e93 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -1,5 +1,7 @@ package com.lasarobotics.library.nav; +import com.lasarobotics.library.util.MathUtil; + /** * PID Targeting */ @@ -13,6 +15,10 @@ public class PID { protected double Kp = 0.2, Ki = 0.01, Kd = 1; // PID constant multipliers protected double dt = 100.0; // delta time protected double output = 0; // the drive amount that effects the PV. + protected double outputLast = 0; // last output power + protected double maxChange = 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 public PID() { this.Kp = 0.005; @@ -91,6 +97,19 @@ public double getOutputValue() { return output; } + public void setOutputRange(double min, double max) { + this.minOutput = min; + this.maxOutput = max; + } + + public double getMaxAcceleration() { + return maxChange; + } + + public void setMaxAcceleration(double maxChange) { + this.maxChange = maxChange; + } + private void update() { // calculate the difference between the desired value and the actual value error = setpoint - processValue; @@ -103,7 +122,17 @@ private void update() { // desired setpoint. output = (Kp * error) + (Ki * integral) + (Kd * derivative); - // remember the error for the next time around. + //Clamp output + output = MathUtil.coerce(minOutput, maxOutput, output); + + //Make sure the system does not move too fast + if ((output - outputLast) > maxChange) + output = outputLast + (maxChange); + else if ((outputLast - output) > maxChange) + output = outputLast - (maxChange); + + // remember the error and output for the next time around. previousError = error; + outputLast = output; } } 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..74549da 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,52 @@ 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; + } } 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 2eb7125..7e216be 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,21 +1,44 @@ package com.qualcomm.ftcrobotcontroller.opmodes; +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.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.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 { private static final double WHEEL_RADIUS = 2; private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; + 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.05; + private static final double NAVX_YAW_PID_I = 0.0; + private static final double NAVX_YAW_PID_D = 0.0; + + private static final double DISTANCE_FEET = 1; //distance in feet + + private static final DecimalFormat df = new DecimalFormat("#.##"); + + NavXDevice navx; + NavXPIDController yawPIDController; + NavXPIDController.PIDState yawPIDState; + DcMotor frontLeft, frontRight, backRight; EncodedMotor backLeft; Controller one; @@ -30,43 +53,130 @@ public void init() { new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); //set wheel radius for distance calculations backRight = hardwareMap.dcMotor.get("backRight"); + 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 - backLeft.setTargetPosition(1, Units.Distance.FEET); + backLeft.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); backLeft.reset(); //Create PID looper pidBackLeft = new PID(); - pidBackLeft.setSetpoint(Units.Distance.convertToAngle(1, WHEEL_RADIUS, WHEEL_RADIUS_UNIT, - Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); + pidBackLeft.setSetpoint(Units.Distance.convertToAngle(DISTANCE_FEET, + WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, + WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); + pidBackLeft.setMaxAcceleration(0.02); + + //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(0.02, MathUtil.coerce(-1, 1, power)); + } + public void loop() { long time = System.nanoTime(); double timeDelta = (time - lastTime) / 1000000000.0; one.update(gamepad1); backLeft.update(); pidBackLeft.addMeasurement(Math.abs(backLeft.getCurrentPosition()), timeDelta); - double power = pidBackLeft.getOutputValue(); - Tank.motor4(frontLeft, frontRight, backLeft, backRight, - MathUtil.coerce(-1, 1, power), MathUtil.coerce(-1, 1, -power)); + double power = pidBackLeft.getOutputValue(); + double powerCompensation = 0; + + navx.displayTelemetry(telemetry); + if (yawPIDController.isUpdateAvailable(yawPIDState)) { + if (!yawPIDState.isOnTarget()) { + powerCompensation = yawPIDState.getOutput(); + } + } + + double left = 0; + double right = 0; + + /*if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + backLeft.setPowerFloat(); + backRight.setPowerFloat(); + } + else + {*/ + telemetry.addData("Original Power: ", power); + + /*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 = MathUtil.coerce(-1, 1, 0.5 + power); + right = MathUtil.coerce(-1, 1, 0.5 - power); + + //double[] result = MathUtil.normalize(left, right, 1, true); + //double powerFactor = power >= 1 ? 1 : MathUtil.coerce(-1, 1, Math.abs(power)); + //double powerFactor = 1; + //left = coerce(result[0] * powerFactor); + //right = coerce(result[1] * powerFactor); + //left = coerce(left); + //right = coerce(right); + //} + + Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); telemetry.addData("Back Left (counts): ", backLeft.getCurrentPosition()); telemetry.addData("Back Left (feet): ", backLeft.getCurrentPosition(Units.Distance.FEET)); 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/navx/NavXDriveForwardPID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java index b727cad..ddd0ae4 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXDriveForwardPID.java @@ -3,6 +3,7 @@ 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; @@ -40,10 +41,10 @@ public class NavXDriveForwardPID extends OpMode implements NavXDataReceiver { public void init() { //Create motors WITH ENCODERS (highly preferable) - frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + 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); 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 index 17a4cf3..3969127 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -4,6 +4,7 @@ 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; @@ -40,10 +41,10 @@ public class NavXRotatePID extends OpMode implements NavXDataReceiver { public void init() { //Create motors WITH ENCODERS (highly preferable) - frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); - backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), WHEEL_RADIUS, WHEEL_RADIUS_UNIT); + 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); From 7d41b2376cee8e0b57e4d3dd8f8afb7b9d4422f1 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Mon, 11 Jan 2016 13:46:43 -0600 Subject: [PATCH 22/43] Attempt better Encoder Test code --- .../opmodes/EncoderTest.java | 41 +++++++++++++------ .../opmodes/navx/NavXRotatePID.java | 2 +- 2 files changed, 29 insertions(+), 14 deletions(-) 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 7e216be..81177f4 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,6 +1,5 @@ package com.qualcomm.ftcrobotcontroller.opmodes; -import com.kauailabs.navx.ftc.navXPIDController; import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; import com.lasarobotics.library.nav.EncodedMotor; @@ -25,8 +24,8 @@ public class EncoderTest 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_TOLERANCE_DEGREES = 10.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.05; private static final double NAVX_YAW_PID_I = 0.0; private static final double NAVX_YAW_PID_D = 0.0; @@ -53,6 +52,9 @@ public void init() { new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); //set wheel radius for distance calculations backRight = hardwareMap.dcMotor.get("backRight"); + backLeft.setDirection(DcMotor.Direction.FORWARD); + frontLeft.setDirection(DcMotor.Direction.FORWARD); + backRight.setDirection(DcMotor.Direction.REVERSE); frontRight.setDirection(DcMotor.Direction.REVERSE); @@ -84,13 +86,13 @@ public void init() { //Allow crossing over the bounds (see setContinuous() documentation) yawPIDController.setContinuous(true); //Set angle tolerance - //yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + 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); + //yawPIDController.setTolerance(navXPIDController.ToleranceType.NONE, 0); //Start data collection yawPIDController.start(); @@ -123,9 +125,18 @@ public void loop() { } } + //Divide test into 3 segments + //1 Rotate to angle + //2 Move and rotate to angle + //3 Rotate to angle + + int phase = 0; + double left = 0; double right = 0; + + /*if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { frontLeft.setPowerFloat(); frontRight.setPowerFloat(); @@ -136,21 +147,25 @@ public void loop() { {*/ telemetry.addData("Original Power: ", power); - /*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 = 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); + + //Only problem is if power = 0 + + //Take the directional power compensation and multiply it by the power - left = MathUtil.coerce(-1, 1, 0.5 + power); - right = MathUtil.coerce(-1, 1, 0.5 - power); + //left = MathUtil.coerce(-1, 1, -1 + powerCompensation); + //right = MathUtil.coerce(-1, 1, 1 + powerCompensation); //double[] result = MathUtil.normalize(left, right, 1, true); //double powerFactor = power >= 1 ? 1 : MathUtil.coerce(-1, 1, Math.abs(power)); //double powerFactor = 1; //left = coerce(result[0] * powerFactor); //right = coerce(result[1] * powerFactor); - //left = coerce(left); - //right = coerce(right); + left = coerce(left); + right = coerce(right); //} Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); 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 index 3969127..09433b5 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/NavXRotatePID.java @@ -96,7 +96,7 @@ public void loop() { } else { double power = yawPIDState.getOutput(); Tank.motor4(frontLeft, frontRight, backLeft, backRight, - power, power); + power, -power); telemetry.addData("Motor Power", df.format(power)); } } From 1573c177bedc9745ddae1956b0b2b7076e025584 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Mon, 11 Jan 2016 13:59:43 -0600 Subject: [PATCH 23/43] Start update --- .../opmodes/EncoderTest.java | 50 +++++++++++++++---- 1 file changed, 41 insertions(+), 9 deletions(-) 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 81177f4..3fa8496 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -1,5 +1,6 @@ package com.qualcomm.ftcrobotcontroller.opmodes; +import com.kauailabs.navx.ftc.navXPIDController; import com.lasarobotics.library.controller.Controller; import com.lasarobotics.library.drive.Tank; import com.lasarobotics.library.nav.EncodedMotor; @@ -44,6 +45,7 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { PID pidBackLeft; long lastTime = 0; + int phase = 0; public void init() { frontLeft = hardwareMap.dcMotor.get("frontLeft"); @@ -86,13 +88,13 @@ public void init() { //Allow crossing over the bounds (see setContinuous() documentation) yawPIDController.setContinuous(true); //Set angle tolerance - yawPIDController.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, NAVX_TOLERANCE_DEGREES); + //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); + yawPIDController.setTolerance(navXPIDController.ToleranceType.NONE, 0); //Start data collection yawPIDController.start(); @@ -130,12 +132,46 @@ public void loop() { //2 Move and rotate to angle //3 Rotate to angle - int phase = 0; - double left = 0; double right = 0; - + switch (phase) { + case 0: //rotate to angle + case 2: + left = coerce(powerCompensation); + right = coerce(powerCompensation); + + 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(); + return; + } /*if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { frontLeft.setPowerFloat(); @@ -147,10 +183,6 @@ public void loop() { {*/ telemetry.addData("Original Power: ", power); - 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); //Only problem is if power = 0 From 003eaec98ef4b91c5b230a32cb2478d5e60df64e Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 15 Jan 2016 18:46:48 -0600 Subject: [PATCH 24/43] Updates --- .../library/nav/EncodedMotor.java | 3 + .../com/lasarobotics/library/nav/PID.java | 12 +- .../opmodes/EncoderTest.java | 106 +++++++++++++----- 3 files changed, 91 insertions(+), 30 deletions(-) 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 index 88ef0ac..38baad1 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -22,18 +22,21 @@ public class EncodedMotor extends DcMotor { public EncodedMotor(DcMotor motor, MotorInfo info) { super(motor.getController(), motor.getPortNumber(), motor.getDirection()); enableEncoder(); + reset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info) { super(controller, portNumber); enableEncoder(); + reset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info, Direction direction) { super(controller, portNumber, direction); enableEncoder(); + reset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } 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 index 3740e93..5538822 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -26,6 +26,10 @@ public PID() { this.Kd = 0; } public PID(double p, double i, double d) { + setCoefficients(p, i, d); + } + + public void setCoefficients(double p, double i, double d) { this.Kp = p; this.Ki = i; this.Kd = d; @@ -126,10 +130,10 @@ private void update() { output = MathUtil.coerce(minOutput, maxOutput, output); //Make sure the system does not move too fast - if ((output - outputLast) > maxChange) - output = outputLast + (maxChange); - else if ((outputLast - output) > maxChange) - output = outputLast - (maxChange); + if ((output - outputLast) > maxChange * dt) + output = outputLast + (maxChange * dt); + else if ((outputLast - output) > maxChange * dt) + output = outputLast - (maxChange * dt); // remember the error and output for the next time around. previousError = error; 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 3fa8496..9df9215 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -20,18 +20,24 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { private static final double WHEEL_RADIUS = 2; private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; - private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 1; 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 = 10.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_TARGET_ANGLE_DEGREES = 0.0; //target angle for PID private static final double NAVX_YAW_PID_P = 0.05; - private static final double NAVX_YAW_PID_I = 0.0; - private static final double NAVX_YAW_PID_D = 0.0; + private static final double NAVX_YAW_PID_I = 0.01; + private static final double NAVX_YAW_PID_D = 0.005; - private static final double DISTANCE_FEET = 1; //distance in feet + private static final double PID_P = 0.05; + private static final double PID_I = 0.01; + private static final double PID_D = 0.005; + private static final double PID_MAX_ACCEL = 100000; + + private static final double DISTANCE_FEET = 10; //distance in feet + private static final double MIN_POWER = 0; //distance in feet private static final DecimalFormat df = new DecimalFormat("#.##"); @@ -39,24 +45,26 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { NavXPIDController yawPIDController; NavXPIDController.PIDState yawPIDState; - DcMotor frontLeft, frontRight, backRight; - EncodedMotor backLeft; + EncodedMotor frontLeft, frontRight, backLeft, backRight; Controller one; - PID pidBackLeft; + PID pidLeft; + PID pidRight; long lastTime = 0; int phase = 0; public void init() { - frontLeft = hardwareMap.dcMotor.get("frontLeft"); - frontRight = hardwareMap.dcMotor.get("frontRight"); + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), + new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); //set wheel radius for distance calculations - backRight = hardwareMap.dcMotor.get("backRight"); + backRight = new EncodedMotor(hardwareMap.dcMotor.get("backRight"), + 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); @@ -65,14 +73,24 @@ public void init() { //TODO these are all currently ASYNCHRONOUS //TODO meaning that they can only really be called from init() or a state machine backLeft.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); - backLeft.reset(); + backRight.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + frontLeft.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); + frontRight.setTargetPosition(DISTANCE_FEET, Units.Distance.FEET); //Create PID looper - pidBackLeft = new PID(); - pidBackLeft.setSetpoint(Units.Distance.convertToAngle(DISTANCE_FEET, + pidLeft = new PID(); + pidLeft.setSetpoint(Units.Distance.convertToAngle(DISTANCE_FEET, + WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, + WHEEL_RADIUS_UNIT, Units.Distance.FEET, 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_FEET, WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); - pidBackLeft.setMaxAcceleration(0.02); + pidRight.setMaxAcceleration(PID_MAX_ACCEL); + pidRight.setCoefficients(PID_P, PID_I, PID_D); //Initialize the navX controller //Make sure to implement NavXDataReceiver @@ -107,7 +125,7 @@ public void init_loop() { } private double coerce(double power) { - return MathUtil.deadband(0.02, MathUtil.coerce(-1, 1, power)); + return MathUtil.deadband(MIN_POWER, MathUtil.coerce(-1, 1, power)); } public void loop() { @@ -115,9 +133,19 @@ public void loop() { double timeDelta = (time - lastTime) / 1000000000.0; one.update(gamepad1); backLeft.update(); - pidBackLeft.addMeasurement(Math.abs(backLeft.getCurrentPosition()), timeDelta); + backRight.update(); + frontLeft.update(); + frontRight.update(); - double power = pidBackLeft.getOutputValue(); + double leftPos = Math.abs(backLeft.getCurrentPosition()) + + Math.abs(frontLeft.getCurrentPosition()); + double rightPos = Math.abs(backRight.getCurrentPosition()) + + Math.abs(frontRight.getCurrentPosition()); + + pidLeft.addMeasurement(leftPos, timeDelta); + pidRight.addMeasurement(rightPos, timeDelta); + + double power = (pidLeft.getOutputValue() + pidRight.getOutputValue()) / 2; double powerCompensation = 0; navx.displayTelemetry(telemetry); @@ -135,10 +163,26 @@ public void loop() { double left = 0; double right = 0; - switch (phase) { + //x(t) = x(0) + + + /*if (!backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { + 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); + } + else + { + left = 0; + right = 0; + }*/ + + /*switch (phase) { case 0: //rotate to angle case 2: - left = coerce(powerCompensation); + left = coerce(-powerCompensation); right = coerce(powerCompensation); if (MathUtil.equal(powerCompensation, 0)) { @@ -170,8 +214,7 @@ public void loop() { frontRight.setPowerFloat(); backLeft.setPowerFloat(); backRight.setPowerFloat(); - return; - } + }*/ /*if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { frontLeft.setPowerFloat(); @@ -196,14 +239,25 @@ public void loop() { //double powerFactor = 1; //left = coerce(result[0] * powerFactor); //right = coerce(result[1] * powerFactor); + + + //left = power; + //right = power; + + 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 (phase != 3) Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); - telemetry.addData("Back Left (counts): ", backLeft.getCurrentPosition()); - telemetry.addData("Back Left (feet): ", backLeft.getCurrentPosition(Units.Distance.FEET)); + 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); From 901f7bcb9f4127bb93fd6581c6bac3a0b8d76602 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 21 Jan 2016 14:50:51 -0600 Subject: [PATCH 25/43] Pre-smart-turning update --- .../opmodes/EncoderTest.java | 61 +---- .../opmodes/FtcOpModeRegister.java | 30 +-- .../opmodes/NavXAutoTest.java | 245 ++++++++++++++++++ 3 files changed, 268 insertions(+), 68 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java 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 9df9215..0362f10 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -27,16 +27,16 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { private static final double NAVX_TOLERANCE_DEGREES = 10.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.05; - private static final double NAVX_YAW_PID_I = 0.01; - private static final double NAVX_YAW_PID_D = 0.005; + private static final double NAVX_YAW_PID_P = 0.005; + private static final double NAVX_YAW_PID_I = 0; + private static final double NAVX_YAW_PID_D = 0; private static final double PID_P = 0.05; private static final double PID_I = 0.01; private static final double PID_D = 0.005; - private static final double PID_MAX_ACCEL = 100000; + private static final double PID_MAX_ACCEL = 1; - private static final double DISTANCE_FEET = 10; //distance in feet + private static final double DISTANCE_FEET = 5; //distance in feet private static final double MIN_POWER = 0; //distance in feet private static final DecimalFormat df = new DecimalFormat("#.##"); @@ -163,23 +163,8 @@ public void loop() { double left = 0; double right = 0; - //x(t) = x(0) + - /*if (!backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { - 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); - } - else - { - left = 0; - right = 0; - }*/ - - /*switch (phase) { + switch (phase) { case 0: //rotate to angle case 2: left = coerce(-powerCompensation); @@ -214,40 +199,14 @@ public void loop() { frontRight.setPowerFloat(); backLeft.setPowerFloat(); backRight.setPowerFloat(); - }*/ - - /*if (backLeft.hasReachedPosition(DISTANCE_FEET, Units.Distance.FEET)) { - frontLeft.setPowerFloat(); - frontRight.setPowerFloat(); - backLeft.setPowerFloat(); - backRight.setPowerFloat(); } - else - {*/ telemetry.addData("Original Power: ", power); - //Only problem is if power = 0 - - //Take the directional power compensation and multiply it by the power - - //left = MathUtil.coerce(-1, 1, -1 + powerCompensation); - //right = MathUtil.coerce(-1, 1, 1 + powerCompensation); - - //double[] result = MathUtil.normalize(left, right, 1, true); - //double powerFactor = power >= 1 ? 1 : MathUtil.coerce(-1, 1, Math.abs(power)); - //double powerFactor = 1; - //left = coerce(result[0] * powerFactor); - //right = coerce(result[1] * powerFactor); - - - //left = power; - //right = power; - - 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 = MathUtil.coerce(-1, 1, power) + +// Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); +// right = MathUtil.coerce(-1, 1, power) - +// Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); left = coerce(left); right = coerce(right); 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 d1c2ffc..61fe613 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -31,12 +31,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE package com.qualcomm.ftcrobotcontroller.opmodes; -import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXDriveForwardPID; -import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXNavigationTest; -import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXRotatePID; import com.qualcomm.ftcrobotcontroller.opmodes.navx.NavXSensorTest; -import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXDriveStraightPIDLoopOp; -import com.qualcomm.ftcrobotcontroller.opmodes.navx.kauailabs.navXProcessedOp; import com.qualcomm.robotcore.eventloop.opmode.OpModeManager; import com.qualcomm.robotcore.eventloop.opmode.OpModeRegister; @@ -54,19 +49,20 @@ public class FtcOpModeRegister implements OpModeRegister { public void register(OpModeManager manager) { //Custom op modes manager.register("Null Op", NullOp.class); - manager.register("Mecanum Prototype", MecanumPrototypeTeleop.class); - manager.register("MonkeyC Do", MonkeyCDo.class); - manager.register("MonkeyC Write", MonkeyCWrite.class); - manager.register("Logging Sample", LoggingSample.class); - manager.register("Options Sample", OptionsSample.class); - manager.register("Sensors Test", SensorsTest.class); - manager.register("Encoder Test", EncoderTest.class); - manager.register("NavX Official Test", navXProcessedOp.class); - manager.register("NavX Official Forward PID Test", navXDriveStraightPIDLoopOp.class); + //manager.register("Mecanum Prototype", MecanumPrototypeTeleop.class); + //manager.register("MonkeyC Do", MonkeyCDo.class); + //manager.register("MonkeyC Write", MonkeyCWrite.class); + //manager.register("Logging Sample", LoggingSample.class); + //manager.register("Options Sample", OptionsSample.class); + //manager.register("Sensors Test", SensorsTest.class); + //manager.register("Encoder Test", EncoderTest.class); + //manager.register("NavX Official Test", navXProcessedOp.class); + //manager.register("NavX Official Forward PID Test", navXDriveStraightPIDLoopOp.class); manager.register("NavX Sensor Test", NavXSensorTest.class); - manager.register("NavX Rotate PID Test", NavXRotatePID.class); - manager.register("NavX Forward PID Test", NavXDriveForwardPID.class); - manager.register("NavX Navigation Test", NavXNavigationTest.class); + //manager.register("NavX Rotate PID Test", NavXRotatePID.class); + //manager.register("NavX Forward PID Test", NavXDriveForwardPID.class); + //manager.register("NavX Navigation Test", NavXNavigationTest.class); + manager.register("NavX Auto Test", NavXAutoTest.class); manager.register("Teleop 5998", Teleop5998.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..43ad2cb --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java @@ -0,0 +1,245 @@ +/* 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.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +import com.lasarobotics.library.util.MathUtil; +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 = 0.1; + private final double MIN_MOTOR_OUTPUT_VALUE = -.5; + private final double MAX_MOTOR_OUTPUT_VALUE = .5; + private final double YAW_PID_P = 0.1; + private final double YAW_PID_I = 0; + private final double YAW_PID_D = 12; + DcMotor frontLeft, frontRight, backLeft, backRight, intake; + int DEVICE_TIMEOUT_MS = 500; + Timers mTimers = new Timers(); + private NavXDevice navx; + 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 { + 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(90, 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) { + 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(); //reset 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(); + DecimalFormat df = new DecimalFormat("#.##"); + + try { + while (backLeft.getCurrentPosition() < encoderCounts && + !Thread.currentThread().isInterrupted()) { + 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); + telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + + df.format(drive_speed)); + } else { + double output = yawPIDResult.getOutput(); + backLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - output)); + frontLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - output)); + backRight.setPower(MathUtil.coerce(-1, 1, drive_speed + output)); + frontRight.setPower(MathUtil.coerce(-1, 1, drive_speed + output)); + telemetry.addData("PIDOutput", df.format(limit(drive_speed + output)) + ", " + + df.format(limit(drive_speed - output))); + } + 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 { + blockForMs(500); + Log.d("navx", "started turn for " + deg); + while (Math.abs(navx.getRotation().x - deg) > TOLERANCE_DEGREES) { + Log.d("navx", "current yaw " + navx.getRotation().x); + if (deg > 0) { + frontLeft.setPower(-power); + backLeft.setPower(-power); + frontRight.setPower(power); + backRight.setPower(power); + } else { + frontLeft.setPower(power); + backLeft.setPower(power); + frontRight.setPower(-power); + backRight.setPower(-power); + } + } + Log.d("navx", "ended turn for " + deg); + frontLeft.setPower(0); + frontRight.setPower(0); + backRight.setPower(0); + backLeft.setPower(0); + blockForMs(500); + } +// 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); +// } +} From 4e670caf49c0bb315f65b8927907084c334b9cf1 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 21 Jan 2016 15:38:30 -0600 Subject: [PATCH 26/43] Complete VERY ACCURATE rotation --- .../lasarobotics/library/util/MathUtil.java | 28 ++++++++ .../opmodes/NavXAutoTest.java | 71 ++++++++++++------- 2 files changed, 75 insertions(+), 24 deletions(-) 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 74549da..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 @@ -136,4 +136,32 @@ public static double[] normalize(double[] vals, double normalizeTo, boolean norm 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/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java index 43ad2cb..791bb25 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java @@ -33,9 +33,11 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 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; @@ -65,20 +67,23 @@ public class NavXAutoTest extends LinearOpMode { /* 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 = 0.1; - private final double MIN_MOTOR_OUTPUT_VALUE = -.5; - private final double MAX_MOTOR_OUTPUT_VALUE = .5; - private final double YAW_PID_P = 0.1; + 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 = 12; + 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 Math.min(Math.max(a, MIN_MOTOR_OUTPUT_VALUE), MAX_MOTOR_OUTPUT_VALUE); + return MathUtil.deadband(MIN_DRIVE_POWER, MathUtil.coerce(MIN_MOTOR_OUTPUT_VALUE, MAX_MOTOR_OUTPUT_VALUE, a)); } @Override @@ -96,9 +101,9 @@ public void runOpMode() throws InterruptedException { waitForStart(); //driveForEncoderCounts(4500, .5); - turnToDeg(90, 1); - //blockForMs(500); - //driveForEncoderCounts(3900, .5); + turnToDeg(40, 1); + blockForMs(500); + driveForEncoderCounts(3900, .5); //turnToDeg(90); //blockForMs(500); //driveForEncoderCounts(500, .5); @@ -111,7 +116,7 @@ public void runOpMode() throws InterruptedException { private void blockForMs(int ms) throws InterruptedException { mTimers.startClock("delay"); - while (mTimers.getClockValue("delay") < ms) { + while (mTimers.getClockValue("delay") < ms && opModeIsActive()) { waitOneFullHardwareCycle(); } waitOneFullHardwareCycle(); @@ -136,11 +141,10 @@ private void driveForEncoderCounts(int encoderCounts, double drive_speed) throws yawPIDController.setPID(YAW_PID_P, YAW_PID_I, YAW_PID_D); yawPIDController.enable(true); navXPIDController.PIDResult yawPIDResult = new navXPIDController.PIDResult(); - DecimalFormat df = new DecimalFormat("#.##"); try { while (backLeft.getCurrentPosition() < encoderCounts && - !Thread.currentThread().isInterrupted()) { + !Thread.currentThread().isInterrupted() && opModeIsActive()) { telemetry.addData("Encoder", backLeft.getCurrentPosition()); if (yawPIDController.waitForNewUpdate(yawPIDResult, DEVICE_TIMEOUT_MS)) { if (yawPIDResult.isOnTarget()) { @@ -177,20 +181,40 @@ private void driveForEncoderCounts(int encoderCounts, double drive_speed) throws } private void turnToDeg(int deg, double power) throws InterruptedException { - blockForMs(500); Log.d("navx", "started turn for " + deg); - while (Math.abs(navx.getRotation().x - deg) > TOLERANCE_DEGREES) { + + NavXPIDController yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); + navx.reset(); //reset 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 (deg > 0) { - frontLeft.setPower(-power); - backLeft.setPower(-power); - frontRight.setPower(power); - backRight.setPower(power); + + 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 { - frontLeft.setPower(power); - backLeft.setPower(power); - frontRight.setPower(-power); - backRight.setPower(-power); + /* A timeout occurred */ + Log.w("navXDriveStraightOp", "Yaw PID waitForNewUpdate() TIMEOUT."); } } Log.d("navx", "ended turn for " + deg); @@ -198,7 +222,6 @@ private void turnToDeg(int deg, double power) throws InterruptedException { frontRight.setPower(0); backRight.setPower(0); backLeft.setPower(0); - blockForMs(500); } // private void turnToDeg(int deg) throws InterruptedException { // Log.d("navx","started turn for " +deg); From 191f8417c08edef89c0095fa9d2c778d791f9192 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 21 Jan 2016 16:25:56 -0600 Subject: [PATCH 27/43] Upload directional motion upgrade --- .../com/lasarobotics/library/nav/PID.java | 12 ++++--- .../opmodes/EncoderTest.java | 36 +++++++++++-------- .../opmodes/FtcOpModeRegister.java | 2 +- .../opmodes/NavXAutoTest.java | 14 +++----- 4 files changed, 36 insertions(+), 28 deletions(-) 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 index 5538822..7f5bd11 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -35,16 +35,20 @@ public void setCoefficients(double p, double i, double d) { this.Kd = d; } - public void setSetpoint(double setpoint) { - this.setpoint = setpoint; - } - 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; + } + /** * Gets the most recent error - the difference between the setpoint and the measured value * 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 0362f10..a870d59 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -164,7 +164,7 @@ public void loop() { double right = 0; - switch (phase) { + /*switch (phase) { case 0: //rotate to angle case 2: left = coerce(-powerCompensation); @@ -199,21 +199,29 @@ public void loop() { frontRight.setPowerFloat(); backLeft.setPowerFloat(); backRight.setPowerFloat(); - } + }*/ telemetry.addData("Original Power: ", power); - -// left = MathUtil.coerce(-1, 1, power) + -// Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); -// right = MathUtil.coerce(-1, 1, power) - -// Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); - - left = coerce(left); - right = coerce(right); - - - //if (phase != 3) - Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); + double avgDist = Math.abs(pidLeft.getOutputValue() - pidLeft.getSetpoint()) + Math.abs(pidRight.getOutputValue() - pidRight.getSetpoint()); + avgDist /= 2; + if (avgDist < 500) { + backLeft.setPowerFloat(); + backRight.setPowerFloat(); + frontLeft.setPowerFloat(); + frontRight.setPowerFloat(); + } else { + + left = MathUtil.coerce(-1, 1, power) + + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); + right = MathUtil.coerce(-1, 1, power) - + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * 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); 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 61fe613..9e8adc4 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -55,7 +55,7 @@ public void register(OpModeManager manager) { //manager.register("Logging Sample", LoggingSample.class); //manager.register("Options Sample", OptionsSample.class); //manager.register("Sensors Test", SensorsTest.class); - //manager.register("Encoder Test", EncoderTest.class); + manager.register("NavX Encoder Test", EncoderTest.class); //manager.register("NavX Official Test", navXProcessedOp.class); //manager.register("NavX Official Forward PID Test", navXDriveStraightPIDLoopOp.class); manager.register("NavX Sensor Test", NavXSensorTest.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 index 791bb25..3694d5d 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java @@ -103,7 +103,7 @@ public void runOpMode() throws InterruptedException { //driveForEncoderCounts(4500, .5); turnToDeg(40, 1); blockForMs(500); - driveForEncoderCounts(3900, .5); + driveForEncoderCounts(3900, -.5); //turnToDeg(90); //blockForMs(500); //driveForEncoderCounts(500, .5); @@ -152,16 +152,12 @@ private void driveForEncoderCounts(int encoderCounts, double drive_speed) throws frontRight.setPower(drive_speed); backLeft.setPower(drive_speed); backRight.setPower(drive_speed); - telemetry.addData("PIDOutput", df.format(drive_speed) + ", " + - df.format(drive_speed)); } else { double output = yawPIDResult.getOutput(); - backLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - output)); - frontLeft.setPower(MathUtil.coerce(-1, 1, drive_speed - output)); - backRight.setPower(MathUtil.coerce(-1, 1, drive_speed + output)); - frontRight.setPower(MathUtil.coerce(-1, 1, drive_speed + output)); - telemetry.addData("PIDOutput", df.format(limit(drive_speed + output)) + ", " + - df.format(limit(drive_speed - output))); + 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 { From 82a16ef19ccd87f44eba4b4947f64cfaf2e0ef50 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 13 Feb 2016 11:18:23 -0600 Subject: [PATCH 28/43] Add encoder test --- .../com/lasarobotics/library/util/Log.java | 20 +++ .../opmodes/BasicEncoderTest.java | 138 ++++++++++++++++++ 2 files changed, 158 insertions(+) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java 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..e8fc146 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 *

    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..7bb9d0f --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java @@ -0,0 +1,138 @@ +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 = 2; + private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 1; + + private static final double PID_P = 0.050; //0.05 + private static final double PID_I = 0.000; //0.01 + private static final double PID_D = 0.000; //0.005 + private static final double DISTANCE = 1; //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() { + switch (phase) { + case 0: + long time = System.nanoTime(); + double timeDelta = (time - lastTime) / 1000000000.0; + + double leftPos = Math.abs(backLeft.getCurrentPosition()) + + Math.abs(frontLeft.getCurrentPosition()); + double rightPos = Math.abs(backRight.getCurrentPosition()) + + Math.abs(frontRight.getCurrentPosition()); + + telemetry.addData("Pos", leftPos + ", " + rightPos); + + pidLeft.addMeasurement(leftPos, timeDelta); + pidRight.addMeasurement(rightPos, timeDelta); + + telemetry.addData("PID", pidLeft.getOutputValue() + ", " + pidRight.getOutputValue()); + + 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() { + log.saveAs(Log.FileType.CSV); + } +} From b932e8cf8021716b03d456b0a6daadc8f4d4fc8d Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sat, 13 Feb 2016 13:11:59 -0600 Subject: [PATCH 29/43] Add a test teleop --- .../opmodes/FtcOpModeRegister.java | 2 + .../opmodes/Teleop5998.java | 116 +++++++++++------- .../opmodes/first/K9AutoTime.java | 2 +- 3 files changed, 73 insertions(+), 47 deletions(-) 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 8b1caf3..dcef865 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,8 @@ public class FtcOpModeRegister implements OpModeRegister { public void register(OpModeManager manager) { //Custom op modes manager.register("Null", NullOp.class); + manager.register("Teleop 5998", Teleop5998.class); + //manager.register("Basic Encoder Test", BasicEncoderTest.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/Teleop5998.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java index 3d28b4d..5554bf2 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java @@ -3,18 +3,21 @@ 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; + Servo slide, dump, carabiner, climber; Controller firstController; Controller secondController; DcMotor goalOne, goalTwo; + double modfier = 0.25; LiftStatus status; Timers robotTimer; @@ -29,23 +32,37 @@ public void init() { 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"); + 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(.53); - //dump.setPosition(.53); + 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); - Tank.motor4(frontLeft, frontRight, backLeft, backRight, -firstController.left_stick_y, - firstController.right_stick_y); + //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); - if (firstController.right_bumper == ButtonState.PRESSED) { + //Hanging + if (firstController.right_bumper == ButtonState.HELD) { goalOne.setPower(1); } else if (firstController.right_trigger > 0.2) { goalOne.setPower(-1); @@ -53,7 +70,7 @@ public void loop() { goalOne.setPower(0); } - if (firstController.left_bumper == ButtonState.PRESSED) { + if (firstController.left_bumper == ButtonState.HELD) { goalTwo.setPower(1); } else if (firstController.left_trigger > 0.2) { goalTwo.setPower(-1); @@ -61,58 +78,65 @@ public void loop() { goalTwo.setPower(0); } - if (secondController.dpad_up == ButtonState.PRESSED) { + 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.dpad_down == ButtonState.PRESSED) { + } else if (secondController.a == ButtonState.PRESSED) { intake.setPower(-1); - } else if (secondController.dpad_left == ButtonState.PRESSED) { - intake.setPower(0); - } else if (secondController.dpad_right == ButtonState.PRESSED) { + } else if (secondController.b == ButtonState.PRESSED) { intake.setPower(0); } - if (secondController.left_bumper == ButtonState.HELD) { - lift.setPower(.3); - } else if (secondController.right_bumper == ButtonState.HELD) { - lift.setPower(-.3); + //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.x == ButtonState.HELD) { - status = LiftStatus.LEFT; - robotTimer.startClock("dumpTimer"); - slide.setPosition(1); - } else if (secondController.b == ButtonState.HELD) { - status = LiftStatus.RIGHT; - robotTimer.startClock("dumpTimer"); - slide.setPosition(0); - } else if (secondController.a == ButtonState.HELD) { - status = LiftStatus.CENTER; - slide.setPosition(0.5); - dump.setPosition(0.5); + 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 ((status == LiftStatus.LEFT) && (robotTimer.getClockValue("dumpTimer") >= 100)) { - dump.setPosition(0); - } else if ((status == LiftStatus.RIGHT) && (robotTimer.getClockValue("dumpTimer") >= 100)) { - dump.setPosition(1); - }*/ - + 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 - intake.setPower(0); - frontLeft.setPower(0); - frontRight.setPower(0); - backLeft.setPower(0); - backRight.setPower(0); - goalOne.setPower(0); - goalTwo.setPower(0); - lift.setPower(0); - //slide.setPosition(.53); - //dump.setPosition(.53); + 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 { diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/K9AutoTime.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/K9AutoTime.java index 53abae7..25a19c7 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/K9AutoTime.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/K9AutoTime.java @@ -155,7 +155,7 @@ public void loop() { } /* - * set the motor power + * set the motor power */ motorRight.setPower(left); motorLeft.setPower(right); From 35ecd334ff924471013db99fdfa3a3866ed6edd6 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Sun, 14 Feb 2016 21:48:34 -0600 Subject: [PATCH 30/43] Prevent gamepad connection from throwing NullPointerException --- .../library/controller/Controller.java | 104 +++++++++++------- 1 file changed, 65 insertions(+), 39 deletions(-) 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 6b67737..68f5a65 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; } @@ -175,27 +187,32 @@ public void reset() { * Update the Controller states from a Gamepad. * CALL THIS METHOD ON EVERY EVENT LOOP! */ - //FIXME is Gamepad g necessary? public void update(Gamepad g) { - dpad_up = handleUpdate(dpad_up, g.dpad_up); - dpad_down = handleUpdate(dpad_down, g.dpad_down); - dpad_left = handleUpdate(dpad_left, g.dpad_left); - dpad_right = handleUpdate(dpad_right, g.dpad_right); - a = handleUpdate(a, g.a); - b = handleUpdate(b, g.b); - x = handleUpdate(x, g.x); - y = handleUpdate(y, g.y); - guide = handleUpdate(guide, g.guide); - start = handleUpdate(start, g.start); - back = handleUpdate(back, g.back); - left_bumper = handleUpdate(left_bumper, g.left_bumper); - right_bumper = handleUpdate(right_bumper, g.right_bumper); - left_trigger = g.left_trigger; - 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 = handleUpdate(dpad_up, g.dpad_up); + dpad_down = handleUpdate(dpad_down, g.dpad_down); + dpad_left = handleUpdate(dpad_left, g.dpad_left); + dpad_right = handleUpdate(dpad_right, g.dpad_right); + a = handleUpdate(a, g.a); + b = handleUpdate(b, g.b); + x = handleUpdate(x, g.x); + y = handleUpdate(y, g.y); + guide = handleUpdate(guide, g.guide); + start = handleUpdate(start, g.start); + back = handleUpdate(back, g.back); + left_bumper = handleUpdate(left_bumper, g.left_bumper); + right_bumper = handleUpdate(right_bumper, g.right_bumper); + left_trigger = g.left_trigger; + 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(); + } } /** @@ -205,7 +222,7 @@ public void update(Gamepad g) { * @param updatedstatus Boolean from Gamepad * @return The new state */ - private int handleUpdate(Integer b, boolean updatedstatus) { + private int handleUpdate(Integer b, boolean updatedstatus) throws NullPointerException { if (updatedstatus) { if (b == ButtonState.NOT_PRESSED || b == ButtonState.RELEASED) return ButtonState.PRESSED; @@ -219,6 +236,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{" + From 477db7cf753be8fdb7855faa5134fdddea14d57e Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Mon, 15 Feb 2016 00:26:28 -0600 Subject: [PATCH 31/43] Successful auto-correcting drive! --- .../library/nav/EncodedMotor.java | 54 ++++++++++++------- .../lasarobotics/library/nav/Navigator.java | 2 +- .../com/lasarobotics/library/nav/PID.java | 12 +++-- .../com/lasarobotics/library/util/Log.java | 13 ++++- .../opmodes/BasicEncoderTest.java | 46 +++++++++++----- .../opmodes/EncoderTest.java | 34 ++++++------ .../opmodes/FtcOpModeRegister.java | 6 ++- .../opmodes/NavXAutoTest.java | 4 +- .../opmodes/Teleop5998.java | 1 - .../opmodes/TeleopPurpleBot.java | 36 +++++++++++++ .../opmodes/TestPurpleBot.java | 39 ++++++++++++++ .../opmodes/first/MRGyroTest.java | 4 +- .../opmodes/first/PushBotAutoSensors.java | 6 +-- .../opmodes/first/PushBotHardware.java | 6 +-- .../navXDriveStraightPIDLinearOp.java | 2 +- .../kauailabs/navXDriveStraightPIDLoopOp.java | 2 +- .../kauailabs/navXRotateToAnglePIDLoopOp.java | 2 +- .../opmodes/navx/kauailabs/navXZeroYawOp.java | 4 +- 18 files changed, 199 insertions(+), 74 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TeleopPurpleBot.java create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/TestPurpleBot.java 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 index 38baad1..51b37d6 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -11,6 +11,7 @@ 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; /** @@ -22,21 +23,21 @@ public class EncodedMotor extends DcMotor { public EncodedMotor(DcMotor motor, MotorInfo info) { super(motor.getController(), motor.getPortNumber(), motor.getDirection()); enableEncoder(); - reset(); + requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info) { super(controller, portNumber); enableEncoder(); - reset(); + requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info, Direction direction) { super(controller, portNumber, direction); enableEncoder(); - reset(); + requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); } @@ -52,7 +53,7 @@ public void moveDistance(double distance) { if (distance == 0) return; - reset(); + requestReset(); if (distance < 0) super.setDirection(Direction.REVERSE); @@ -68,7 +69,7 @@ public void moveDistance(double distance, Units.Distance unit) { /** * Checks if the robot has reached the expected encoder position. - * After reaching the position, it is recommended to reset the encoders. + * 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 @@ -79,7 +80,7 @@ public boolean hasReachedPosition(double position) { /** * Test if the encoder has reached a specific position in a unit - * After reaching the position, it is recommended to reset the encoders. + * After reaching the position, it is recommended to requestReset the encoders. * * @param position Position, in units * @param unit Unit of position @@ -90,14 +91,14 @@ public boolean hasReachedPosition(double position, Units.Distance unit) { } public void enableEncoder() { - //This command requires (at least ?) one execution loop to reset the encoders + //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 reset the encoders + //This command requires (at least ?) one execution loop to requestReset the encoders setMode(DcMotorController.RunMode.RUN_WITHOUT_ENCODERS); isEncoderEnabled = false; @@ -108,7 +109,9 @@ public boolean isEncoderEnabled() { } public void update() { - //Check for encoder reset + //Check for encoder requestReset + if (requestReset && !encodersResetting) + resetEncoder(); if (encodersResetting && super.getCurrentPosition() == 0) { encodersResetting = false; encoderOffset = 0; @@ -116,18 +119,33 @@ public void update() { } } + 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 encoders. + * Reset encoder asynchronously + * + * (May take two frames for full effect) */ - public void reset() { + public void requestReset() { if (encodersResetting) return; - //This command requires (at least ?) one execution loop to reset the encoders - setMode(DcMotorController.RunMode.RESET_ENCODERS); + requestReset = true; + } - encodersResetting = true; - encoderOffset = -super.getCurrentPosition(); + /** + * Reset encoder immediately + * (May take one frame for full effect) + */ + public void resetNow() { + resetEncoder(); } public void setTargetPosition(double position, Units.Distance unit) { @@ -159,10 +177,10 @@ public double convertEncoderCountsToDistance(double encoderCounts, Units.Distanc } /** - * Tests if the encoder has reset. - * Note that encoders take at least one execution loop to fully reset. + * Tests if the encoder has requestReset. + * Note that encoders take at least one execution loop to fully requestReset. * - * @return True for reset, false for not reset. + * @return True for requestReset, false for not requestReset. */ public boolean hasEncoderReset() { return getCurrentPosition() == 0; diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java index 0efdbc4..3f66a78 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java @@ -206,7 +206,7 @@ public static class NavigatorTarget { /** * Instantiate a navigator target with a target position in field coordinates and a target heading * - * @param targetPosition Target position, in field coordinates. These coordinates can be reset(). + * @param targetPosition Target position, in field coordinates. These coordinates can be requestReset(). * @param heading Target heading, or direction to face. 0 is North, -90 or 270 is West, etc. Relative to robot. */ public NavigatorTarget(Vector2 targetPosition, double heading) { 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 index 7f5bd11..20fe541 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -133,11 +133,13 @@ private void update() { //Clamp output output = MathUtil.coerce(minOutput, maxOutput, output); - //Make sure the system does not move too fast - if ((output - outputLast) > maxChange * dt) - output = outputLast + (maxChange * dt); - else if ((outputLast - output) > maxChange * dt) - output = outputLast - (maxChange * dt); + if (maxChange > 0) { + //Make sure the system does not move too fast + if ((output - outputLast) > maxChange * dt) + output = outputLast + (maxChange * dt); + else if ((outputLast - output) > maxChange * dt) + output = outputLast - (maxChange * dt); + } // remember the error and output for the next time around. previousError = error; 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 e8fc146..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 @@ -67,9 +67,20 @@ public void add(String tag, long 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/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java index 7bb9d0f..3c5e559 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java @@ -14,13 +14,18 @@ * Basic navigational test - driving forward and rotation */ public class BasicEncoderTest extends OpMode { - private static final double WHEEL_RADIUS = 2; + private static final double WHEEL_RADIUS = 3.75 / 2.0; private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; - private static final double WHEEL_MECHANICAL_ADVANTAGE = 1; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; - private static final double PID_P = 0.050; //0.05 - private static final double PID_I = 0.000; //0.01 - private static final double PID_D = 0.000; //0.005 + //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;//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; //distance in feet private static final Units.Distance DISTANCE_UNIT = Units.Distance.FEET; private static final double MIN_POWER = 0; @@ -55,10 +60,10 @@ public void init() { backRight.setDirection(DcMotor.Direction.REVERSE); frontRight.setDirection(DcMotor.Direction.REVERSE); - /*backLeft.setTargetPosition(DISTANCE, DISTANCE_UNIT); + backLeft.setTargetPosition(DISTANCE, DISTANCE_UNIT); backRight.setTargetPosition(DISTANCE, DISTANCE_UNIT); frontLeft.setTargetPosition(DISTANCE, DISTANCE_UNIT); - frontRight.setTargetPosition(DISTANCE, DISTANCE_UNIT);*/ + frontRight.setTargetPosition(DISTANCE, DISTANCE_UNIT); //Create PID looper pidLeft = new PID(); @@ -85,15 +90,21 @@ public void init_loop() { @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 = Math.abs(backLeft.getCurrentPosition()) + - Math.abs(frontLeft.getCurrentPosition()); - double rightPos = Math.abs(backRight.getCurrentPosition()) + - Math.abs(frontRight.getCurrentPosition()); + double leftPos = (backLeft.getCurrentPosition() + + frontLeft.getCurrentPosition()) / 2; + double rightPos = (backRight.getCurrentPosition() + + frontRight.getCurrentPosition()) / 2; telemetry.addData("Pos", leftPos + ", " + rightPos); @@ -101,6 +112,12 @@ public void loop() { 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; @@ -119,8 +136,8 @@ public void loop() { telemetry.addData("Power", left + ", " + right); Tank.motor4(frontLeft, frontRight, backLeft, backRight, left, right); - if (left == 0 && right == 0) - phase = 1; + //if (left == 0 && right == 0) + // phase = 1; return; case 1: backLeft.setPowerFloat(); @@ -133,6 +150,7 @@ public void loop() { @Override public void stop() { - log.saveAs(Log.FileType.CSV); + 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 a870d59..4963c29 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -18,9 +18,9 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { - private static final double WHEEL_RADIUS = 2; + private static final double WHEEL_RADIUS = 3.75 / 2.0; private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; - private static final double WHEEL_MECHANICAL_ADVANTAGE = 1; + private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; private static final String NAVX_DIM = "dim"; //device interface module name private static final int NAVX_PORT = 1; //port on device interface module @@ -31,12 +31,12 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { private static final double NAVX_YAW_PID_I = 0; private static final double NAVX_YAW_PID_D = 0; - private static final double PID_P = 0.05; - private static final double PID_I = 0.01; - private static final double PID_D = 0.005; - private static final double PID_MAX_ACCEL = 1; + 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; - private static final double DISTANCE_FEET = 5; //distance in feet + private static final double DISTANCE_FEET = 1; //distance in feet private static final double MIN_POWER = 0; //distance in feet private static final DecimalFormat df = new DecimalFormat("#.##"); @@ -54,13 +54,13 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { int phase = 0; public void init() { - frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("frontLeft"), + frontLeft = new EncodedMotor(hardwareMap.dcMotor.get("lf"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); - frontRight = new EncodedMotor(hardwareMap.dcMotor.get("frontRight"), + frontRight = new EncodedMotor(hardwareMap.dcMotor.get("rf"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); - backLeft = new EncodedMotor(hardwareMap.dcMotor.get("backLeft"), + 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("backRight"), + backRight = new EncodedMotor(hardwareMap.dcMotor.get("rb"), new MotorInfo(WHEEL_RADIUS, WHEEL_RADIUS_UNIT, WHEEL_MECHANICAL_ADVANTAGE)); backLeft.setDirection(DcMotor.Direction.FORWARD); @@ -137,10 +137,10 @@ public void loop() { frontLeft.update(); frontRight.update(); - double leftPos = Math.abs(backLeft.getCurrentPosition()) + - Math.abs(frontLeft.getCurrentPosition()); - double rightPos = Math.abs(backRight.getCurrentPosition()) + - Math.abs(frontRight.getCurrentPosition()); + double leftPos = (backLeft.getCurrentPosition() + + frontLeft.getCurrentPosition()) / 2; + double rightPos = (backRight.getCurrentPosition() + + frontRight.getCurrentPosition()) / 2; pidLeft.addMeasurement(leftPos, timeDelta); pidRight.addMeasurement(rightPos, timeDelta); @@ -211,9 +211,9 @@ public void loop() { frontRight.setPowerFloat(); } else { - left = MathUtil.coerce(-1, 1, power) + + left = MathUtil.coerce(-1, 1, power) - Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); - right = MathUtil.coerce(-1, 1, power) - + right = MathUtil.coerce(-1, 1, power) + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); left = coerce(left); 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 dcef865..764a813 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -48,8 +48,10 @@ public class FtcOpModeRegister implements OpModeRegister { public void register(OpModeManager manager) { //Custom op modes manager.register("Null", NullOp.class); - manager.register("Teleop 5998", Teleop5998.class); - //manager.register("Basic Encoder Test", BasicEncoderTest.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("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 index 3694d5d..4a8c337 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavXAutoTest.java @@ -131,7 +131,7 @@ private void driveForEncoderCounts(int encoderCounts, double drive_speed) throws backLeft.setMode(DcMotorController.RunMode.RUN_USING_ENCODERS); NavXPIDController yawPIDController = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); - navx.reset(); //reset the NavX raw + navx.reset(); //requestReset the NavX raw /* Configure the PID controller */ yawPIDController.setSetpoint(0.0); @@ -180,7 +180,7 @@ 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(); //reset the NavX yaw + navx.reset(); //requestReset the NavX yaw /* Configure the PID controller */ yawPIDController.setSetpoint(deg); diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java index 5554bf2..97ff142 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/Teleop5998.java @@ -123,7 +123,6 @@ public void loop() { } else if (secondController.back == ButtonState.PRESSED) { climber.setPosition(1); } - } public void stop() { // make sure nothing moves after the end of the match 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/MRGyroTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/MRGyroTest.java index c25c84a..677621b 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/MRGyroTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/first/MRGyroTest.java @@ -74,9 +74,9 @@ public void runOpMode() throws InterruptedException { } while (opModeIsActive()) { - // if the A and B buttons are pressed, reset Z heading. + // if the A and B buttons are pressed, requestReset Z heading. if (gamepad1.a && gamepad1.b) { - // reset heading. + // requestReset heading. sensorGyro.resetZAxisIntegrator(); } 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 2a3a85f..ed6fa6b 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 @@ -117,7 +117,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()) { // @@ -165,7 +165,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()) { // @@ -207,7 +207,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 fa8e5db..e7c3759 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 @@ -822,7 +822,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() { // @@ -853,7 +853,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() { // @@ -884,7 +884,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/kauailabs/navXDriveStraightPIDLinearOp.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/navx/kauailabs/navXDriveStraightPIDLinearOp.java index cec6d1c..1271fa0 100644 --- 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 @@ -109,7 +109,7 @@ public void runOpMode() throws InterruptedException { waitForStart(); - /* reset the navX-Model device yaw angle so that whatever direction */ + /* requestReset the navX-Model device yaw angle so that whatever direction */ /* it is currently pointing will be zero degrees. */ navx_device.zeroYaw(); 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 index 5303f37..45144a0 100644 --- 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 @@ -112,7 +112,7 @@ public double limit(double a) { @Override public void start() { - /* reset the navX-Model device yaw angle so that whatever direction */ + /* 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(); 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 index cb49829..52a73f8 100644 --- 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 @@ -104,7 +104,7 @@ public void init() { @Override public void start() { - /* reset the navX-Model device yaw angle so that whatever direction */ + /* 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(); 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 index 2d44982..ab75619 100644 --- 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 @@ -43,7 +43,7 @@ OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE * Acquires processed data from navX-Micro * and displays it in the Robot DriveStation * as telemetry data. This opmode demonstrates how to "zero" - * (reset to zero degrees) the yaw. This causes whatever + * (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. @@ -133,7 +133,7 @@ public void loop() { telemetry.addData("6 Motion", motion); /* If the left 'bumper' button pressed, - reset (zero) the current yaw angle. This causes whatever + requestReset (zero) the current yaw angle. This causes whatever direction the navX-Model device is currently pointing to now be zero degrees. */ From 3b1a32d4c1a576a279b4dd43c4dacd95c726babb Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Mon, 15 Feb 2016 09:53:22 -0600 Subject: [PATCH 32/43] [UNTESTED] PID acceleration implementation --- .../com/lasarobotics/library/nav/PID.java | 38 +++++++++++++------ .../opmodes/EncoderTest.java | 16 +++++--- 2 files changed, 38 insertions(+), 16 deletions(-) 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 index 20fe541..3e80cb8 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -15,8 +15,10 @@ public class PID { protected double Kp = 0.2, Ki = 0.01, Kd = 1; // PID constant multipliers 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 maxChange = 0; // maximum change in output per second + 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 @@ -102,6 +104,10 @@ public double getLastTimeDelta() { } public double getOutputValue() { + return outputFlattened; + } + + public double getOutputValueRaw() { return output; } @@ -111,11 +117,19 @@ public void setOutputRange(double min, double max) { } public double getMaxAcceleration() { - return maxChange; + return maxChangePositive; } public void setMaxAcceleration(double maxChange) { - this.maxChange = maxChange; + this.maxChangePositive = maxChange; + } + + public double getMaxDeceleration() { + return maxChangeNegative; + } + + public void setMaxDeceleration(double maxChange) { + this.maxChangeNegative = Math.abs(maxChange); } private void update() { @@ -132,17 +146,19 @@ private void update() { //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); - if (maxChange > 0) { - //Make sure the system does not move too fast - if ((output - outputLast) > maxChange * dt) - output = outputLast + (maxChange * dt); - else if ((outputLast - output) > maxChange * dt) - output = outputLast - (maxChange * dt); - } + //Clamp the flattened output + outputFlattened = MathUtil.coerce(minOutput, maxOutput, outputFlattened); // remember the error and output for the next time around. previousError = error; - outputLast = output; + outputLast = outputFlattened; } } 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 4963c29..8f1ef6c 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -35,10 +35,13 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { private static final double PID_I = 0.0; private static final double PID_D = 0.0; private static final double PID_MAX_ACCEL = 0; + 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 = 2; //how ferociously should we rotate? + private static final DecimalFormat df = new DecimalFormat("#.##"); NavXDevice navx; @@ -83,6 +86,7 @@ public void init() { WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); pidLeft.setMaxAcceleration(PID_MAX_ACCEL); + pidLeft.setMaxDeceleration(PID_MAX_DECEL); pidLeft.setCoefficients(PID_P, PID_I, PID_D); pidRight = new PID(); @@ -90,6 +94,7 @@ public void init() { WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); pidRight.setMaxAcceleration(PID_MAX_ACCEL); + pidRight.setMaxDeceleration(PID_MAX_DECEL); pidRight.setCoefficients(PID_P, PID_I, PID_D); //Initialize the navX controller @@ -204,17 +209,18 @@ public void loop() { double avgDist = Math.abs(pidLeft.getOutputValue() - pidLeft.getSetpoint()) + Math.abs(pidRight.getOutputValue() - pidRight.getSetpoint()); avgDist /= 2; + telemetry.addData("avgDist", avgDist); if (avgDist < 500) { - backLeft.setPowerFloat(); + telemetry.addData("DONE", "COASTING..."); + /*backLeft.setPowerFloat(); backRight.setPowerFloat(); frontLeft.setPowerFloat(); - frontRight.setPowerFloat(); + frontRight.setPowerFloat();*/ } else { - left = MathUtil.coerce(-1, 1, power) - - Math.sin(MathUtil.coerce(-1, 1, powerCompensation * 2) * Math.PI / 2); + 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 * 2) * Math.PI / 2); + Math.sin(MathUtil.coerce(-1, 1, powerCompensation * FORWARD_ROTATION_FEROCITY) * Math.PI / 2); left = coerce(left); right = coerce(right); From b71bc5c69d1385ec27bd1870503bfa1be7cd043f Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 17 Mar 2016 12:19:12 -0500 Subject: [PATCH 33/43] Improvements? --- .../library/nav/EncodedMotor.java | 8 +++ .../lasarobotics/library/nav/MotorInfo.java | 8 ++- .../com/lasarobotics/library/nav/PID.java | 21 ++++++- .../com/lasarobotics/library/util/Units.java | 5 +- .../opmodes/EncoderTest.java | 38 ++++++------- .../opmodes/FtcOpModeRegister.java | 1 + .../opmodes/NavigationTest.java | 56 +++++++++++++++++++ 7 files changed, 110 insertions(+), 27 deletions(-) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/NavigationTest.java 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 index 51b37d6..508f7d4 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/EncodedMotor.java @@ -13,6 +13,7 @@ public class EncodedMotor extends DcMotor { 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 @@ -25,6 +26,7 @@ public EncodedMotor(DcMotor motor, MotorInfo info) { enableEncoder(); requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info) { @@ -32,6 +34,7 @@ protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo i enableEncoder(); requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; } protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo info, Direction direction) { @@ -39,6 +42,7 @@ protected EncodedMotor(DcMotorController controller, int portNumber, MotorInfo i enableEncoder(); requestReset(); this.wheelRadius = info.getEffectiveWheelRadius(Units.Distance.METERS); + this.motorInfo = info; } /** @@ -152,6 +156,10 @@ 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; 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 index 1798168..2ba6b1b 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java @@ -9,8 +9,12 @@ public class MotorInfo { private double wheelRadius; //wheel radius in meters private double mechanicalAdvantage; //mechanical advantage (2x = 2x effective radius) - public MotorInfo(double wheelRadius, Units.Distance radiusUnits) { - this.wheelRadius = radiusUnits.convertTo(Units.Distance.METERS, wheelRadius); + 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; } 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 index 3e80cb8..d9a7b87 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -1,6 +1,7 @@ package com.lasarobotics.library.nav; import com.lasarobotics.library.util.MathUtil; +import com.lasarobotics.library.util.Units; /** * PID Targeting @@ -21,12 +22,14 @@ public class PID { 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.Kp = 0.005; this.Ki = 0; this.Kd = 0; } + public PID(double p, double i, double d) { setCoefficients(p, i, d); } @@ -51,6 +54,12 @@ 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 * @@ -132,6 +141,14 @@ 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; @@ -154,8 +171,8 @@ private void update() { else if ((outputLast - outputFlattened) > maxChangeNegative * dt && maxChangeNegative > 0) outputFlattened = outputLast - (maxChangeNegative * dt); - //Clamp the flattened output - outputFlattened = MathUtil.coerce(minOutput, maxOutput, outputFlattened); + //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; 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 index 7c0c1ce..836ceb4 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/util/Units.java @@ -63,7 +63,8 @@ public enum Angle implements Unit { DEGREES(1), RADIANS(180.0 / Math.PI), REVOLUTIONS(360.0), - ENCODER_COUNTS(4.0); //encoders have 1440 resolutions / rotation, so 1 count = 1/4 degree + 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; @@ -111,7 +112,7 @@ public Angle getDefaultUnit() { } } - interface Unit { + private interface Unit { /** * Default unit. All conversion factors are relative to converting to this unit * 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 8f1ef6c..861abf5 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -18,12 +18,12 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { - private static final double WHEEL_RADIUS = 3.75 / 2.0; - private static final Units.Distance WHEEL_RADIUS_UNIT = Units.Distance.INCHES; - private static final double WHEEL_MECHANICAL_ADVANTAGE = 2; + //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 = 1; //port on device interface module + private static final int NAVX_PORT = 0; //port on device interface module private static final double NAVX_TOLERANCE_DEGREES = 10.0; //degrees of tolerance for PID controllers private static final double NAVX_TARGET_ANGLE_DEGREES = 0.0; //target angle for PID @@ -34,13 +34,13 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { 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; + private static final double PID_MAX_ACCEL = 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 = 2; //how ferociously should we rotate? + private static final double FORWARD_ROTATION_FEROCITY = 4; //how ferociously should we rotate? - lower if oscillating private static final DecimalFormat df = new DecimalFormat("#.##"); @@ -57,14 +57,11 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { int phase = 0; public void init() { - 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)); + 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); @@ -82,17 +79,13 @@ public void init() { //Create PID looper pidLeft = new PID(); - pidLeft.setSetpoint(Units.Distance.convertToAngle(DISTANCE_FEET, - WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, - WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); + 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.setSetpoint(Units.Distance.convertToAngle(DISTANCE_FEET, - WHEEL_RADIUS / WHEEL_MECHANICAL_ADVANTAGE, - WHEEL_RADIUS_UNIT, Units.Distance.FEET, Units.Angle.ENCODER_COUNTS)); + 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); @@ -206,11 +199,14 @@ public void loop() { 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 (avgDist < 500) { + if (false && avgDist < 500) { telemetry.addData("DONE", "COASTING..."); /*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 764a813..02d3717 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/FtcOpModeRegister.java @@ -52,6 +52,7 @@ public void register(OpModeManager manager) { 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/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() { + + } +} From be51e27829375f64eea53c2a5f5347633c05c83d Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 17 Mar 2016 17:56:22 -0500 Subject: [PATCH 34/43] PID loop tuning --- .../sensor/kauailabs/navx/NavXPIDController.java | 4 ++++ .../com/kauailabs/navx/ftc/navXPIDController.java | 12 +++++++----- .../opmodes/BasicEncoderTest.java | 8 ++++---- .../ftcrobotcontroller/opmodes/EncoderTest.java | 15 ++++++++------- 4 files changed, 23 insertions(+), 16 deletions(-) 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 index 25493eb..3d3fec1 100644 --- 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 @@ -40,6 +40,10 @@ public String getCoefficientString() { return "p: " + p + ", i: " + i + ", d: " + d + ", ff: " + ff; } + public String getCoefficientDebug() { + return "p: " + p * error_current + ", i: " + i * error_total + ", d: " + d * error_d; + } + public void reset() { super.reset(); } 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 index dd52e1a..97a7b16 100644 --- a/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java +++ b/lib-navx/src/main/java/com/kauailabs/navx/ftc/navXPIDController.java @@ -64,6 +64,11 @@ public class navXPIDController implements IDataArrivalSubscriber { 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; @@ -72,14 +77,10 @@ public class navXPIDController implements IDataArrivalSubscriber { double tolerance_amount; private Object sync_event = new Object(); private boolean timestamped = true; - /* Error statistics */ - private double error_current = 0.0; - private double error_previous = 0.0; - private double error_total = 0.0; /* Input/Output Clamps */ private double max_input = 0.0; private double min_input = 0.0; - private double max_output = 1.0; + private double max_output = 1; private double min_output = -1.0; private ToleranceType tolerance_type; /* Behavior */ @@ -384,6 +385,7 @@ public double stepController(double process_variable, int num_missed_samples) { } /* 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) + diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java index 3c5e559..05acddb 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/BasicEncoderTest.java @@ -14,19 +14,19 @@ * Basic navigational test - driving forward and rotation */ public class BasicEncoderTest extends OpMode { - private static final double WHEEL_RADIUS = 3.75 / 2.0; + 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 = 2; + 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;//PID_PU * 0.5; //0.05 + 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; //distance in feet + 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; 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 861abf5..3efef74 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -11,6 +11,7 @@ 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; @@ -21,20 +22,18 @@ 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_TOLERANCE_DEGREES = 10.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; - private static final double NAVX_YAW_PID_D = 0; + private static final double NAVX_TARGET_ANGLE_DEGREES = 180.0; //target angle for PID + private static final double NAVX_YAW_PID_P = 0.009; //.01 + private static final double NAVX_YAW_PID_I = 0.0002; //.0005 + private static final double NAVX_YAW_PID_D = 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 = 3; + 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 @@ -148,6 +147,8 @@ public void loop() { navx.displayTelemetry(telemetry); if (yawPIDController.isUpdateAvailable(yawPIDState)) { + telemetry.addData("NavX Debug Coefficients", yawPIDController.getCoefficientDebug()); + DbgLog.error(yawPIDController.getCoefficientDebug()); if (!yawPIDState.isOnTarget()) { powerCompensation = yawPIDState.getOutput(); } From f3a9eacc251a27b2e64d8eb938fbe3fdabc323ef Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 17 Mar 2016 17:57:50 -0500 Subject: [PATCH 35/43] PID loop tuning --- .../com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 3efef74..d047421 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -25,7 +25,7 @@ public class EncoderTest 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_TARGET_ANGLE_DEGREES = 180.0; //target angle for PID + private static final double NAVX_TARGET_ANGLE_DEGREES = 90.0; //target angle for PID private static final double NAVX_YAW_PID_P = 0.009; //.01 private static final double NAVX_YAW_PID_I = 0.0002; //.0005 private static final double NAVX_YAW_PID_D = 0.11; //.05 From 64103004496b9250ab33a0411a1e444f9815242d Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 17 Mar 2016 17:59:09 -0500 Subject: [PATCH 36/43] Revert PID loop tuning --- .../qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 d047421..d8b59e6 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/EncoderTest.java @@ -26,9 +26,9 @@ public class EncoderTest extends OpMode implements NavXDataReceiver { 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.009; //.01 - private static final double NAVX_YAW_PID_I = 0.0002; //.0005 - private static final double NAVX_YAW_PID_D = 0.11; //.05 + 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; From 97cfe4757e6c912bb6ee30aaa5e390166ca8bd2e Mon Sep 17 00:00:00 2001 From: BrendanHollaway Date: Thu, 17 Mar 2016 18:06:49 -0500 Subject: [PATCH 37/43] AutoTuner that may work... probably not --- .../opmodes/AutoTunePID.java | 177 ++++++++++++++++++ 1 file changed, 177 insertions(+) create mode 100644 sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java 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..e70b417 --- /dev/null +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java @@ -0,0 +1,177 @@ +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("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; + Iterator it = time_displacement.keySet().iterator(); + while (it.hasNext()) { + double next = Math.abs(time_displacement.get(it.next())); // measuring amplitude differentials. + max = Math.max(max, next); + min = Math.min(min, next); + } + return max - min <= 1; + } + + 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 { + navx = new NavXDevice(hardwareMap, "dim", 0); + tune_PID(120); + } +} From 8a0cd2629c399c69489df321bbfe07c37fe0eff1 Mon Sep 17 00:00:00 2001 From: BrendanHollaway Date: Thu, 17 Mar 2016 18:13:58 -0500 Subject: [PATCH 38/43] Auto tune PID thing that will probably blow up everything --- .../qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java index e70b417..b1f89e1 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java @@ -171,7 +171,13 @@ void setRightPower(double power) { } 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); tune_PID(120); + DbgLog.error("kP: %.2f, kI: %.2f, kD: %.2f", kP, kI, kD); + telemetry.addData("hi", String.format(" kP: %.2f, kI: %.2f, kD: %.2f", kP, kI, kD)); } } From c9376453bfcfdd4d1c3f21e54b0af960593dfff3 Mon Sep 17 00:00:00 2001 From: BrendanHollaway Date: Thu, 17 Mar 2016 18:22:29 -0500 Subject: [PATCH 39/43] AutoTuner that may work... probably not v2 --- .../com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java index b1f89e1..dfa0040 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java @@ -157,14 +157,14 @@ public boolean isTuned(double kP, double timeout) throws InterruptedException { } void setLeftPower(double power) { - if (opModeIsActive()) + if (!opModeIsActive()) return; motorBL.setPower(Range.clip(-power, -1, 1)); motorFL.setPower(Range.clip(-power, -1, 1)); } void setRightPower(double power) { - if (opModeIsActive()) + if (!opModeIsActive()) return; motorBR.setPower(Range.clip(power, -1, 1)); motorFR.setPower(Range.clip(power, -1, 1)); From 7edd14297ac910c7931cff0c58688d898796868d Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Thu, 17 Mar 2016 18:42:59 -0500 Subject: [PATCH 40/43] WARNING! THIS CODE IS HIGHLY EXPLOSIVE. :boom: --- .../opmodes/AutoTunePID.java | 19 +++++++++++++------ 1 file changed, 13 insertions(+), 6 deletions(-) diff --git a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java index dfa0040..ff7c79d 100644 --- a/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java +++ b/sample/src/main/java/com/qualcomm/ftcrobotcontroller/opmodes/AutoTunePID.java @@ -138,6 +138,8 @@ public boolean isTuned(double kP, double timeout) throws InterruptedException { 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)); } @@ -147,13 +149,13 @@ public boolean isTuned(double kP, double timeout) throws InterruptedException { return false; double max = Double.MIN_VALUE; double min = Double.MAX_VALUE; - Iterator it = time_displacement.keySet().iterator(); - while (it.hasNext()) { - double next = Math.abs(time_displacement.get(it.next())); // measuring amplitude differentials. + 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); } - return max - min <= 1; + DbgLog.error(String.format("max-min: %.2f", max - min)); + return max - min <= 2; } void setLeftPower(double power) { @@ -176,8 +178,13 @@ public void runOpMode() throws InterruptedException { motorFL = hardwareMap.dcMotor.get("lf"); motorBL = hardwareMap.dcMotor.get("lb"); navx = new NavXDevice(hardwareMap, "dim", 0); + waitForStart(); tune_PID(120); - DbgLog.error("kP: %.2f, kI: %.2f, kD: %.2f", kP, kI, kD); - telemetry.addData("hi", String.format(" kP: %.2f, kI: %.2f, kD: %.2f", kP, kI, kD)); + + 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 } } From 96bd21f3c59f933147e97653ba7a67f3686dd610 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Fri, 1 Apr 2016 09:40:56 -0500 Subject: [PATCH 41/43] Write simple NavX rotation nav lib --- .../library/drive/DriveConfiguration.java | 1 + .../library/drive/DriveSpecification.java | 31 --- .../lasarobotics/library/nav/Navigator.java | 236 ------------------ .../com/lasarobotics/library/nav/PID.java | 70 ++++-- .../library/nav/navigator/Navigator.java | 36 +++ .../library/nav/navigator/TankNav.java | 185 ++++++++++++++ .../kauailabs/navx/NavXPIDController.java | 31 +-- 7 files changed, 292 insertions(+), 298 deletions(-) delete mode 100644 ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java delete mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/Navigator.java create mode 100644 ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/TankNav.java 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 index 22068cc..358883c 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveConfiguration.java @@ -6,6 +6,7 @@ import com.qualcomm.robotcore.hardware.DcMotor; /** + * [NOT IMPLEMENTED] * Drivetrain configuration information - tells where motors are placed in addition to */ public class DriveConfiguration { diff --git a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java b/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java deleted file mode 100644 index 606611c..0000000 --- a/ftc-library/src/main/java/com/lasarobotics/library/drive/DriveSpecification.java +++ /dev/null @@ -1,31 +0,0 @@ -package com.lasarobotics.library.drive; - -import com.lasarobotics.library.util.Units; - -/** - * Drivetrain specification used by a variety of classes - */ -public class DriveSpecification { - Drivetrain drive; //Drive train type - double wheelRadius; //wheel radius in meters - - /** - * Instantiate a drIve train specIfication - * - * @param drive Current drive train - * @param wheelRadius Wheel radius - * @param radiusUnit Unit of wheel radius - */ - DriveSpecification(Drivetrain drive, double wheelRadius, Units.Distance radiusUnit) { - this.drive = drive; - this.wheelRadius = radiusUnit.convertTo(Units.Distance.METERS, wheelRadius); - } - - public double getWheelRadius() { - return wheelRadius; - } - - public Drivetrain getDrivetrain() { - return drive; - } -} diff --git a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java b/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java deleted file mode 100644 index 3f66a78..0000000 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/Navigator.java +++ /dev/null @@ -1,236 +0,0 @@ -package com.lasarobotics.library.nav; - -import android.util.Log; - -import com.lasarobotics.library.drive.DriveSpecification; -import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; -import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; -import com.lasarobotics.library.util.Vector2; -import com.lasarobotics.library.util.Vector3; - -/** - * 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 - */ -public class Navigator { - - static NavXDevice navx; - - /** - * Controllers - we track all axis for finding out when we are on a platform, - * but because robots only have really 1 DoF, we only track the acceleration - * relative to the field and the yaw rotation - */ - static NavXPIDController accelX; - static NavXPIDController accelY; - //static NavXPIDController accelZ; - //static NavXPIDController rotX; - //static NavXPIDController rotY; - static NavXPIDController rotZ; - - Vector3 startLocation = new Vector3<>(0.0, 0.0, 0.0); //robot start location in field coordinates - double startHeading = 0.0; //robot start heading relative to field - DriveSpecification driveSpec; //drive specifications - - Vector3 position = new Vector3<>(0.0, 0.0, 0.0); //current position - Vector3 velocity = new Vector3<>(0.0, 0.0, 0.0); //current velocity - Vector3 acceleration = new Vector3<>(0.0, 0.0, 0.0); //current acceleration - - NavigatorTarget target = null; //current target - double toleranceRot = 2; //rotational tolerance in degrees - double toleranceLoc = 0.01; //directional tolerance in meters - - // - // INITIALIZATION - // - - public Navigator(NavXDevice navx, DriveSpecification driveSpec) { - Navigator.navx = navx; - zero(new Vector3<>(0.0, 0.0, 0.0), 0.0); - initialize(driveSpec); - } - - public Navigator(NavXDevice navx, DriveSpecification driveSpec, Vector2 startLocation, double startHeading) { - Navigator.navx = navx; - zero(startLocation, startHeading); - initialize(driveSpec); - } - - public Navigator(NavXDevice navx, DriveSpecification driveSpec, Vector3 startLocation, double startHeading) { - Navigator.navx = navx; - zero(startLocation, startHeading); - initialize(driveSpec); - } - - private void initialize(DriveSpecification driveSpec) { - zero(); - this.driveSpec = driveSpec; - - accelX = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_X); - accelY = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Y); - //accelZ = new NavXPIDController(navx, NavXPIDController.DataSource.LINEAR_ACCEL_Z); - - //rotX = new NavXPIDController(navx, NavXPIDController.DataSource.PITCH); - //rotY = new NavXPIDController(navx, NavXPIDController.DataSource.ROLL); - rotZ = new NavXPIDController(navx, NavXPIDController.DataSource.YAW); - } - - public void zero() { - zero(new Vector3<>(0.0, 0.0, 0.0), 0.0); - } - - public void zero(Vector2 startLocation, double startHeading) { - zero(new Vector3<>(startLocation.x, startLocation.y, 0.0), startHeading); - } - - public void zero(Vector3 startLocation, double startHeading) { - this.startLocation = startLocation; - this.startHeading = startHeading; - navx.reset(); - - position = new Vector3<>(0.0, 0.0, 0.0); - velocity = new Vector3<>(0.0, 0.0, 0.0); - acceleration = new Vector3<>(0.0, 0.0, 0.0); - } - - public void setPID(Controller controller, double p, double i, double d) { - controller.getController().setPID(p, i, d); - Log.d("Navigator PID", controller.getController().getCoefficientString()); - } - - public void setTarget(NavigatorTarget target) { - boolean start = this.target == null; //if true, we should start tasks - this.target = target; - updateParameters(); - } - - // - // TARGETING - // - - private void updateParameters() { - accelX.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceLoc); - accelY.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceLoc); - rotZ.setTolerance(NavXPIDController.ToleranceType.ABSOLUTE, toleranceRot); - - //accelX.setSetpoint(); - } - - public enum Controller { - ACCEL_X, - ACCEL_Y, - GYRO_Z; - - public NavXPIDController getController() { - switch (this) { - case ACCEL_X: - return accelX; - case ACCEL_Y: - return accelY; - case GYRO_Z: - return rotZ; - default: - throw new RuntimeException("No such instance!"); - } - } - } - - //TODO: implement Bezier path-based targeting ? - - // - // RESULTS - // - - public static class NavigatorResult { - Vector2 power; - - NavigatorResult() { - power = new Vector2(0.0f, 0.0f); - } - - /** - * Get a power vector suitable for arcade control - * - * @return A vector with the power returning as an X and Y magnitude - */ - public Vector2 getPowerVector() { - return power; - } - - /** - * Get a power vector suitable for tank drive control - * - * @return A vector with the first index being the left side motors and the second index the right motors - */ - public Vector2 getPowerTankDrive() { - //FIXME implement - throw new RuntimeException("Not yet implemented!"); - } - - /** - * Get a power vector suitable for arcade control or an omnidirectional drive train - * - * @return A vector with the first index being the magnitude and the second a heading, - * which can can be from -360 (counterclockwise rotation) to 360 (clockwise) - */ - public Vector2 getPowerMagnitudeDirection() { - //FIXME implement - throw new RuntimeException("Not yet implemented!"); - } - - } - - public static class NavigatorTarget { - Vector2 targetPosition; - double heading; - - /** - * Instantiate a navigator target with a target position in field coordinates and a target heading - * - * @param targetPosition Target position, in field coordinates. These coordinates can be requestReset(). - * @param heading Target heading, or direction to face. 0 is North, -90 or 270 is West, etc. Relative to robot. - */ - public NavigatorTarget(Vector2 targetPosition, double heading) { - this.targetPosition = targetPosition; - this.heading = heading; - } - - /** - * Instantiate a navigator target with a 2D target vector, either in field coordinates - * or a distance relative to the robot's current position - * If robotRelative is true, the target position is a vector representing the - * distance away from the current location - * The direction of the vector represents the heading. - * Otherwise, targetPosition is a location in field coordinates, defined by startLocation. - * - * @param targetPosition Robot's target position, in either field or inertial coordinates - * @param heading Robot's target heading, either relative to the field (as with field oriented - * drive) or to the robot's current rotation - * @param robotRelative True if targetPosition is in inertial (robot-relative) coordianates, - * False if targetPosition is in field coordinates - */ - public NavigatorTarget(Vector2 targetPosition, double heading, boolean robotRelative) { - //FIXME implement - throw new RuntimeException("Not yet implemented!"); - } - } -} 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 index d9a7b87..b04ffef 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/PID.java @@ -7,13 +7,13 @@ * 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 Kp = 0.2, Ki = 0.01, Kd = 1; // PID constant multipliers 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 @@ -25,19 +25,21 @@ public class PID { protected double minPower = 0; // minimum power for anti-stall protection public PID() { - this.Kp = 0.005; - this.Ki = 0; - this.Kd = 0; + this.coefficients = new PIDCoefficients(0.05, 0, 0); } public PID(double p, double i, double d) { - setCoefficients(p, i, d); + this.coefficients = new PIDCoefficients(p, i, d); + } + + public PID(PIDCoefficients coefficients) { + this.coefficients = coefficients; } public void setCoefficients(double p, double i, double d) { - this.Kp = p; - this.Ki = i; - this.Kd = d; + coefficients.p = p; + coefficients.i = i; + coefficients.d = d; } public void addMeasurement(double measuredValue, double timeDelta) { @@ -96,16 +98,13 @@ public double getPreviousError() { return previousError; } - public double getCoefficientProportional() { - return Kp; - } - - public double getCoefficientIntegral() { - return Ki; + public PIDCoefficients getCoefficients() { + return coefficients; } - public double getCoefficientDerivative() { - return Kd; + public void setCoefficients(PIDCoefficients coefficients) { + coefficients.ff = 0.0; + this.coefficients = coefficients; } public double getLastTimeDelta() { @@ -159,7 +158,7 @@ private void update() { // calculate how much drive the output in order to get to the // desired setpoint. - output = (Kp * error) + (Ki * integral) + (Kd * derivative); + output = (coefficients.p * error) + (coefficients.i * integral) + (coefficients.d * derivative); //Clamp output output = MathUtil.coerce(minOutput, maxOutput, output); @@ -178,4 +177,41 @@ else if ((outputLast - outputFlattened) > maxChangeNegative * dt && maxChangeNeg 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..0b955b8 --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/Navigator.java @@ -0,0 +1,36 @@ +package com.lasarobotics.library.nav.navigator; + +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDataReceiver; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; + +/** + * 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; + } +} 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..308b8da --- /dev/null +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/navigator/TankNav.java @@ -0,0 +1,185 @@ +package com.lasarobotics.library.nav.navigator; + +import com.kauailabs.navx.ftc.navXPIDController; +import com.lasarobotics.library.nav.EncodedMotor; +import com.lasarobotics.library.nav.PID; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXDevice; +import com.lasarobotics.library.sensor.kauailabs.navx.NavXPIDController; +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(); + + public TankNav(NavXDevice navx, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight) { + super(navx); + this.motorsLeft = motorsLeft; + this.motorsRight = motorsRight; + } + + public TankNav(NavXDevice navx, NavigationParams params, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight) { + super(navx); + this.motorsLeft = motorsLeft; + this.motorsRight = motorsRight; + } + + 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; + } + + public void rotateInPlaceAsyncStart(double targetDegrees, double distTolernace) { + navx.reset(); + + //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.kRotStatic); + //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.ABSOLUTE, distTolernace); + //Start data collection + rotPID.start(); + + //Initiate blank PID state + rotState = new NavXPIDController.PIDState(); + + updateTime(); + } + + public AsyncMotorResult rotateInPlaceAsyncRun() { + double dt = updateTime(); + + double power = 0.0; + if (rotPID.isUpdateAvailable(rotState)) + lastResult = new AsyncMotorResult(-power, power, rotState.isOnTarget()); + return lastResult; + } + + public void rotateInPlace(double targetDegrees) throws InterruptedException { + rotateInPlace(targetDegrees, 0, 0); + } + + public void rotateInPlace(double targetDegrees, double distTolerance, double secTimeout) throws InterruptedException { + AsyncMotorResult result = new AsyncMotorResult(); + + rotateInPlaceAsyncStart(targetDegrees, distTolerance); + long t = System.nanoTime(); + while (true) { + result = rotateInPlaceAsyncRun(); + 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; + + 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 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/NavXPIDController.java b/ftc-library/src/main/java/com/lasarobotics/library/sensor/kauailabs/navx/NavXPIDController.java index 3d3fec1..085f78b 100644 --- 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 @@ -3,8 +3,11 @@ 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 */ @@ -20,28 +23,24 @@ public NavXPIDController(NavXDevice navX, DataSource dataSource) { setOutputRange(-1.0, 1.0); } - public double getCoefficientPotential() { - return p; - } - - public double getCoefficientIntegral() { - return i; + public PID.PIDCoefficients getCoefficients() { + return new PID.PIDCoefficients(p, i, d, ff); } - public double getCoefficientDerivative() { - return d; - } - - public double getCoefficientFastForward() { - return 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 getCoefficientDebug() { - return "p: " + p * error_current + ", i: " + i * error_total + ", d: " + d * error_d; + 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() { @@ -86,6 +85,10 @@ public double getOutputValue() { return enableAntistall ? MathUtil.deadband(antistallDeadband, super.get()) : super.get(); } + public double getError() { + return super.getError(); + } + public double getAntistallDeadband() { return antistallDeadband; } From f3dac7da74f2a8b2329263372965e47de0001c25 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Tue, 5 Apr 2016 00:23:54 -0500 Subject: [PATCH 42/43] Write advanced navigator --- .../lasarobotics/library/nav/MotorInfo.java | 18 +++ .../library/nav/navigator/Navigator.java | 5 + .../library/nav/navigator/TankNav.java | 136 ++++++++++++++++-- 3 files changed, 151 insertions(+), 8 deletions(-) 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 index 2ba6b1b..046e39f 100644 --- a/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java +++ b/ftc-library/src/main/java/com/lasarobotics/library/nav/MotorInfo.java @@ -23,6 +23,24 @@ public MotorInfo(double wheelRadius, Units.Distance radiusUnits, double mechanic 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); } 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 index 0b955b8..72cd849 100644 --- 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 @@ -2,6 +2,7 @@ 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: @@ -33,4 +34,8 @@ abstract class Navigator implements NavXDataReceiver { 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 index 308b8da..2f4245d 100644 --- 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 @@ -2,9 +2,12 @@ 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; /** @@ -19,17 +22,21 @@ public class TankNav extends Navigator { NavXPIDController.PIDState rotState; AsyncMotorResult lastResult; long lastTime = System.nanoTime(); + Telemetry telemetry; - public TankNav(NavXDevice navx, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight) { + public TankNav(NavXDevice navx, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight, Telemetry telemetry) { super(navx); this.motorsLeft = motorsLeft; this.motorsRight = motorsRight; + this.telemetry = telemetry; } - public TankNav(NavXDevice navx, NavigationParams params, EncodedMotor[] motorsLeft, EncodedMotor[] motorsRight) { + 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() { @@ -53,6 +60,21 @@ private double updateTime() { 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 distTolernace) { navx.reset(); @@ -78,26 +100,119 @@ public void rotateInPlaceAsyncStart(double targetDegrees, double distTolernace) updateTime(); } - public AsyncMotorResult rotateInPlaceAsyncRun() { + public AsyncMotorResult rotateInPlaceAsyncRun(double powerFactor) { double dt = updateTime(); + updateMotors(); double power = 0.0; if (rotPID.isUpdateAvailable(rotState)) - lastResult = new AsyncMotorResult(-power, power, rotState.isOnTarget()); + power = rotPID.getOutputValue(); + lastResult = new AsyncMotorResult(-power * powerFactor, power * powerFactor, rotState.isOnTarget()); + navx.displayTelemetry(telemetry); return lastResult; } - public void rotateInPlace(double targetDegrees) throws InterruptedException { - rotateInPlace(targetDegrees, 0, 0); + public void rotateInPlace(double targetDegrees, double power) throws InterruptedException { + rotateInPlace(targetDegrees, power, 0, 0); } - public void rotateInPlace(double targetDegrees, double distTolerance, double secTimeout) throws InterruptedException { + public void rotateInPlace(double targetDegrees, double power, double distTolerance, double secTimeout) throws InterruptedException { AsyncMotorResult result = new AsyncMotorResult(); rotateInPlaceAsyncStart(targetDegrees, distTolerance); long t = System.nanoTime(); while (true) { - result = rotateInPlaceAsyncRun(); + result = rotateInPlaceAsyncRun(power); + 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 moveStabilizedAsyncStart(double distTarget, Units.Distance distUnit) { + navx.reset(); + + //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; @@ -127,6 +242,7 @@ public static class NavigationParams { double kRotMovingFerocity = 4; double kTransMaxAccel = 0.0; double kTransMaxDecel = 0.0; + double kMinMotorPower = 0.00; NavigationParams() { @@ -152,6 +268,10 @@ public void setTranslationMaxAccel(double maxAccel, double maxDecel) { kTransMaxAccel = maxAccel; kTransMaxDecel = maxDecel; } + + public void setMinimumMotorPower(double minPower) { + this.kMinMotorPower = minPower; + } } public static class AsyncMotorResult { From 76a32c5d0a567c18a69dcb4d2b809ac23c78bc78 Mon Sep 17 00:00:00 2001 From: Arthur Pachachura Date: Wed, 6 Apr 2016 22:51:04 -0500 Subject: [PATCH 43/43] Add Nav improvements --- .../library/nav/navigator/TankNav.java | 53 +++++++++++++------ .../sensor/kauailabs/navx/NavXDevice.java | 9 ++++ 2 files changed, 47 insertions(+), 15 deletions(-) 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 index 2f4245d..798dc7b 100644 --- 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 @@ -29,6 +29,7 @@ public TankNav(NavXDevice navx, EncodedMotor[] motorsLeft, EncodedMotor[] motors 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) { @@ -75,8 +76,15 @@ private double meanPos(EncodedMotor[] motors) { return mean; } - public void rotateInPlaceAsyncStart(double targetDegrees, double distTolernace) { + 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 @@ -86,11 +94,14 @@ public void rotateInPlaceAsyncStart(double targetDegrees, double distTolernace) //Allow crossing over the bounds (see setContinuous() documentation) rotPID.setContinuous(true); //Set P,I,D coefficients - rotPID.setCoefficients(params.kRotStatic); + 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.ABSOLUTE, distTolernace); + if (angleTolerance > 0) + rotPID.setTolerance(navXPIDController.ToleranceType.ABSOLUTE, angleTolerance); + else + rotPID.setTolerance(navXPIDController.ToleranceType.NONE, 0); //Start data collection rotPID.start(); @@ -102,41 +113,53 @@ public void rotateInPlaceAsyncStart(double targetDegrees, double distTolernace) public AsyncMotorResult rotateInPlaceAsyncRun(double powerFactor) { double dt = updateTime(); - updateMotors(); + //updateMotors(); double power = 0.0; - if (rotPID.isUpdateAvailable(rotState)) - power = rotPID.getOutputValue(); - lastResult = new AsyncMotorResult(-power * powerFactor, power * powerFactor, rotState.isOnTarget()); - navx.displayTelemetry(telemetry); + 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) throws InterruptedException { + public void rotateInPlace(double targetDegrees, double power) { rotateInPlace(targetDegrees, power, 0, 0); } - public void rotateInPlace(double targetDegrees, double power, double distTolerance, double secTimeout) throws InterruptedException { - AsyncMotorResult result = new AsyncMotorResult(); + 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))) - break; + ((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()); - Thread.sleep(5); + 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 @@ -214,7 +237,7 @@ public void moveStabilized(double distTarget, Units.Distance distUnit, double po while (true) { result = moveStabilizedAsyncRun(power, distTolerance); if (result.isAtTarget() || - ((secTimeout > 0 && (System.nanoTime() - t) / 1000000000.0 > secTimeout))) + ((secTimeout > 0 && ((System.nanoTime() - t) / 1000000000.0 > secTimeout)))) break; t = System.nanoTime(); for (EncodedMotor l : motorsLeft) 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 index f7d18c3..9f03a5e 100644 --- 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 @@ -2,6 +2,7 @@ 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; @@ -83,6 +84,14 @@ 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 ?

     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