forked from FIRST-Tech-Challenge/FtcRobotController
-
Notifications
You must be signed in to change notification settings - Fork 0
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
5 changed files
with
218 additions
and
4 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
151 changes: 151 additions & 0 deletions
151
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/OTOSOpMode.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,151 @@ | ||
| /* | ||
| SPDX-License-Identifier: MIT | ||
| Copyright (c) 2024 SparkFun Electronics | ||
| */ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| import com.acmerobotics.roadrunner.ftc.SparkFunOTOS; | ||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
| import com.qualcomm.robotcore.eventloop.opmode.TeleOp; | ||
|
|
||
| /* | ||
| * This OpMode illustrates how to use the SparkFun Qwiic Optical Tracking Odometry Sensor (OTOS) | ||
| * | ||
| * The OpMode assumes that the sensor is configured with a name of "sensor_otos". | ||
| * | ||
| * 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 the sensor's product page: https://www.sparkfun.com/products/24904 | ||
| */ | ||
| @TeleOp(name = "Sensor: SparkFun OTOS", group = "Sensor") | ||
| //@Disabled | ||
| public class OTOSOpMode extends LinearOpMode { | ||
| // Create an instance of the sensor | ||
| SparkFunOTOS myOtos; | ||
|
|
||
| @Override | ||
| public void runOpMode() throws InterruptedException { | ||
| // Get a reference to the sensor | ||
| myOtos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos"); | ||
|
|
||
| // All the configuration for the OTOS is done in this helper method, check it out! | ||
| configureOtos(); | ||
|
|
||
| // Wait for the start button to be pressed | ||
| waitForStart(); | ||
|
|
||
| // Loop until the OpMode ends | ||
| while (opModeIsActive()) { | ||
| // Get the latest position, which includes the x and y coordinates, plus the | ||
| // heading angle | ||
| SparkFunOTOS.Pose2D pos = myOtos.getPosition(); | ||
|
|
||
| // Reset the tracking if the user requests it | ||
| if (gamepad1.y) { | ||
| myOtos.resetTracking(); | ||
| } | ||
|
|
||
| // Re-calibrate the IMU if the user requests it | ||
| if (gamepad1.x) { | ||
| myOtos.calibrateImu(); | ||
| } | ||
|
|
||
| // Inform user of available controls | ||
| telemetry.addLine("Press Y (triangle) on Gamepad to reset tracking"); | ||
| telemetry.addLine("Press X (square) on Gamepad to calibrate the IMU"); | ||
| telemetry.addLine(); | ||
|
|
||
| // Log the position to the telemetry | ||
| telemetry.addData("X coordinate", pos.x); | ||
| telemetry.addData("Y coordinate", pos.y); | ||
| telemetry.addData("Heading angle", pos.h); | ||
|
|
||
| // Update the telemetry on the driver station | ||
| telemetry.update(); | ||
| } | ||
| } | ||
|
|
||
| private void configureOtos() { | ||
| telemetry.addLine("Configuring OTOS..."); | ||
| telemetry.update(); | ||
|
|
||
| // Set the desired units for linear and angular measurements. Can be either | ||
| // meters or inches for linear, and radians or degrees for angular. If not | ||
| // set, the default is inches and degrees. Note that this setting is not | ||
| // stored in the sensor, it's part of the library, so you need to set at the | ||
| // start of all your programs. | ||
| // myOtos.setLinearUnit(SparkFunOTOS.LinearUnit.METERS); | ||
| myOtos.setLinearUnit(SparkFunOTOS.LinearUnit.INCHES); | ||
| // myOtos.setAngularUnit(SparkFunOTOS.AngularUnit.RADIANS); | ||
| myOtos.setAngularUnit(SparkFunOTOS.AngularUnit.DEGREES); | ||
|
|
||
| // Assuming you've mounted your sensor to a robot and it's not centered, | ||
| // you can specify the offset for the sensor relative to the center of the | ||
| // robot. The units default to inches and degrees, but if you want to use | ||
| // different units, specify them before setting the offset! Note that as of | ||
| // firmware version 1.0, these values will be lost after a power cycle, so | ||
| // you will need to set them each time you power up the sensor. For example, if | ||
| // the sensor is mounted 5 inches to the left (negative X) and 10 inches | ||
| // forward (positive Y) of the center of the robot, and mounted 90 degrees | ||
| // clockwise (negative rotation) from the robot's orientation, the offset | ||
| // would be {-5, 10, -90}. These can be any value, even the angle can be | ||
| // tweaked slightly to compensate for imperfect mounting (eg. 1.3 degrees). | ||
| SparkFunOTOS.Pose2D offset = new SparkFunOTOS.Pose2D(0, 0, 180); | ||
| myOtos.setOffset(offset); | ||
|
|
||
| // Here we can set the linear and angular scalars, which can compensate for | ||
| // scaling issues with the sensor measurements. Note that as of firmware | ||
| // version 1.0, these values will be lost after a power cycle, so you will | ||
| // need to set them each time you power up the sensor. They can be any value | ||
| // from 0.872 to 1.127 in increments of 0.001 (0.1%). It is recommended to | ||
| // first set both scalars to 1.0, then calibrate the angular scalar, then | ||
| // the linear scalar. To calibrate the angular scalar, spin the robot by | ||
| // multiple rotations (eg. 10) to get a precise error, then set the scalar | ||
| // to the inverse of the error. Remember that the angle wraps from -180 to | ||
| // 180 degrees, so for example, if after 10 rotations counterclockwise | ||
| // (positive rotation), the sensor reports -15 degrees, the required scalar | ||
| // would be 3600/3585 = 1.004. To calibrate the linear scalar, move the | ||
| // robot a known distance and measure the error; do this multiple times at | ||
| // multiple speeds to get an average, then set the linear scalar to the | ||
| // inverse of the error. For example, if you move the robot 100 inches and | ||
| // the sensor reports 103 inches, set the linear scalar to 100/103 = 0.971 | ||
| myOtos.setLinearScalar(1.0); | ||
| myOtos.setAngularScalar(1.0); | ||
|
|
||
| // The IMU on the OTOS includes a gyroscope and accelerometer, which could | ||
| // have an offset. Note that as of firmware version 1.0, the calibration | ||
| // will be lost after a power cycle; the OTOS performs a quick calibration | ||
| // when it powers up, but it is recommended to perform a more thorough | ||
| // calibration at the start of all your programs. Note that the sensor must | ||
| // be completely stationary and flat during calibration! When calling | ||
| // calibrateImu(), you can specify the number of samples to take and whether | ||
| // to wait until the calibration is complete. If no parameters are provided, | ||
| // it will take 255 samples and wait until done; each sample takes about | ||
| // 2.4ms, so about 612ms total | ||
| myOtos.calibrateImu(); | ||
|
|
||
| // Reset the tracking algorithm - this resets the position to the origin, | ||
| // but can also be used to recover from some rare tracking errors | ||
| myOtos.resetTracking(); | ||
|
|
||
| // After resetting the tracking, the OTOS will report that the robot is at | ||
| // the origin. If your robot does not start at the origin, or you have | ||
| // another source of location information (eg. vision odometry), you can set | ||
| // the OTOS location to match and it will continue to track from there. | ||
| SparkFunOTOS.Pose2D currentPosition = new SparkFunOTOS.Pose2D(0, 0, 0); | ||
| myOtos.setPosition(currentPosition); | ||
|
|
||
| // Get the hardware and firmware version | ||
| SparkFunOTOS.Version hwVersion = new SparkFunOTOS.Version(); | ||
| SparkFunOTOS.Version fwVersion = new SparkFunOTOS.Version(); | ||
| myOtos.getVersionInfo(hwVersion, fwVersion); | ||
|
|
||
| telemetry.addLine("OTOS configured! Press start to get position data!"); | ||
| telemetry.addLine(); | ||
| telemetry.addLine(String.format("OTOS Hardware Version: v%d.%d", hwVersion.major, hwVersion.minor)); | ||
| telemetry.addLine(String.format("OTOS Firmware Version: v%d.%d", fwVersion.major, fwVersion.minor)); | ||
| telemetry.update(); | ||
| } | ||
| } |
50 changes: 50 additions & 0 deletions
50
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/RROtosTest.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,50 @@ | ||
| package org.firstinspires.ftc.teamcode; | ||
|
|
||
| import com.acmerobotics.dashboard.FtcDashboard; | ||
| import com.acmerobotics.dashboard.telemetry.MultipleTelemetry; | ||
| import com.acmerobotics.dashboard.telemetry.TelemetryPacket; | ||
| import com.acmerobotics.roadrunner.Pose2d; | ||
| import com.acmerobotics.roadrunner.PoseVelocity2d; | ||
| import com.acmerobotics.roadrunner.Vector2d; | ||
| import com.acmerobotics.roadrunner.ftc.SparkFunOTOS; | ||
| import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; | ||
|
|
||
| public class RROtosTest extends LinearOpMode { | ||
| @Override | ||
| public void runOpMode() throws InterruptedException { | ||
| telemetry = new MultipleTelemetry(telemetry, FtcDashboard.getInstance().getTelemetry()); | ||
|
|
||
| SparkFunOTOSDrive drive = new SparkFunOTOSDrive(hardwareMap, new Pose2d(0, 0, 0)); | ||
|
|
||
| waitForStart(); | ||
|
|
||
| while (opModeIsActive()) { | ||
| drive.setDrivePowers(new PoseVelocity2d( | ||
| new Vector2d( | ||
| -gamepad1.left_stick_y, | ||
| -gamepad1.left_stick_x | ||
| ), | ||
| -gamepad1.right_stick_x | ||
| )); | ||
|
|
||
| drive.updatePoseEstimate(); | ||
|
|
||
| telemetry.addData("x", drive.pose.position.x); | ||
| telemetry.addData("y", drive.pose.position.y); | ||
| telemetry.addData("heading (deg)", Math.toDegrees(drive.pose.heading.toDouble())); | ||
|
|
||
| SparkFunOTOS.Pose2D oPose = drive.otos.getPosition(); | ||
| telemetry.addData("oX",oPose.x); | ||
| telemetry.addData("oY", oPose.y); | ||
| telemetry.addData("oH", oPose.h); | ||
| telemetry.addData("oH (deg)", Math.toDegrees(oPose.h)); | ||
|
|
||
| telemetry.update(); | ||
|
|
||
| TelemetryPacket packet = new TelemetryPacket(); | ||
| packet.fieldOverlay().setStroke("#3F51B5"); | ||
| Drawing.drawRobot(packet.fieldOverlay(), drive.pose); | ||
| FtcDashboard.getInstance().sendTelemetryPacket(packet); | ||
| } | ||
| } | ||
| } |
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