SCDS/FTCRoadRunnerRobotics
7
0
Fork 0
Code Issues 2 Pull Requests Actions Packages Projects Releases Wiki Activity

can now use SparkFunOTOSCorrected

Browse Source
This commit is contained in:
brotherhobo
2024-09-04 20:57:29 -04:00
parent 425c001a1b
commit cfc6964799
3 changed files with 53 additions and 1 deletions
Download Patch File Download Diff File Expand all files Collapse all files

46
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/SparkFunOTOSCorrected.kt Normal file
View File

@ -0,0 +1,46 @@
package com.acmerobotics.roadrunner.ftc
import com.qualcomm.hardware.sparkfun.SparkFunOTOS
import com.qualcomm.robotcore.hardware.I2cDeviceSynch
import com.qualcomm.robotcore.hardware.configuration.annotations.DeviceProperties
import com.qualcomm.robotcore.hardware.configuration.annotations.I2cDeviceType
import java.util.*
@I2cDeviceType
@DeviceProperties(
name = "SparkFun OTOS Corrected",
xmlTag = "SparkFunOTOS2",
description = "SparkFun Qwiic Optical Tracking Odometry Sensor Corrected"
)
class SparkFunOTOSCorrected(deviceClient: I2cDeviceSynch) : SparkFunOTOS(deviceClient) {
/**
* Gets only the position and velocity measured by the
* OTOS in a single burst read
* @param pos Position measured by the OTOS
* @param vel Velocity measured by the OTOS
*/
fun getPosVel(pos: Pose2D, vel: Pose2D) {
// Read all pose registers
val rawData = deviceClient.read(REG_POS_XL.toInt(), 12)
// Convert raw data to pose units
pos.set(regsToPose(Arrays.copyOfRange(rawData, 0, 6), INT16_TO_METER, INT16_TO_RAD))
vel.set(regsToPose(Arrays.copyOfRange(rawData, 6, 12), INT16_TO_MPS, INT16_TO_RPS))
}
// Modified version of poseToRegs to fix pose setting issue
// see https://discord.com/channels/225450307654647808/1246977443030368349/1271702497659977760
override fun poseToRegs(rawData: ByteArray, pose: Pose2D, xyToRaw: Double, hToRaw: Double) {
// Convert pose units to raw data
val rawX = (_distanceUnit.toMeters(pose.x) * xyToRaw).toInt().toShort()
val rawY = (_distanceUnit.toMeters(pose.y) * xyToRaw).toInt().toShort()
val rawH = (_angularUnit.toRadians(pose.h) * hToRaw).toInt().toShort()
// Store raw data in buffer
rawData[0] = (rawX.toInt() and 0xFF).toByte()
rawData[1] = ((rawX.toInt() shr 8) and 0xFF).toByte()
rawData[2] = (rawY.toInt() and 0xFF).toByte()
rawData[3] = ((rawY.toInt() shr 8) and 0xFF).toByte()
rawData[4] = (rawH.toInt() and 0xFF).toByte()
rawData[5] = ((rawH.toInt() shr 8) and 0xFF).toByte()
}
}

6
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/OTOSLocalizer.java
View File

@ -65,6 +65,12 @@ public class OTOSLocalizer extends Localizer {
hardwareMap = map;
// TODO: replace this with your OTOS port
/*
TODO: If you want to use the "SparkFunOTOSCorrected" version of OTOS, then replace the
'SparkFunOTOS.class' below with 'SparkFunOTOSCorrected.class' and set the OTOS as a
"SparkFunOTOS Corrected" in your robot confg
*/
SparkFunOTOS
otos = hardwareMap.get(SparkFunOTOS.class, "sensor_otos");
otos.setLinearUnit(DistanceUnit.INCH);

2
TeamCode/src/main/java/org/firstinspires/ftc/teamcode/pedroPathing/localization/localizers/TwoWheelLocalizer.java
View File

@ -220,7 +220,7 @@ public class TwoWheelLocalizer extends Localizer { // todo: make two wheel odo w
forwardEncoder.update();
strafeEncoder.update();
double currentIMUOrientation =MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
double currentIMUOrientation = MathFunctions.normalizeAngle(imu.getRobotYawPitchRollAngles().getYaw(AngleUnit.RADIANS));
deltaRadians = MathFunctions.getTurnDirection(previousIMUOrientation, currentIMUOrientation) * MathFunctions.getSmallestAngleDifference(currentIMUOrientation, previousIMUOrientation);
previousIMUOrientation = currentIMUOrientation;
}