From 7bf8b0c3576269c0d5ee21c474f87880a779c19f Mon Sep 17 00:00:00 2001 From: carlos Date: Thu, 17 Oct 2024 11:03:33 -0700 Subject: [PATCH 1/4] Add files --- TeamCode/src/main/java/bBlueAutoV1.java | 236 ++++++++++++++++++ .../src/main/java/nbRedAutoV1/bRedAutoV1.java | 174 +++++++++++++ .../ftc/teamcode/SensorBNO055IMU.java | 186 ++++++++++++++ .../teamcode/SensorBNO055IMUCalibration.java | 230 +++++++++++++++++ 4 files changed, 826 insertions(+) create mode 100644 TeamCode/src/main/java/bBlueAutoV1.java create mode 100644 TeamCode/src/main/java/nbRedAutoV1/bRedAutoV1.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java 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)); + } +} From c1076a832c4d383e87939ad11df46018462ccebf Mon Sep 17 00:00:00 2001 From: carlos Date: Thu, 17 Oct 2024 11:05:12 -0700 Subject: [PATCH 2/4] Rebased changes --- TeamCode/src/main/java/GeneratedPath.java | 107 ++++++++++++++++++ TeamCode/src/main/java/nbRedAutoV1/Java.java | 8 ++ .../ftc/teamcode/PedroConstants.java | 10 +- .../ftc/teamcode/pedroPathing/TUNING.md | 3 +- .../tuning/FollowerConstants.java | 2 +- 5 files changed, 123 insertions(+), 7 deletions(-) create mode 100644 TeamCode/src/main/java/GeneratedPath.java create mode 100644 TeamCode/src/main/java/nbRedAutoV1/Java.java diff --git a/TeamCode/src/main/java/GeneratedPath.java b/TeamCode/src/main/java/GeneratedPath.java new file mode 100644 index 0000000..301a70a --- /dev/null +++ b/TeamCode/src/main/java/GeneratedPath.java @@ -0,0 +1,107 @@ +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 GeneratedPath { + public GeneratedPath() { + PathBuilder builder = new PathBuilder(); + + builder + .addPath( + // Line 1 + new BezierLine( + new Point(9.757, 84.983, Point.CARTESIAN), + new Point(28.573, 76.302, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierLine( + new Point(28.573, 76.302, Point.CARTESIAN), + new Point(36.203, 76.140, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 3 + new BezierLine( + new Point(36.203, 76.140, Point.CARTESIAN), + new Point(35.067, 35.716, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 4 + new BezierLine( + new Point(35.067, 35.716, Point.CARTESIAN), + new Point(73.705, 34.742, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 5 + new BezierLine( + new Point(73.705, 34.742, Point.CARTESIAN), + new Point(73.705, 24.839, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 6 + new BezierLine( + new Point(73.705, 24.839, Point.CARTESIAN), + new Point(7.630, 26.462, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 7 + new BezierLine( + new Point(7.630, 26.462, Point.CARTESIAN), + new Point(64.126, 22.728, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 8 + new BezierLine( + new Point(64.126, 22.728, Point.CARTESIAN), + new Point(63.964, 13.150, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 9 + new BezierLine( + new Point(63.964, 13.150, Point.CARTESIAN), + new Point(12.338, 15.260, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 10 + new BezierLine( + new Point(12.338, 15.260, Point.CARTESIAN), + new Point(63.802, 13.150, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 11 + new BezierLine( + new Point(63.802, 13.150, Point.CARTESIAN), + new Point(63.639, 11.689, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 12 + new BezierLine( + new Point(63.639, 11.689, Point.CARTESIAN), + new Point(12.014, 11.689, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + } +} diff --git a/TeamCode/src/main/java/nbRedAutoV1/Java.java b/TeamCode/src/main/java/nbRedAutoV1/Java.java new file mode 100644 index 0000000..c8df1e0 --- /dev/null +++ b/TeamCode/src/main/java/nbRedAutoV1/Java.java @@ -0,0 +1,8 @@ +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 Java { +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java index bbef593..6ed40c0 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PedroConstants.java @@ -56,19 +56,19 @@ public class PedroConstants { public static final double ROBOT_WEIGHT_IN_KG = 10.5; // Maximum velocity of the robot going forward - public static final double ROBOT_SPEED_FORWARD = 72.0693; + public static final double ROBOT_SPEED_FORWARD = 79.0257; // Maximum velocity of the robot going right - public static final double ROBOT_SPEED_LATERAL = 24.1401; + public static final double ROBOT_SPEED_LATERAL = 12.3941; // Rate of deceleration when power is cut-off when the robot is moving forward - public static final double FORWARD_ZERO_POWER_ACCEL = -74.3779; + public static final double FORWARD_ZERO_POWER_ACCEL = -50.7945; // Rate of deceleration when power is cut-off when the robot is moving to the right - public static final double LATERAL_ZERO_POWER_ACCEL = -111.8409; + public static final double LATERAL_ZERO_POWER_ACCEL = -92.733; // Determines how fast your robot will decelerate as a factor of how fast your robot will coast to a stop - public static final double ZERO_POWER_ACCEL_MULT = 4; + public static final double ZERO_POWER_ACCEL_MULT = 2.5; /* Centripetal force correction - increase if robot is correcting into the path - decrease if robot is correcting away from the path */ diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md index bd22f46..c7fa481 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/TUNING.md @@ -71,7 +71,8 @@ measurements will be in centimeters. of how fast your robot will coast to a stop. Honestly, this is up to you. I personally used 4, but what works best for you is most important. Higher numbers will cause a faster brake, but increase oscillations at the end. Lower numbers will do the opposite. This can be found on line `107` in - `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more sensitive than the others. For reference, + `FollowerConstants`, named `zeroPowerAccelerationMultiplier`. The drive PID is much, much more +* sensitive than the others. For reference, my P values were in the hundredths and thousandths place values, and my D values were in the hundred thousandths and millionths place values. To tune this, enable `useDrive`, `useHeading`, and `useTranslational` in the `Follower` dropdown in FTC Dashboard. Next, run `StraightBackAndForth` diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java index ba53959..4830913 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/tuning/FollowerConstants.java @@ -40,7 +40,7 @@ public class FollowerConstants { // Translational PIDF coefficients (don't use integral) public static CustomPIDFCoefficients translationalPIDFCoefficients = new CustomPIDFCoefficients( - 0.06, + 0.1, 0, 0, 0); From 552bb3e25af3a6607a46288c982d612414d3387c Mon Sep 17 00:00:00 2001 From: robotics1 Date: Thu, 17 Oct 2024 11:06:13 -0700 Subject: [PATCH 3/4] Add new files --- .../firstinspires/ftc/teamcode/AutoTest.java | 91 +++++++++++++++++ .../firstinspires/ftc/teamcode/RedAuto.java | 99 +++++++++++++++++++ 2 files changed, 190 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java new file mode 100644 index 0000000..7f5fea7 --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoTest.java @@ -0,0 +1,91 @@ +package org.firstinspires.ftc.teamcode; + +import com.acmerobotics.dashboard.FtcDashboard; +import com.acmerobotics.dashboard.config.Config; +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; +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.BezierCurve; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.BezierLine; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.PathChain; +import org.firstinspires.ftc.teamcode.pedroPathing.pathGeneration.Point; + +/** + * This is the Circle autonomous OpMode. It runs the robot in a PathChain that's actually not quite + * a circle, but some Bezier curves that have control points set essentially in a square. However, + * it turns enough to tune your centripetal force correction and some of your heading. Some lag in + * heading is to be expected. + * + * @author Anyi Lin - 10158 Scott's Bots + * @author Aaron Yang - 10158 Scott's Bots + * @author Harrison Womack - 10158 Scott's Bots + * @version 1.0, 3/12/2024 + */ +@Config +@Autonomous (name = "Test", group = "Autonomous Pathing Tuning") +public class AutoTest extends OpMode { + private Telemetry telemetryA; + + + private Follower follower; + + private PathChain test; + + @Override + public void init() { + follower = new Follower(hardwareMap); + + test = follower.pathBuilder() + .addPath( + new BezierLine( + new Point(8.000, 60.000, Point.CARTESIAN), + new Point(18.000, 60.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 2 + new BezierCurve( + new Point(18.000, 60.000, Point.CARTESIAN), + new Point(18.000, 23.000, Point.CARTESIAN), + new Point(48.000, 23.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 3 + new BezierLine( + new Point(48.000, 23.000, Point.CARTESIAN), + new Point(60.000, 36.000, Point.CARTESIAN) + ) + ) + + .addPath( + // Line 4 + new BezierLine( + new Point(60.000, 36.000, Point.CARTESIAN), + new Point(60.000, 49.000, Point.CARTESIAN) + ) + ).build(); + + + follower.followPath(test); + + telemetryA = new MultipleTelemetry(this.telemetry, FtcDashboard.getInstance().getTelemetry()); + telemetryA.update(); + } + + + @Override + public void loop() { + follower.update(); + if (follower.atParametricEnd()) { + follower.followPath(test); + } + + follower.telemetryDebug(telemetryA); + } +} diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java new file mode 100644 index 0000000..ef7ea4f --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RedAuto.java @@ -0,0 +1,99 @@ +package org.firstinspires.ftc.teamcode; + + + +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 RedAuto { + + + public class GeneratedPath { + + public GeneratedPath() { + PathBuilder builder = new PathBuilder(); + + builder + .addPath( + // Line 1 + new BezierLine( + new Point(131.499, 58.931, Point.CARTESIAN), + new Point(131.986, 18.183, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierLine( + new Point(131.986, 18.183, Point.CARTESIAN), + new Point(90.264, 40.911, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 3 + new BezierLine( + new Point(90.264, 40.911, Point.CARTESIAN), + new Point(83.445, 26.300, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 4 + new BezierLine( + new Point(83.445, 26.300, Point.CARTESIAN), + new Point(136.207, 14.286, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 5 + new BezierLine( + new Point(136.207, 14.286, Point.CARTESIAN), + new Point(81.497, 24.352, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 6 + new BezierLine( + new Point(81.497, 24.352, Point.CARTESIAN), + new Point(82.634, 12.988, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 7 + new BezierLine( + new Point(82.634, 12.988, Point.CARTESIAN), + new Point(133.935, 11.364, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 8 + new BezierLine( + new Point(133.935, 11.364, Point.CARTESIAN), + new Point(82.309, 11.689, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 9 + new BezierLine( + new Point(82.309, 11.689, Point.CARTESIAN), + new Point(83.445, 2.598, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 10 + new BezierLine( + new Point(83.445, 2.598, Point.CARTESIAN), + new Point(132.149, 10.390, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + } + } +} From 50db1f91759b7dc58fda6fd7e45f7b375458317a Mon Sep 17 00:00:00 2001 From: robotics1 Date: Thu, 17 Oct 2024 11:07:10 -0700 Subject: [PATCH 4/4] Add new files (2nd try) --- .../firstinspires/ftc/teamcode/BlueAuto.java | 128 ++++++++++ .../ftc/teamcode/SensorBNO055IMU.java | 186 ++++++++++++++ .../teamcode/SensorBNO055IMUCalibration.java | 230 ++++++++++++++++++ 3 files changed, 544 insertions(+) create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMU.java create mode 100644 TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorBNO055IMUCalibration.java diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java new file mode 100644 index 0000000..1af2c8e --- /dev/null +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/BlueAuto.java @@ -0,0 +1,128 @@ +package org.firstinspires.ftc.teamcode; + + +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 BlueAuto { + + + + public void GeneratedPath() { + PathBuilder builder = new PathBuilder(); + + builder + .addPath( + // Line 1 + new BezierLine( + new Point(9.757, 84.983, Point.CARTESIAN), + new Point(8.442, 129.227, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 2 + new BezierLine( + new Point(8.442, 129.227, Point.CARTESIAN), + new Point(52.762, 101.628, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 3 + new BezierLine( + new Point(52.762, 101.628, Point.CARTESIAN), + new Point(79.224, 116.564, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 4 + new BezierLine( + new Point(79.224, 116.564, Point.CARTESIAN), + new Point(54.548, 130.525, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 5 + new BezierLine( + new Point(54.548, 130.525, Point.CARTESIAN), + new Point(12.338, 133.772, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 6 + new BezierLine( + new Point(12.338, 133.772, Point.CARTESIAN), + new Point(52.437, 101.628, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 7 + new BezierLine( + new Point(52.437, 101.628, Point.CARTESIAN), + new Point(71.594, 120.947, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 8 + new BezierLine( + new Point(71.594, 120.947, Point.CARTESIAN), + new Point(52.275, 120.785, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 9 + new BezierLine( + new Point(52.275, 120.785, Point.CARTESIAN), + new Point(11.039, 131.012, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 10 + new BezierLine( + new Point(11.039, 131.012, Point.CARTESIAN), + new Point(70.782, 120.460, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 11 + new BezierLine( + new Point(70.782, 120.460, Point.CARTESIAN), + new Point(50.327, 142.377, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 12 + new BezierLine( + new Point(50.327, 142.377, Point.CARTESIAN), + new Point(13.799, 134.422, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 13 + new BezierLine( + new Point(13.799, 134.422, Point.CARTESIAN), + new Point(13.799, 134.422, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation() + .addPath( + // Line 14 + new BezierLine( + new Point(13.799, 134.422, Point.CARTESIAN), + new Point(71.919, 103.901, Point.CARTESIAN) + ) + ) + .setTangentHeadingInterpolation(); + } +} \ No newline at end of file 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)); + } +}