This repository has been archived by the owner on Oct 27, 2020. It is now read-only.
-
Notifications
You must be signed in to change notification settings - Fork 3.2k
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
- Loading branch information
Showing
29 changed files
with
1,470 additions
and
130 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
187 changes: 187 additions & 0 deletions
187
...src/main/java/org/firstinspires/ftc/robotcontroller/external/samples/SensorBNO055IMU.java
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 1,187 @@ | ||
| /* | ||
| Copyright (c) 2016 Robert Atkinson | ||
| All rights reserved. | ||
| Redistribution and use in source and binary forms, with or without modification, | ||
| are permitted (subject to the limitations in the disclaimer below) provided that | ||
| the following conditions are met: | ||
| Redistributions of source code must retain the above copyright notice, this list | ||
| of conditions and the following disclaimer. | ||
| Redistributions in binary form must reproduce the above copyright notice, this | ||
| list of conditions and the following disclaimer in the documentation and/or | ||
| other materials provided with the distribution. | ||
| Neither the name of Robert Atkinson nor the names of his contributors may be used to | ||
| endorse or promote products derived from this software without specific prior | ||
| written permission. | ||
| NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS | ||
| LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS | ||
| "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, | ||
| THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESSFOR 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.robotcontroller.external.samples; | ||
|
|
||
| import com.qualcomm.hardware.bosch.BNO055IMU; | ||
| import com.qualcomm.hardware.bosch.JustLoggingAccelerationIntegrator; | ||
| import com.qualcomm.robotcore.eventloop.opmode.Autonomous; | ||
| import com.qualcomm.robotcore.eventloop.opmode.Disabled; | ||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
|
|
||
| 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; | ||
|
|
||
| /** | ||
| * {@link SensorBNO055IMU} gives a short demo on how to use the BNO055 Inertial Motion Unit (IMU) from AdaFruit. | ||
| * | ||
| * 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 <a href="http://www.adafruit.com/products/2472">Adafruit IMU</a> | ||
| */ | ||
| @Autonomous(name = "Sensor: BNO055 IMU", group = "Sensor") | ||
| @Disabled // Comment this out to add to the opmode list | ||
| 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, "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<String>() { | ||
| @Override public String value() { | ||
| return imu.getSystemStatus().toShortString(); | ||
| } | ||
| }) | ||
| .addData("calib", new Func<String>() { | ||
| @Override public String value() { | ||
| return imu.getCalibrationStatus().toString(); | ||
| } | ||
| }); | ||
|
|
||
| telemetry.addLine() | ||
| .addData("heading", new Func<String>() { | ||
| @Override public String value() { | ||
| return formatAngle(angles.angleUnit, angles.firstAngle); | ||
| } | ||
| }) | ||
| .addData("roll", new Func<String>() { | ||
| @Override public String value() { | ||
| return formatAngle(angles.angleUnit, angles.secondAngle); | ||
| } | ||
| }) | ||
| .addData("pitch", new Func<String>() { | ||
| @Override public String value() { | ||
| return formatAngle(angles.angleUnit, angles.thirdAngle); | ||
| } | ||
| }); | ||
|
|
||
| telemetry.addLine() | ||
| .addData("grvty", new Func<String>() { | ||
| @Override public String value() { | ||
| return gravity.toString(); | ||
| } | ||
| }) | ||
| .addData("mag", new Func<String>() { | ||
| @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)); | ||
| } | ||
| } |
Oops, something went wrong.