Skip to content

Commit

Permalink
Update library, add initial draft of PinpointDrive and example class
Browse files Browse the repository at this point in the history
  • Loading branch information
@j5155
j5155 committed Sep 20, 2024
1 parent 5874659 commit 7bab5bf
Show file tree
Hide file tree
Showing 3 changed files with 315 additions and 1 deletion.
2 changes: 1 addition & 1 deletion TeamCode/build.gradle
View file
Original file line number Diff line number Diff line change
Expand Up @@ -38,5 +38,5 @@ dependencies {
implementation "com.acmerobotics.roadrunner:core:1.0.0"
implementation "com.acmerobotics.roadrunner:actions:1.0.0"
implementation "com.acmerobotics.dashboard:dashboard:0.4.16"
implementation "com.github.jdhs-ftc:road-runner-ftc-otos:a02ba3fe51"
implementation "com.github.jdhs-ftc:road-runner-ftc-otos:14c7f4ca9a"
}
122 changes: 122 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/PinpointDrive.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,122 @@
package org.firstinspires.ftc.teamcode;



import static com.acmerobotics.roadrunner.ftc.OTOSKt.OTOSPoseToRRPose;
import static com.acmerobotics.roadrunner.ftc.OTOSKt.RRPoseToOTOSPose;

import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.PoseVelocity2d;
import com.acmerobotics.roadrunner.Vector2d;
import com.acmerobotics.roadrunner.ftc.FlightRecorder;
import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
import com.qualcomm.hardware.sparkfun.SparkFunOTOS;
import com.qualcomm.robotcore.hardware.HardwareMap;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;
import org.firstinspires.ftc.teamcode.messages.PoseMessage;

/**
* Experimental extension of MecanumDrive that uses the Gobilda Pinpoint sensor for localization.
* <p>
* Released under the BSD 3-Clause Clear License by j5155 from 12087 Capital City Dynamics
* Portions of this code made and released under the MIT License by Gobilda (Base 10 Assets, LLC)
* Unless otherwise noted, comments are from Gobilda
*/
public class PinpointDrive extends MecanumDrive {
public static class Params {
/*
Set the odometry pod positions relative to the point that the odometry computer tracks around.
The X pod offset refers to how far sideways from the tracking point the
X (forward) odometry pod is. Left of the center is a positive number,
right of center is a negative number. the Y pod offset refers to how far forwards from
the tracking point the Y (strafe) odometry pod is. forward of center is a positive number,
backwards is a negative number.
*/
//these are tuned for 3110-0002-0001 Product Insight #1
// RR localizer note: These units are inches, presets are converted from mm (which is why they are inexact)
public double xOffset = -3.3071;
public double yOffset = -6.6142;

/*
Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
If you're using another kind of odometry pod, input the number of ticks per millimeter of that pod.
RR LOCALIZER NOTE: this is ticks per MILLIMETER, NOT inches per tick.
This value should be more than one; the value for the Gobilda 4 Bar Pod is approximately 20.
To get this value from inPerTick, first convert the value to millimeters (multiply by 25.4)
and then take its inverse (one over the value)
*/
public double encoderResolution = GoBildaPinpointDriver.goBILDA_4_BAR_POD;

/*
Set the direction that each of the two odometry pods count. The X (forward) pod should
increase when you move the robot forward. And the Y (strafe) pod should increase when
you move the robot to the left.
*/
public GoBildaPinpointDriver.EncoderDirection xDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
public GoBildaPinpointDriver.EncoderDirection yDirection = GoBildaPinpointDriver.EncoderDirection.FORWARD;
}

public static Params PARAMS = new Params();
public GoBildaPinpointDriver pinpoint;
private Pose2d lastPinpointPose = pose;

public PinpointDrive(HardwareMap hardwareMap, Pose2d pose) {
super(hardwareMap, pose);
pinpoint = hardwareMap.get(GoBildaPinpointDriver.class,"pinpoint");

// RR localizer note: don't love this conversion (change driver?)
pinpoint.setOffsets(DistanceUnit.MM.fromInches(PARAMS.xOffset), DistanceUnit.MM.fromInches(PARAMS.yOffset));


pinpoint.setEncoderResolution(PARAMS.encoderResolution);

/*
Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
The IMU will automatically calibrate when first powered on, but recalibrating before running
the robot is a good idea to ensure that the calibration is "good".
resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
This is recommended before you run your autonomous, as a bad initial calibration can cause
an incorrect starting value for x, y, and heading.
*/
//pinpoint.recalibrateIMU();
pinpoint.resetPosAndIMU();

pinpoint.setPosition(pose);
}
@Override
public PoseVelocity2d updatePoseEstimate() {
if (lastPinpointPose != pose) {
// RR localizer note:
// Something else is modifying our pose (likely for relocalization),
// so we override otos pose with the new pose.
// This could potentially cause up to 1 loop worth of drift.
// I don't like this solution at all, but it preserves compatibility.
// The only alternative is to add getter and setters, but that breaks compat.
// Potential alternate solution: timestamp the pose set and backtrack it based on speed?
pinpoint.setPosition(pose);
}
pinpoint.bulkUpdate(); // RR LOCALIZER NOTE: this is not ideal for loop times.
// Driver needs update to be optimized
pose = pinpoint.getPosition();
lastPinpointPose = pose;

// RR standard
poseHistory.add(pose);
while (poseHistory.size() > 100) {
poseHistory.removeFirst();
}

FlightRecorder.write("ESTIMATED_POSE", new PoseMessage(pose));

Pose2d velocity = pinpoint.getVelocity();
// RR localizer note
// "velocity.position" is a little weird
// it's because velocity is stored as a roadrunner pose2d right now
// maybe not ideal for this, probably I should change it to use a PoseVelocity2d directly instead
return new PoseVelocity2d(new Vector2d(velocity.position.x, velocity.position.y),velocity.heading.toDouble());
}


}
192 changes: 192 additions & 0 deletions TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,192 @@
/* MIT License
* Copyright (c) [2024] [Base 10 Assets, LLC]
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/

package org.firstinspires.ftc.teamcode;

import com.acmerobotics.roadrunner.Pose2d;
import com.acmerobotics.roadrunner.ftc.GoBildaPinpointDriver;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;

import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit;
import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit;

import java.util.Locale;

/*
This opmode shows how to use the goBILDA® Pinpoint Odometry Computer.
The goBILDA Odometry Computer is a device designed to solve the Pose Exponential calculation
commonly associated with Dead Wheel Odometry systems. It reads two encoders, and an integrated
system of senors to determine the robot's current heading, X position, and Y position.
it uses an ESP32-S3 as a main cpu, with an STM LSM6DSV16X IMU.
It is validated with goBILDA "Dead Wheel" Odometry pods, but should be compatible with any
quadrature rotary encoder. The ESP32 PCNT peripheral is speced to decode quadrature encoder signals
at a maximum of 40mhz per channel. Though the maximum in-application tested number is 130khz.
The device expects two perpendicularly mounted Dead Wheel pods. The encoder pulses are translated
into mm and their readings are transformed by an "offset", this offset describes how far away
the pods are from the "tracking point", usually the center of rotation of the robot.
Dead Wheel pods should both increase in count when moved forwards and to the left.
The gyro will report an increase in heading when rotated counterclockwise.
The Pose Exponential algorithm used is described on pg 181 of this book:
https://github.com/calcmogul/controls-engineering-in-frc
For support, contact [email protected]
-Ethan Doak
*/

@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode")
//@Disabled
public class SensorGoBildaPinpointExample extends LinearOpMode {

GoBildaPinpointDriver odo; // Declare OpMode member for the Odometry Computer

double oldTime = 0;


@Override
public void runOpMode() {

// Initialize the hardware variables. Note that the strings used here must correspond
// to the names assigned during the robot configuration step on the DS or RC devices.

odo = hardwareMap.get(GoBildaPinpointDriver.class,"odo");

/*
Set the odometry pod positions relative to the point that the odometry computer tracks around.
The X pod offset refers to how far sideways from the tracking point the
X (forward) odometry pod is. Left of the center is a positive number,
right of center is a negative number. the Y pod offset refers to how far forwards from
the tracking point the Y (strafe) odometry pod is. forward of center is a positive number,
backwards is a negative number.
*/
odo.setOffsets(-84.0, -168.0); //these are tuned for 3110-0002-0001 Product Insight #1

/*
Set the kind of pods used by your robot. If you're using goBILDA odometry pods, select either
the goBILDA_SWINGARM_POD, or the goBILDA_4_BAR_POD.
If you're using another kind of odometry pod, uncomment setEncoderResolution and input the
number of ticks per mm of your odometry pod.
*/
odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD);
//odo.setEncoderResolution(13.26291192);


/*
Set the direction that each of the two odometry pods count. The X (forward) pod should
increase when you move the robot forward. And the Y (strafe) pod should increase when
you move the robot to the left.
*/
odo.setEncoderDirections(GoBildaPinpointDriver.EncoderDirection.FORWARD, GoBildaPinpointDriver.EncoderDirection.FORWARD);


/*
Before running the robot, recalibrate the IMU. This needs to happen when the robot is stationary
The IMU will automatically calibrate when first powered on, but recalibrating before running
the robot is a good idea to ensure that the calibration is "good".
resetPosAndIMU will reset the position to 0,0,0 and also recalibrate the IMU.
This is recommended before you run your autonomous, as a bad initial calibration can cause
an incorrect starting value for x, y, and heading.
*/
//odo.recalibrateIMU();
odo.resetPosAndIMU();

telemetry.addData("Status", "Initialized");
telemetry.addData("X offset", odo.getXOffset());
telemetry.addData("Y offset", odo.getYOffset());
telemetry.addData("Device Version Number:", odo.getDeviceVersion());
telemetry.addData("Device SCalar", odo.getYawScalar());
telemetry.update();

// Wait for the game to start (driver presses START)
waitForStart();
resetRuntime();


// run until the end of the match (driver presses STOP)
while (opModeIsActive()) {

/*
Request a bulk update from the Pinpoint odometry computer. This checks almost all outputs
from the device in a single I2C read.
*/
odo.bulkUpdate();


if (gamepad1.a){
odo.resetPosAndIMU(); //resets the position to 0 and recalibrates the IMU
}

if (gamepad1.b){
odo.recalibrateIMU(); //recalibrates the IMU without resetting position
}

/*
This code prints the loop frequency of the REV Control Hub. This frequency is effected
by I2C reads/writes. So it's good to keep an eye on. This code calculates the amount
of time each cycle takes and finds the frequency (number of updates per second) from
that cycle time.
*/
double newTime = getRuntime();
double loopTime = newTime-oldTime;
double frequency = 1/loopTime;
oldTime = newTime;


/*
gets the current Position (x & y in mm, and heading in degrees) of the robot, and prints it.
*/
Pose2d pos = odo.getPosition();
String data = String.format(Locale.US, "{X: %.3f, Y: %.3f, H: %.3f}", pos.position.x, pos.position.y, pos.heading.toDouble());
telemetry.addData("Position", data);


/*
gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and prints it.
*/
Pose2d vel = odo.getVelocity();
String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", vel.position.x, vel.position.y, vel.heading.toDouble());
telemetry.addData("Velocity", velocity);

telemetry.addData("X Encoder:", odo.getEncoderX()); //gets the raw data from the X encoder
telemetry.addData("Y Encoder:",odo.getEncoderY()); //gets the raw data from the Y encoder
telemetry.addData("Pinpoint Frequency", odo.getFrequency()); //prints/gets the current refresh rate of the Pinpoint

/*
Gets the Pinpoint device status. Pinpoint can reflect a few states. But we'll primarily see
READY: the device is working as normal
CALIBRATING: the device is calibrating and outputs are put on hold
NOT_READY: the device is resetting from scratch. This should only happen after a power-cycle
FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in
FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in
FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in
*/
telemetry.addData("Status", odo.getDeviceStatus());

telemetry.addData("REV Hub Frequency: ", frequency); //prints the control system refresh rate
telemetry.update();
}
}}

0 comments on commit 7bab5bf

Please sign in to comment.