From 4160ffc5363cc0659b741f7577527339c1f7c47b Mon Sep 17 00:00:00 2001 From: Ethan Doak Date: Tue, 18 Mar 2025 09:59:32 -0500 Subject: [PATCH 1/5] mec driving to points --- .../ftc/teamcode/GoBildaPinpointDriver.java | 224 ++++++++++++++++-- .../SensorGoBildaPinpointExample.java | 23 +- 2 files changed, 220 insertions(+), 27 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java index 2632b2a5818a..cc697e3ae250 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java @@ -35,6 +35,7 @@ import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; import java.nio.ByteBuffer; import java.nio.ByteOrder; @@ -128,7 +129,8 @@ public enum DeviceStatus{ FAULT_X_POD_NOT_DETECTED (1 << 2), FAULT_Y_POD_NOT_DETECTED (1 << 3), FAULT_NO_PODS_DETECTED (1 << 2 | 1 << 3), - FAULT_IMU_RUNAWAY (1 << 4); + FAULT_IMU_RUNAWAY (1 << 4), + FAULT_BAD_READ (1 << 5); private final int status; @@ -149,7 +151,7 @@ public enum GoBildaOdometryPods { goBILDA_4_BAR_POD; } //enum that captures a limited scope of read data. More options may be added in future update - public enum readData { + public enum ReadData { ONLY_UPDATE_HEADING, } @@ -246,15 +248,66 @@ private DeviceStatus lookupStatus (int s){ if ((s & DeviceStatus.READY.status) != 0){ return DeviceStatus.READY; } + if ((s & DeviceStatus.FAULT_BAD_READ.status) != 0){ + return DeviceStatus.FAULT_BAD_READ; + } else { return DeviceStatus.NOT_READY; } } + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the maximum change between this reading and the previous one + * @return newValue if the position is good, oldValue otherwise + */ + private Float isPositionCorrupt(float oldValue, float newValue, int threshold){ + boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + + /** + * Confirm that the number received is a number, and does not include a change above the threshold + * @param oldValue the reading from the previous cycle + * @param newValue the new reading + * @param threshold the velocity allowed to be reported + * @return newValue if the velocity is good, oldValue otherwise + */ + private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ + boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; + + if(!isCorrupt){ + return newValue; + } + + deviceStatus = DeviceStatus.FAULT_BAD_READ.status; + return oldValue; + } + /** * Call this once per loop to read new data from the Odometry Computer. Data will only update once this is called. */ public void update(){ + final int positionThreshold = 5000; //more than one FTC field in mm + final int headingThreshold = 120; //About 20 full rotations in Radians + final int velocityThreshold = 10000; //10k mm/sec is faster than an FTC robot should be going... + final int headingVelocityThreshold = 120; //About 20 rotations per second + + float oldPosX = xPosition; + float oldPosY = yPosition; + float oldPosH = hOrientation; + float oldVelX = xVelocity; + float oldVelY = yVelocity; + float oldVelH = hVelocity; + byte[] bArr = deviceClient.read(Register.BULK_READ.bVal, 40); deviceStatus = byteArrayToInt(Arrays.copyOfRange (bArr, 0, 4), ByteOrder.LITTLE_ENDIAN); loopTime = byteArrayToInt(Arrays.copyOfRange (bArr, 4, 8), ByteOrder.LITTLE_ENDIAN); @@ -266,17 +319,39 @@ public void update(){ xVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 28,32), ByteOrder.LITTLE_ENDIAN); yVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 32,36), ByteOrder.LITTLE_ENDIAN); hVelocity = byteArrayToFloat(Arrays.copyOfRange(bArr, 36,40), ByteOrder.LITTLE_ENDIAN); + + /* + * Check to see if any of the floats we have received from the device are NaN or are too large + * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. + */ + xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold); + yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold); + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold); + xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); + yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); + hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); + } /** * Call this once per loop to read new data from the Odometry Computer. This is an override of the update() function * which allows a narrower range of data to be read from the device for faster read times. Currently ONLY_UPDATE_HEADING * is supported. - * @param data GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING + * @param data GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING */ - public void update(readData data) { - if (data == readData.ONLY_UPDATE_HEADING) { + public void update(ReadData data) { + if (data == ReadData.ONLY_UPDATE_HEADING) { + final int headingThreshold = 120; + + float oldPosH = hOrientation; + hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); + + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold); + + if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ + deviceStatus = DeviceStatus.READY.status; + } } } @@ -287,12 +362,28 @@ public void update(readData data) { * the Y pod offset refers to how far forwards (in mm) from the tracking point the Y (strafe) odometry pod is. forward of center is a positive number, backwards is a negative number.
* @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ public void setOffsets(double xOffset, double yOffset){ writeFloat(Register.X_POD_OFFSET, (float) xOffset); writeFloat(Register.Y_POD_OFFSET, (float) yOffset); } + + /** + * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

+ * The most common tracking position is the center of the robot.

+ * 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.
+ * @param xOffset how sideways from the center of the robot is the X (forward) pod? Left increases + * @param yOffset how far forward from the center of the robot is the Y (Strafe) pod? forward increases + * @param distanceUnit the unit of distance used for offsets. + */ + public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit){ + writeFloat(Register.X_POD_OFFSET, (float) distanceUnit.toMm(xOffset)); + writeFloat(Register.Y_POD_OFFSET, (float) distanceUnit.toMm(yOffset)); + } + /** * Recalibrates the Odometry Computer's internal IMU.

* Robot MUST be stationary

@@ -300,6 +391,7 @@ public void setOffsets(double xOffset, double yOffset){ */ public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} + /** * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

* Robot MUST be stationary

@@ -345,11 +437,23 @@ public void setEncoderResolution(GoBildaOdometryPods pods){ * Sets the encoder resolution in ticks per mm of the odometry pods.
* You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. * @param ticks_per_mm should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ public void setEncoderResolution(double ticks_per_mm){ writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) ticks_per_mm,ByteOrder.LITTLE_ENDIAN))); } + /** + * Sets the encoder resolution in ticks per mm of the odometry pods.
+ * You can find this number by dividing the counts-per-revolution of your encoder by the circumference of the wheel. + * @param ticks_per_unit should be somewhere between 10 ticks/mm and 100 ticks/mm a goBILDA Swingarm pod is ~13.26291192 + * @param distanceUnit unit used for distance + */ + public void setEncoderResolution(double ticks_per_unit, DistanceUnit distanceUnit){ + double resolution = distanceUnit.toMm(ticks_per_unit); + writeByteArray(Register.MM_PER_TICK,(floatToByteArray((float) resolution,ByteOrder.LITTLE_ENDIAN))); + } + /** * Tuning this value should be unnecessary.
* The goBILDA Odometry Computer has a per-device tuned yaw offset already applied when you receive it.

@@ -414,6 +518,8 @@ public Pose2D setPosition(Pose2D pos){ * FAULT_NO_PODS_DETECTED - the device does not detect any pods plugged in. PURPLE LED
* FAULT_X_POD_NOT_DETECTED - The device does not detect an X pod plugged in. BLUE LED
* FAULT_Y_POD_NOT_DETECTED - The device does not detect a Y pod plugged in. ORANGE LED
+ * FAULT_BAD_READ - The Java code has detected a bad I²C read, the result reported is a + * duplicate of the last good read. */ public DeviceStatus getDeviceStatus(){return lookupStatus(deviceStatus); } @@ -450,45 +556,124 @@ public double getFrequency(){ /** * @return the estimated X (forward) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ - public double getPosX(){return xPosition; } + public double getPosX(){ + return xPosition; + } + + /** + * @return the estimated X (forward) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xPosition); + } /** * @return the estimated Y (Strafe) position of the robot in mm + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ - public double getPosY(){return yPosition; } + public double getPosY(){ + return yPosition; + } + + /** + * @return the estimated Y (Strafe) position of the robot in specified unit + * @param distanceUnit the unit that the estimated position will return in + */ + public double getPosY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yPosition); + } /** - * @return the estimated H (heading) position of the robot in Radians + * @return the unnormalized estimated H (heading) position of the robot in radians + * unnormalized heading is not constrained from -180° to 180°. It will continue counting multiple rotations. + * @deprecated two overflows for this function exist with AngleUnit parameter. These minimize the possibility of unit confusion. */ - public double getHeading(){return hOrientation;} + public double getHeading(){ + return hOrientation; + } + + /** + * @return the normalized estimated H (heading) position of the robot in specified unit + * normalized heading is wrapped from -180°, to 180°. + */ + public double getHeading(AngleUnit angleUnit){ + return angleUnit.fromRadians(hOrientation); + } + + /** + * + * @param unnormalizedAngleUnit + * @return the unnormalized estimated H (heading) position of the robot in specified unit + * unnormalized heading is not constrained from -180° to 180°. It will continue counting + * multiple rotations. + */ + public double getHeading(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hOrientation); + } /** * @return the estimated X (forward) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ - public double getVelX(){return xVelocity; } + public double getVelX(){ + return xVelocity; + } + + /** + * @return the estimated X (forward) velocity of the robot in specified unit/sec + */ + public double getVelX(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(xVelocity); + } /** * @return the estimated Y (strafe) velocity of the robot in mm/sec + * @deprecated The overflow for this function has a DistanceUnit, which can reduce the chance of unit confusion. */ - public double getVelY(){return yVelocity; } + public double getVelY(){ + return yVelocity; + } + + /** + * @return the estimated Y (strafe) velocity of the robot in specified unit/sec + */ + public double getVelY(DistanceUnit distanceUnit){ + return distanceUnit.fromMm(yVelocity); + } /** * @return the estimated H (heading) velocity of the robot in radians/sec + * @deprecated The overflow for this function has an AngleUnit, which can reduce the chance of unit confusion. */ - public double getHeadingVelocity(){return hVelocity; } + public double getHeadingVelocity() { + return hVelocity; + } + + /** + * @return the estimated H (heading) velocity of the robot in specified unit/sec + */ + public double getHeadingVelocity(UnnormalizedAngleUnit unnormalizedAngleUnit){ + return unnormalizedAngleUnit.fromRadians(hVelocity); + } /** * This uses its own I2C read, avoid calling this every loop. - * @return the user-set offset for the X (forward) pod + * @return the user-set offset for the X (forward) pod in specified unit */ - public float getXOffset(){return readFloat(Register.X_POD_OFFSET);} + public float getXOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.X_POD_OFFSET)); + } /** * This uses its own I2C read, avoid calling this every loop. * @return the user-set offset for the Y (strafe) pod */ - public float getYOffset(){return readFloat(Register.Y_POD_OFFSET);} + public float getYOffset(DistanceUnit distanceUnit){ + return (float) distanceUnit.fromMm(readFloat(Register.Y_POD_OFFSET)); + } /** * @return a Pose2D containing the estimated position of the robot @@ -498,12 +683,13 @@ public Pose2D getPosition(){ xPosition, yPosition, AngleUnit.RADIANS, - hOrientation); + //this wraps the hOrientation variable from -180° to +180° + ((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); } - - /** + * @deprecated This function is not recommended, as velocity is wrapped from -180° to 180°. + * instead use individual getters. * @return a Pose2D containing the estimated velocity of the robot, velocity is unit per second */ public Pose2D getVelocity(){ @@ -511,7 +697,7 @@ public Pose2D getVelocity(){ xVelocity, yVelocity, AngleUnit.RADIANS, - hVelocity); + ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java index 664ce404d056..3d7056eea8a8 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -24,10 +24,17 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; +import com.qualcomm.robotcore.hardware.DcMotor; +import com.qualcomm.robotcore.util.ElapsedTime; +import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; +import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; + +import com.acmerobotics.dashboard.FtcDashboard; + import java.util.Locale; @@ -57,7 +64,7 @@ -Ethan Doak */ -@TeleOp(name="goBILDA® PinPoint Odometry Example", group="Linear OpMode") +@TeleOp(name="Odometry with Drive", group="Linear OpMode") //@Disabled public class SensorGoBildaPinpointExample extends LinearOpMode { @@ -83,16 +90,16 @@ public void runOpMode() { 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 + odo.setOffsets(-84.0, -168.0, DistanceUnit.MM); //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. + number of ticks per unit of your odometry pod. */ odo.setEncoderResolution(GoBildaPinpointDriver.GoBildaOdometryPods.goBILDA_4_BAR_POD); - //odo.setEncoderResolution(13.26291192); + //odo.setEncoderResolution(13.26291192, DistanceUnit.MM); /* @@ -115,8 +122,8 @@ increase when you move the robot forward. And the Y (strafe) pod should increase odo.resetPosAndIMU(); telemetry.addData("Status", "Initialized"); - telemetry.addData("X offset", odo.getXOffset()); - telemetry.addData("Y offset", odo.getYOffset()); + telemetry.addData("X offset", odo.getXOffset(DistanceUnit.MM)); + telemetry.addData("Y offset", odo.getYOffset(DistanceUnit.MM)); telemetry.addData("Device Version Number:", odo.getDeviceVersion()); telemetry.addData("Device Scalar", odo.getYawScalar()); telemetry.update(); @@ -172,8 +179,7 @@ gets the current Position (x & y in mm, and heading in degrees) of the robot, an /* 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.getX(DistanceUnit.MM), vel.getY(DistanceUnit.MM), vel.getHeading(AngleUnit.DEGREES)); + String velocity = String.format(Locale.US,"{XVel: %.3f, YVel: %.3f, HVel: %.3f}", odo.getVelX(DistanceUnit.MM), odo.getVelY(DistanceUnit.MM), odo.getHeadingVelocity(UnnormalizedAngleUnit.DEGREES)); telemetry.addData("Velocity", velocity); @@ -185,6 +191,7 @@ gets the current Velocity (x & y in mm/sec and heading in degrees/sec) and print 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 + FAULT_BAD_READ - The firmware detected a bad I²C read, if a bad read is detected, the device status is updated and the previous position is reported */ telemetry.addData("Status", odo.getDeviceStatus()); From 345335395ff43e91b5545eb149b8f3968108b016 Mon Sep 17 00:00:00 2001 From: Ethan Doak Date: Tue, 18 Mar 2025 10:07:21 -0500 Subject: [PATCH 2/5] Adds checks for invalid reads --- .../firstinspires/ftc/teamcode/GoBildaPinpointDriver.java | 2 +- .../ftc/teamcode/SensorGoBildaPinpointExample.java | 7 +------ 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java index cc697e3ae250..d5ea4d71436d 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java @@ -1,5 +1,5 @@ /* MIT License - * Copyright (c) [2024] [Base 10 Assets, LLC] + * Copyright (c) [2025] [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 diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java index 3d7056eea8a8..78db107d56d2 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -1,5 +1,5 @@ /* MIT License - * Copyright (c) [2024] [Base 10 Assets, LLC] + * Copyright (c) [2025] [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 @@ -24,17 +24,12 @@ import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode; import com.qualcomm.robotcore.eventloop.opmode.TeleOp; -import com.qualcomm.robotcore.hardware.DcMotor; -import com.qualcomm.robotcore.util.ElapsedTime; -import org.firstinspires.ftc.robotcore.external.Telemetry; import org.firstinspires.ftc.robotcore.external.navigation.AngleUnit; import org.firstinspires.ftc.robotcore.external.navigation.DistanceUnit; import org.firstinspires.ftc.robotcore.external.navigation.Pose2D; import org.firstinspires.ftc.robotcore.external.navigation.UnnormalizedAngleUnit; -import com.acmerobotics.dashboard.FtcDashboard; - import java.util.Locale; From 807e88188feef3586222348599fe84976886a2e0 Mon Sep 17 00:00:00 2001 From: Ethan Doak Date: Tue, 18 Mar 2025 10:25:00 -0500 Subject: [PATCH 3/5] wrap heading before sending it to (normalized) AngleUnit. --- .../org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java index d5ea4d71436d..b5a2d73da0a5 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java @@ -600,12 +600,10 @@ public double getHeading(){ * normalized heading is wrapped from -180°, to 180°. */ public double getHeading(AngleUnit angleUnit){ - return angleUnit.fromRadians(hOrientation); + return angleUnit.fromRadians((hOrientation + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI; } /** - * - * @param unnormalizedAngleUnit * @return the unnormalized estimated H (heading) position of the robot in specified unit * unnormalized heading is not constrained from -180° to 180°. It will continue counting * multiple rotations. From 9be8cf4a2eba4ffa14c67c9e6e7d6a50b908bb6e Mon Sep 17 00:00:00 2001 From: Ethan Doak Date: Wed, 19 Mar 2025 17:04:30 -0500 Subject: [PATCH 4/5] Add checks for reads where all results are 0s. --- .../ftc/teamcode/GoBildaPinpointDriver.java | 25 +++++++++++-------- .../SensorGoBildaPinpointExample.java | 4 +-- 2 files changed, 16 insertions(+), 13 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java index b5a2d73da0a5..6ed7d45c1e56 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java @@ -261,10 +261,13 @@ private DeviceStatus lookupStatus (int s){ * @param oldValue the reading from the previous cycle * @param newValue the new reading * @param threshold the maximum change between this reading and the previous one + * @param bulkUpdate true if we are updating the loopTime variable. If not it should be false. * @return newValue if the position is good, oldValue otherwise */ - private Float isPositionCorrupt(float oldValue, float newValue, int threshold){ - boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; + private Float isPositionCorrupt(float oldValue, float newValue, int threshold, boolean bulkUpdate){ + boolean noData = bulkUpdate && (loopTime < 1); + + boolean isCorrupt = noData || Float.isNaN(newValue) || Math.abs(newValue - oldValue) > threshold; if(!isCorrupt){ return newValue; @@ -283,6 +286,7 @@ private Float isPositionCorrupt(float oldValue, float newValue, int threshold){ */ private Float isVelocityCorrupt(float oldValue, float newValue, int threshold){ boolean isCorrupt = Float.isNaN(newValue) || Math.abs(newValue) > threshold; + boolean noData = (loopTime <= 1); if(!isCorrupt){ return newValue; @@ -324,9 +328,9 @@ public void update(){ * Check to see if any of the floats we have received from the device are NaN or are too large * if they are, we return the previously read value and alert the user via the DeviceStatus Enum. */ - xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold); - yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold); - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold); + xPosition = isPositionCorrupt(oldPosX, xPosition, positionThreshold, true); + yPosition = isPositionCorrupt(oldPosY, yPosition, positionThreshold, true); + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, true); xVelocity = isVelocityCorrupt(oldVelX, xVelocity, velocityThreshold); yVelocity = isVelocityCorrupt(oldVelY, yVelocity, velocityThreshold); hVelocity = isVelocityCorrupt(oldVelH, hVelocity, headingVelocityThreshold); @@ -347,7 +351,7 @@ public void update(ReadData data) { hOrientation = byteArrayToFloat(deviceClient.read(Register.H_ORIENTATION.bVal, 4), ByteOrder.LITTLE_ENDIAN); - hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold); + hOrientation = isPositionCorrupt(oldPosH, hOrientation, headingThreshold, false); if (deviceStatus == DeviceStatus.FAULT_BAD_READ.status){ deviceStatus = DeviceStatus.READY.status; @@ -369,7 +373,6 @@ public void setOffsets(double xOffset, double yOffset){ writeFloat(Register.Y_POD_OFFSET, (float) yOffset); } - /** * Sets the odometry pod positions relative to the point that the odometry computer tracks around.

* The most common tracking position is the center of the robot.

@@ -391,7 +394,6 @@ public void setOffsets(double xOffset, double yOffset, DistanceUnit distanceUnit */ public void recalibrateIMU(){writeInt(Register.DEVICE_CONTROL,1<<0);} - /** * Resets the current position to 0,0,0 and recalibrates the Odometry Computer's internal IMU.

* Robot MUST be stationary

@@ -507,6 +509,10 @@ public Pose2D setPosition(Pose2D pos){ */ public int getDeviceVersion(){return readInt(Register.DEVICE_VERSION); } + /** + * @return a scalar that the IMU measured heading is multiplied by. This is tuned for each unit + * and should not need adjusted. + */ public float getYawScalar(){return readFloat(Register.YAW_SCALAR); } /** @@ -697,9 +703,6 @@ public Pose2D getVelocity(){ AngleUnit.RADIANS, ((hVelocity + Math.PI) % (2 * Math.PI) + 2 * Math.PI) % (2 * Math.PI) - Math.PI); } - - - } diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java index 78db107d56d2..19db2f0f9a9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -59,7 +59,7 @@ -Ethan Doak */ -@TeleOp(name="Odometry with Drive", group="Linear OpMode") +@TeleOp(name="goBILDA Pinpoint Example", group="Linear OpMode") //@Disabled public class SensorGoBildaPinpointExample extends LinearOpMode { @@ -141,7 +141,7 @@ increase when you move the robot forward. And the Y (strafe) pod should increase Optionally, you can update only the heading of the device. This takes less time to read, but will not pull any other data. Only the heading (which you can pull with getHeading() or in getPosition(). */ - //odo.update(GoBildaPinpointDriver.readData.ONLY_UPDATE_HEADING); + //odo.update(GoBildaPinpointDriver.ReadData.ONLY_UPDATE_HEADING); if (gamepad1.a){ From 8680515063c62a54c6bbc5e1857c92c5473e0ddd Mon Sep 17 00:00:00 2001 From: Ethan Doak Date: Mon, 24 Mar 2025 13:31:04 -0500 Subject: [PATCH 5/5] Add setters for X, Y, and heading individually. --- .../ftc/teamcode/GoBildaPinpointDriver.java | 36 +++++++++++++++++-- .../SensorGoBildaPinpointExample.java | 2 +- 2 files changed, 35 insertions(+), 3 deletions(-) diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java index 6ed7d45c1e56..3781542c9bc6 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/GoBildaPinpointDriver.java @@ -182,17 +182,16 @@ private int readInt(Register reg){ private float byteArrayToFloat(byte[] byteArray, ByteOrder byteOrder){ return ByteBuffer.wrap(byteArray).order(byteOrder).getFloat(); } + /** * Reads a float from a register * @param reg the register to read * @return the float value stored in that register */ - private float readFloat(Register reg){ return byteArrayToFloat(deviceClient.read(reg.bVal,4),ByteOrder.LITTLE_ENDIAN); } - /** * Converts a float to a byte array * @param value the float array to convert @@ -498,6 +497,39 @@ public Pose2D setPosition(Pose2D pos){ return pos; } + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posX the new X position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posX + */ + public void setPosX(double posX, DistanceUnit distanceUnit){ + writeByteArray(Register.X_POSITION,(floatToByteArray((float) distanceUnit.toMm(posX), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a position that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param posY the new Y position you'd like the Pinpoint to track your robot relive to. + * @param distanceUnit the unit for posY + */ + public void setPosY(double posY, DistanceUnit distanceUnit){ + writeByteArray(Register.Y_POSITION,(floatToByteArray((float) distanceUnit.toMm(posY), ByteOrder.LITTLE_ENDIAN))); + } + + /** + * Send a heading that the Pinpoint should use to track your robot relative to. + * You can use this to update the estimated position of your robot with new external + * sensor data, or to run a robot in field coordinates. + * @param heading the new heading you'd like the Pinpoint to track your robot relive to. + * @param angleUnit Radians or Degrees + */ + public void setHeading(double heading, AngleUnit angleUnit){ + writeByteArray(Register.H_ORIENTATION,(floatToByteArray((float) angleUnit.toRadians(heading), ByteOrder.LITTLE_ENDIAN))); + } + /** * Checks the deviceID of the Odometry Computer. Should return 1. * @return 1 if device is functional. diff --git a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java index 19db2f0f9a9e..6e47d853fd9e 100644 --- a/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java +++ b/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/SensorGoBildaPinpointExample.java @@ -120,7 +120,7 @@ increase when you move the robot forward. And the Y (strafe) pod should increase telemetry.addData("X offset", odo.getXOffset(DistanceUnit.MM)); telemetry.addData("Y offset", odo.getYOffset(DistanceUnit.MM)); telemetry.addData("Device Version Number:", odo.getDeviceVersion()); - telemetry.addData("Device Scalar", odo.getYawScalar()); + telemetry.addData("Heading Scalar", odo.getYawScalar()); telemetry.update(); // Wait for the game to start (driver presses START)