diff --git a/TeamCode/src/main/java/bBlueAutoV1.java b/TeamCode/src/main/java/bBlueAutoV1.java
new file mode 100644
index 0000000..48f57c4
--- /dev/null
+++ b/TeamCode/src/main/java/bBlueAutoV1.java
@@ -0,0 +1,236 @@
+import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
+import com.qualcomm.robotcore.eventloop.opmode.OpMode;
+
+import org.firstinspires.ftc.robotcore.external.Telemetry;
+import org.firstinspires.ftc.teamcode.pedroPathing.follower.Follower;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Path;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+@Autonomous(name = "bBlueAutoV1", group = "V1")
+public class bBlueAutoV1 extends OpMode {
+
+ public Telemetry telemetry;
+ public Follower robot;
+ public PathChain path;
+
+ @Override
+ public void init() {
+ robot = new Follower(hardwareMap);
+
+ PathBuilder builder = new PathBuilder();
+
+ path = builder
+ .addPath(
+ // Line 1
+ new BezierLine(
+ new Point(9.757, 84.983, Point.CARTESIAN),
+ new Point(28.573, 76.302, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 2
+ new BezierLine(
+ new Point(28.573, 76.302, Point.CARTESIAN),
+ new Point(36.203, 76.140, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 3
+ new BezierLine(
+ new Point(36.203, 76.140, Point.CARTESIAN),
+ new Point(35.067, 35.716, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 4
+ new BezierLine(
+ new Point(35.067, 35.716, Point.CARTESIAN),
+ new Point(73.705, 34.742, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 5
+ new BezierLine(
+ new Point(73.705, 34.742, Point.CARTESIAN),
+ new Point(73.705, 24.839, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 6
+ new BezierLine(
+ new Point(73.705, 24.839, Point.CARTESIAN),
+ new Point(7.630, 26.462, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 7
+ new BezierLine(
+ new Point(7.630, 26.462, Point.CARTESIAN),
+ new Point(64.126, 22.728, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 8
+ new BezierLine(
+ new Point(64.126, 22.728, Point.CARTESIAN),
+ new Point(63.964, 13.150, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 9
+ new BezierLine(
+ new Point(63.964, 13.150, Point.CARTESIAN),
+ new Point(12.338, 15.260, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 10
+ new BezierLine(
+ new Point(12.338, 15.260, Point.CARTESIAN),
+ new Point(63.802, 13.150, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 11
+ new BezierLine(
+ new Point(63.802, 13.150, Point.CARTESIAN),
+ new Point(63.639, 11.689, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 12
+ new BezierLine(
+ new Point(63.639, 11.689, Point.CARTESIAN),
+ new Point(12.014, 11.689, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 13
+ new BezierLine(
+ new Point(12.014, 11.689, Point.CARTESIAN),
+ new Point(62.665, 30.196, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 14
+ new BezierLine(
+ new Point(62.665, 30.196, Point.CARTESIAN),
+ new Point(13.312, 51.463, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 15
+ new BezierLine(
+ new Point(13.312, 51.463, Point.CARTESIAN),
+ new Point(16.234, 103.738, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 16
+ new BezierLine(
+ new Point(16.234, 103.738, Point.CARTESIAN),
+ new Point(68.023, 108.284, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 17
+ new BezierLine(
+ new Point(68.023, 108.284, Point.CARTESIAN),
+ new Point(68.185, 121.109, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 18
+ new BezierLine(
+ new Point(68.185, 121.109, Point.CARTESIAN),
+ new Point(21.754, 119.811, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 19
+ new BezierLine(
+ new Point(21.754, 119.811, Point.CARTESIAN),
+ new Point(11.526, 129.227, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 20
+ new BezierLine(
+ new Point(11.526, 129.227, Point.CARTESIAN),
+ new Point(72.568, 111.856, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 21
+ new BezierLine(
+ new Point(72.568, 111.856, Point.CARTESIAN),
+ new Point(58.607, 128.902, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 22
+ new BezierLine(
+ new Point(58.607, 128.902, Point.CARTESIAN),
+ new Point(11.364, 130.850, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 23
+ new BezierLine(
+ new Point(11.364, 130.850, Point.CARTESIAN),
+ new Point(58.931, 128.577, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 24
+ new BezierLine(
+ new Point(58.931, 128.577, Point.CARTESIAN),
+ new Point(58.769, 133.123, Point.CARTESIAN)
+ )
+ )
+
+ .addPath(
+ // Line 25
+ new BezierLine(
+ new Point(58.769, 133.123, Point.CARTESIAN),
+ new Point(13.475, 133.935, Point.CARTESIAN)
+ )
+ ).build();
+ ;
+ }
+
+
+ @Override
+ public void loop() {
+ robot.update();
+ if (robot.atParametricEnd())
+ robot.followPath(path);
+ robot.telemetryDebug(telemetry);
+ }
+}
\ No newline at end of file
diff --git a/TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java b/TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java
new file mode 100644
index 0000000..949d68c
--- /dev/null
+++ b/TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java
@@ -0,0 +1,174 @@
+package nbRedAutoV1;
+
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathBuilder;
+import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point;
+
+public class bRedAutoV1 {
+
+ public bRedAutoV1() {
+ PathBuilder builder = new PathBuilder();
+
+ builder
+ .addPath(
+ // Line 1
+ new BezierLine(
+ new Point(133.935, 83.770, Point.CARTESIAN),
+ new Point(79.874, 117.213, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 2
+ new BezierLine(
+ new Point(79.874, 117.213, Point.CARTESIAN),
+ new Point(79.874, 120.785, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 3
+ new BezierLine(
+ new Point(79.874, 120.785, Point.CARTESIAN),
+ new Point(131.824, 118.349, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 4
+ new BezierLine(
+ new Point(131.824, 118.349, Point.CARTESIAN),
+ new Point(79.549, 120.460, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 5
+ new BezierLine(
+ new Point(79.549, 120.460, Point.CARTESIAN),
+ new Point(79.549, 128.740, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 6
+ new BezierLine(
+ new Point(79.549, 128.740, Point.CARTESIAN),
+ new Point(131.337, 128.090, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 7
+ new BezierLine(
+ new Point(131.337, 128.090, Point.CARTESIAN),
+ new Point(79.549, 128.740, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 8
+ new BezierLine(
+ new Point(79.549, 128.740, Point.CARTESIAN),
+ new Point(79.549, 133.610, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 9
+ new BezierLine(
+ new Point(79.549, 133.610, Point.CARTESIAN),
+ new Point(130.850, 133.285, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 10
+ new BezierLine(
+ new Point(130.850, 133.285, Point.CARTESIAN),
+ new Point(130.201, 36.528, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 11
+ new BezierLine(
+ new Point(130.201, 36.528, Point.CARTESIAN),
+ new Point(84.095, 36.203, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 12
+ new BezierLine(
+ new Point(84.095, 36.203, Point.CARTESIAN),
+ new Point(84.095, 23.378, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 13
+ new BezierLine(
+ new Point(84.095, 23.378, Point.CARTESIAN),
+ new Point(119.811, 23.378, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 14
+ new BezierLine(
+ new Point(119.811, 23.378, Point.CARTESIAN),
+ new Point(127.603, 13.475, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 15
+ new BezierLine(
+ new Point(127.603, 13.475, Point.CARTESIAN),
+ new Point(88.640, 28.248, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 16
+ new BezierLine(
+ new Point(88.640, 28.248, Point.CARTESIAN),
+ new Point(87.666, 15.910, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 17
+ new BezierLine(
+ new Point(87.666, 15.910, Point.CARTESIAN),
+ new Point(127.603, 12.014, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 18
+ new BezierLine(
+ new Point(127.603, 12.014, Point.CARTESIAN),
+ new Point(86.692, 12.825, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 19
+ new BezierLine(
+ new Point(86.692, 12.825, Point.CARTESIAN),
+ new Point(86.692, 10.390, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation()
+ .addPath(
+ // Line 20
+ new BezierLine(
+ new Point(86.692, 10.390, Point.CARTESIAN),
+ new Point(126.467, 9.903, Point.CARTESIAN)
+ )
+ )
+ .setTangentHeadingInterpolation();
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java
new file mode 100644
index 0000000..cc786f3
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java
@@ -0,0 +1,186 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.Acceleration;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.external.navigation.Position;
+import org.firstinspires.ftc.robotcore.external.navigation.Velocity;
+
+import java.util.Locale;
+
+/*
+ * This OpMode gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit.
+ *
+ * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
+ * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
+ *
+ * Use Android Studio to Copy this Class, and Paste it into your team's code folder with a new name.
+ * Remove or comment out the @Disabled line to add this OpMode to the Driver Station OpMode list
+ *
+ * @see Adafruit IMU
+ */
+@TeleOp(name = "Sensor: BNO055 IMU", group = "Sensor")
+public class SensorBNO055IMU extends LinearOpMode
+ {
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ // The IMU sensor object
+ BNO055IMU imu;
+
+ // State used for updating telemetry
+ Orientation angles;
+ Acceleration gravity;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() {
+
+ // Set up the parameters with which we will use our IMU. Note that integration
+ // algorithm here just reports accelerations to the logcat log; it doesn't actually
+ // provide positional information.
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.angleUnit = BNO055IMU.AngleUnit.DEGREES;
+ parameters.accelUnit = BNO055IMU.AccelUnit.METERS_PERSEC_PERSEC;
+ parameters.calibrationDataFile = "BNO055IMUCalibration.json"; // see the calibration sample OpMode
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ parameters.accelerationIntegrationAlgorithm = new JustLoggingAccelerationIntegrator();
+
+ // Retrieve and initialize the IMU. We expect the IMU to be attached to an I2C port
+ // on a Core Device Interface Module, configured to be a sensor of type "AdaFruit IMU",
+ // and named "imu".
+ imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
+ imu.initialize(parameters);
+
+ // Set up our telemetry dashboard
+ composeTelemetry();
+
+ // Wait until we're told to go
+ waitForStart();
+
+ // Start the logging of measured acceleration
+ imu.startAccelerationIntegration(new Position(), new Velocity(), 1000);
+
+ // Loop and update the dashboard
+ while (opModeIsActive()) {
+ telemetry.update();
+ }
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Telemetry Configuration
+ //----------------------------------------------------------------------------------------------
+
+ void composeTelemetry() {
+
+ // At the beginning of each telemetry update, grab a bunch of data
+ // from the IMU that we will then display in separate lines.
+ telemetry.addAction(new Runnable() { @Override public void run()
+ {
+ // Acquiring the angles is relatively expensive; we don't want
+ // to do that in each of the three items that need that info, as that's
+ // three times the necessary expense.
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ gravity = imu.getGravity();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("status", new Func() {
+ @Override public String value() {
+ return imu.getSystemStatus().toShortString();
+ }
+ })
+ .addData("calib", new Func() {
+ @Override public String value() {
+ return imu.getCalibrationStatus().toString();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("heading", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.firstAngle);
+ }
+ })
+ .addData("roll", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.secondAngle);
+ }
+ })
+ .addData("pitch", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.thirdAngle);
+ }
+ });
+
+ telemetry.addLine()
+ .addData("grvty", new Func() {
+ @Override public String value() {
+ return gravity.toString();
+ }
+ })
+ .addData("mag", new Func() {
+ @Override public String value() {
+ return String.format(Locale.getDefault(), "%.3f",
+ Math.sqrt(gravity.xAccel*gravity.xAccel
+ + gravity.yAccel*gravity.yAccel
+ + gravity.zAccel*gravity.zAccel));
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}
diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java
new file mode 100644
index 0000000..5d79cc6
--- /dev/null
+++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java
@@ -0,0 +1,230 @@
+/* Copyright (c) 2017 FIRST. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without modification,
+ * are permitted (subject to the limitations in the disclaimer below) provided that
+ * the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this list
+ * of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice, this
+ * list of conditions and the following disclaimer in the documentation and/or
+ * other materials provided with the distribution.
+ *
+ * Neither the name of FIRST nor the names of its contributors may be used to endorse or
+ * promote products derived from this software without specific prior written permission.
+ *
+ * NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
+ * LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
+ * THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
+ * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
+ * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
+ * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+package org.firstinspires.ftc.teamcode;
+
+import com.qualcomm.hardware.bosch.BNO055IMU;
+import com.qualcomm.robotcore.eventloop.opmode.Disabled;
+import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
+import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
+import com.qualcomm.robotcore.util.ReadWriteFile;
+
+import org.firstinspires.ftc.robotcore.external.Func;
+import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesOrder;
+import org.firstinspires.ftc.robotcore.external.navigation.AxesReference;
+import org.firstinspires.ftc.robotcore.external.navigation.Orientation;
+import org.firstinspires.ftc.robotcore.internal.system.AppUtil;
+
+import java.io.File;
+import java.util.Locale;
+
+/*
+ * This OpMode calibrates a BNO055 IMU per
+ * "Section 3.11 Calibration" of the BNO055 specification.
+ *
+ * Note: this is a Legacy example that will not work with newer Control/Expansion Hubs that use a different IMU
+ * Please use the new SensorIMUOrthogonal or SensorIMUNonOrthogonal samples for a more universal IMU interface.
+ *
+ * Manual calibration of the IMU is definitely NOT necessary: except for the magnetometer (which is not used by the
+ * default "IMU" SensorMode), the BNO055 is internally self-calibrating and thus can be very successfully used without
+ * manual intervention. That said, performing a one-time calibration, saving the results persistently, then loading them
+ * again at each run can help reduce the time that automatic calibration requires.
+ *
+ * This summary of the calibration process from Intel is informative:
+ * http://iotdk.intel.com/docs/master/upm/classupm_1_1_b_n_o055.html
+ *
+ * "This device requires calibration in order to operate accurately. [...] Calibration data is
+ * lost on a power cycle. See one of the examples for a description of how to calibrate the device,
+ * but in essence:
+ *
+ * There is a calibration status register available [...] that returns the calibration status
+ * of the accelerometer (ACC), magnetometer (MAG), gyroscope (GYR), and overall system (SYS).
+ * Each of these values range from 0 (uncalibrated) to 3 (fully calibrated). Calibration [ideally]
+ * involves certain motions to get all 4 values at 3. The motions are as follows (though see the
+ * datasheet for more information):
+ *
+ * 1. GYR: Simply let the sensor sit flat for a few seconds.
+ * 2. ACC: Move the sensor in various positions. Start flat, then rotate slowly by 45
+ * degrees, hold for a few seconds, then continue rotating another 45 degrees and
+ * hold, etc. 6 or more movements of this type may be required. You can move through
+ * any axis you desire, but make sure that the device is lying at least once
+ * perpendicular to the x, y, and z axis.
+ * 3. MAG: Move slowly in a figure 8 pattern in the air, until the calibration values reaches 3.
+ * 4. SYS: This will usually reach 3 when the other items have also reached 3. If not, continue
+ * slowly moving the device though various axes until it does."
+ *
+ * To calibrate the IMU, run this sample OpMode with a gamepad attached to the driver station.
+ * Once the IMU has reached sufficient calibration as reported on telemetry, press the 'A'
+ * button on the gamepad to write the calibration to a file. That file can then be indicated
+ * later when running an OpMode which uses the IMU.
+ *
+ * Note: if your intended uses of the IMU do not include use of all its sensors (for example,
+ * you might not use the magnetometer), then it makes little sense for you to wait for full
+ * calibration of the sensors you are not using before saving the calibration data. Indeed,
+ * it appears that in a SensorMode that doesn't use the magnetometer (for example), the
+ * magnetometer cannot actually be calibrated.
+ *
+ * References:
+ * The AdafruitBNO055IMU Javadoc
+ * The BNO055IMU.Parameters.calibrationDataFile Javadoc
+ * The BNO055 product page: https://www.bosch-sensortec.com/bst/products/all_products/bno055
+ * The BNO055 datasheet: https://www.bosch-sensortec.com/media/boschsensortec/downloads/datasheets/bst-bno055-ds000.pdf
+ */
+@TeleOp(name = "Sensor: BNO055 IMU Calibration", group = "Sensor")
+public class SensorBNO055IMUCalibration extends LinearOpMode
+ {
+ //----------------------------------------------------------------------------------------------
+ // State
+ //----------------------------------------------------------------------------------------------
+
+ // Our sensors, motors, and other devices go here, along with other long term state
+ BNO055IMU imu;
+
+ // State used for updating telemetry
+ Orientation angles;
+
+ //----------------------------------------------------------------------------------------------
+ // Main logic
+ //----------------------------------------------------------------------------------------------
+
+ @Override public void runOpMode() {
+
+ telemetry.log().setCapacity(12);
+ telemetry.log().add("");
+ telemetry.log().add("Please refer to the calibration instructions");
+ telemetry.log().add("contained in the Adafruit IMU calibration");
+ telemetry.log().add("sample OpMode.");
+ telemetry.log().add("");
+ telemetry.log().add("When sufficient calibration has been reached,");
+ telemetry.log().add("press the 'A' button to write the current");
+ telemetry.log().add("calibration data to a file.");
+ telemetry.log().add("");
+
+ // We are expecting the IMU to be attached to an I2C port on a Core Device Interface Module and named "imu".
+ BNO055IMU.Parameters parameters = new BNO055IMU.Parameters();
+ parameters.loggingEnabled = true;
+ parameters.loggingTag = "IMU";
+ imu = hardwareMap.get(BNO055IMU.class, "adafruit_imu");
+ imu.initialize(parameters);
+
+ composeTelemetry();
+ telemetry.log().add("Waiting for start...");
+
+ // Wait until we're told to go
+ while (!isStarted()) {
+ telemetry.update();
+ idle();
+ }
+
+ telemetry.log().add("...started...");
+
+ while (opModeIsActive()) {
+
+ if (gamepad1.a) {
+
+ // Get the calibration data
+ BNO055IMU.CalibrationData calibrationData = imu.readCalibrationData();
+
+ // Save the calibration data to a file. You can choose whatever file
+ // name you wish here, but you'll want to indicate the same file name
+ // when you initialize the IMU in an OpMode in which it is used. If you
+ // have more than one IMU on your robot, you'll of course want to use
+ // different configuration file names for each.
+ String filename = "AdafruitIMUCalibration.json";
+ File file = AppUtil.getInstance().getSettingsFile(filename);
+ ReadWriteFile.writeFile(file, calibrationData.serialize());
+ telemetry.log().add("saved to '%s'", filename);
+
+ // Wait for the button to be released
+ while (gamepad1.a) {
+ telemetry.update();
+ idle();
+ }
+ }
+
+ telemetry.update();
+ }
+ }
+
+ void composeTelemetry() {
+
+ // At the beginning of each telemetry update, grab a bunch of data
+ // from the IMU that we will then display in separate lines.
+ telemetry.addAction(new Runnable() { @Override public void run()
+ {
+ // Acquiring the angles is relatively expensive; we don't want
+ // to do that in each of the three items that need that info, as that's
+ // three times the necessary expense.
+ angles = imu.getAngularOrientation(AxesReference.INTRINSIC, AxesOrder.ZYX, AngleUnit.DEGREES);
+ }
+ });
+
+ telemetry.addLine()
+ .addData("status", new Func() {
+ @Override public String value() {
+ return imu.getSystemStatus().toShortString();
+ }
+ })
+ .addData("calib", new Func() {
+ @Override public String value() {
+ return imu.getCalibrationStatus().toString();
+ }
+ });
+
+ telemetry.addLine()
+ .addData("heading", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.firstAngle);
+ }
+ })
+ .addData("roll", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.secondAngle);
+ }
+ })
+ .addData("pitch", new Func() {
+ @Override public String value() {
+ return formatAngle(angles.angleUnit, angles.thirdAngle);
+ }
+ });
+ }
+
+ //----------------------------------------------------------------------------------------------
+ // Formatting
+ //----------------------------------------------------------------------------------------------
+
+ String formatAngle(AngleUnit angleUnit, double angle) {
+ return formatDegrees(AngleUnit.DEGREES.fromUnit(angleUnit, angle));
+ }
+
+ String formatDegrees(double degrees){
+ return String.format(Locale.getDefault(), "%.1f", AngleUnit.DEGREES.normalize(degrees));
+ }
+}