Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

multithreading odometry #51

Open
wants to merge 2 commits into
base: phoenix6
Choose a base branch
from
Open

multithreading odometry #51

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f50d138
finish multithreading
MatthewChoulas Dec 6, 2023
6b45a74
fix review changes
MatthewChoulas Dec 14, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion build.gradle
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,7 +83,7 @@ dependencies {
simulationRelease wpi.sim.enableRelease()

implementation 'org.jetbrains.kotlin:kotlin-test-junit5'
implementation 'com.github.team4099:FalconUtils:1.1.26'
implementation 'com.github.team4099:FalconUtils:1.1.27r2'
implementation 'org.apache.commons:commons-collections4:4.0'
implementation 'com.google.code.gson:gson:2.10.1'
implementation "io.javalin:javalin:5.3.2"
Expand Down
8 changes: 8 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,14 @@ object Constants {
}

object Drivetrain {

enum class DrivetrainType {
PHOENIX_TALON,
REV_NEO
}

val DRIVETRAIN_TYPE = DrivetrainType.PHOENIX_TALON

const val FRONT_LEFT_DRIVE_ID = 11
const val FRONT_LEFT_STEERING_ID = 21
const val FRONT_LEFT_ANALOG_POTENTIOMETER = 1
Expand Down
2 changes: 2 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -31,6 +31,8 @@ object DrivetrainConstants {
const val FOC_ENABLED = false
const val MINIMIZE_SKEW = false

const val OMOMETRY_UPDATE_FREQUENCY = 250.0
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

can we add units for frequency? t = 1/f so it should be as something like

inline val Double.hertz: Time
  get() = Time(1/this)


const val WHEEL_COUNT = 4
val WHEEL_DIAMETER = 3.827.inches
val DRIVETRAIN_LENGTH = 22.750.inches
Expand Down
72 changes: 43 additions & 29 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -46,6 +46,8 @@ import org.team4099.lib.units.derived.inRotation2ds
import org.team4099.lib.units.derived.radians
import org.team4099.lib.units.inMetersPerSecond
import org.team4099.lib.units.perSecond
import java.util.concurrent.locks.Lock
import java.util.concurrent.locks.ReentrantLock
import java.util.function.Supplier

class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() {
Expand All @@ -58,6 +60,17 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
val gyroInputs = GyroIO.GyroIOInputs()
val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR

companion object {
var odometryLock: Lock = ReentrantLock()
fun setOdometryLock(Locked: Boolean) {
MatthewChoulas marked this conversation as resolved.
Show resolved Hide resolved
if (Locked) {
odometryLock.lock()
} else {
odometryLock.unlock()
}
}
}

var gyroYawOffset = 0.0.radians

val closestAlignmentAngle: Angle
Expand Down Expand Up @@ -181,8 +194,15 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB

override fun periodic() {
val startTime = Clock.realTimestamp
gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)

odometryLock.lock(); // Prevents odometry updates while reading data

gyroIO.updateInputs(gyroInputs)
swerveModules.forEach { it.updateInputs() }

odometryLock.unlock()

gyroNotConnectedAlert.set(!gyroInputs.gyroConnected)

swerveModules.forEach { it.periodic() }

Expand Down Expand Up @@ -272,35 +292,31 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB
}

private fun updateOdometry() {
val wheelDeltas =
mutableListOf(
SwerveModulePosition(),
SwerveModulePosition(),
SwerveModulePosition(),
SwerveModulePosition()
)
for (i in 0 until 4) {
wheelDeltas[i] =
SwerveModulePosition(
swerveModules[i].inputs.drivePosition.inMeters -
lastModulePositions[i].distanceMeters,
swerveModules[i].inputs.steeringPosition.inRotation2ds
)
lastModulePositions[i] =
SwerveModulePosition(
swerveModules[i].inputs.drivePosition.inMeters,
swerveModules[i].inputs.steeringPosition.inRotation2ds
)

var deltaCount =
if (gyroInputs.gyroConnected) gyroInputs.odometryYawPositions.size else Integer.MAX_VALUE
for (i in 0..4) {
deltaCount = Math.min(deltaCount, swerveModules[i].positionDeltas.size)
}
for (deltaIndex in 0..deltaCount) {
sswadkar marked this conversation as resolved.
Show resolved Hide resolved
// Read wheel deltas from each module
val wheelDeltas = arrayOfNulls<SwerveModulePosition>(4)
for (moduleIndex in 0..4) {
wheelDeltas[moduleIndex] = swerveModules[moduleIndex].positionDeltas[deltaIndex]
}

var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray())
var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas)

if (gyroInputs.gyroConnected) {
driveTwist =
edu.wpi.first.math.geometry.Twist2d(
driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians
)
lastGyroYaw = gyroInputs.rawGyroYaw
if (gyroInputs.gyroConnected) {
driveTwist =
edu.wpi.first.math.geometry.Twist2d(
driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians
)
lastGyroYaw = gyroInputs.rawGyroYaw
}

swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist))
odometryPose = odometryPose.exp(Twist2d(driveTwist))
}

// reversing the drift to store the ground truth pose
Expand All @@ -323,8 +339,6 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB

Logger.getInstance().recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d)
}

swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist))
}

/**
Expand Down
2 changes: 2 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,8 @@ interface GyroIO {
var gyroPitchRate = 0.0.radians.perSecond
var gyroRollRate = 0.0.radians.perSecond

var odometryYawPositions = arrayOf<Angle>()

var gyroConnected = false

override fun toLog(table: LogTable?) {
Expand Down
18 changes: 18 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,8 @@ import com.ctre.phoenix6.hardware.Pigeon2
import com.team4099.robot2023.config.constants.Constants
import com.team4099.robot2023.config.constants.DrivetrainConstants
import com.team4099.robot2023.config.constants.GyroConstants
import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.PhoenixOdometryThread
import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.SparkMaxOdometryThread
import org.littletonrobotics.junction.Logger
import org.team4099.lib.units.AngularVelocity
import org.team4099.lib.units.derived.Angle
Expand All @@ -13,6 +15,7 @@ import org.team4099.lib.units.derived.inDegrees
import org.team4099.lib.units.derived.inRadians
import org.team4099.lib.units.inDegreesPerSecond
import org.team4099.lib.units.perSecond
import java.util.Queue
import kotlin.math.IEEErem

object GyroIOPigeon2 : GyroIO {
Expand All @@ -24,6 +27,8 @@ object GyroIOPigeon2 : GyroIO {
var gyroPitchOffset: Angle = 0.0.degrees
var gyroRollOffset: Angle = 0.0.degrees

val yawPositionQueue: Queue<Double>

val rawGyro: Angle = 0.0.degrees

/** The current angle of the drivetrain. */
Expand Down Expand Up @@ -91,6 +96,15 @@ object GyroIOPigeon2 : GyroIO {
pigeon2Configuration.MountPose.MountPoseYaw = GyroConstants.mountYaw.inRadians
pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians

yawPositionQueue =
if (Constants.Drivetrain.DRIVETRAIN_TYPE == Constants.Drivetrain.DrivetrainType.PHOENIX_TALON) {
PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw())
} else {
SparkMaxOdometryThread.getInstance().registerSignal {
pigeon2.getYaw().getValueAsDouble()
}
}

// TODO look into more pigeon configuration stuff
pigeon2.configurator.apply(pigeon2Configuration)
}
Expand All @@ -109,6 +123,10 @@ object GyroIOPigeon2 : GyroIO {
inputs.gyroPitchRate = gyroPitchRate
inputs.gyroRollRate = gyroRollRate

inputs.odometryYawPositions =
yawPositionQueue.stream().map { value: Double -> value.degrees }.toArray() as Array<Angle>
yawPositionQueue.clear()

Logger.getInstance().recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value)
}

Expand Down
23 changes: 23 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,11 +43,15 @@ class SwerveModule(val io: SwerveModuleIO) {

var modulePosition = SwerveModulePosition()

var positionDeltas = mutableListOf<SwerveModulePosition>()

private var speedSetPoint: LinearVelocity = 0.feet.perSecond
private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond

private var steeringSetPoint: Angle = 0.degrees

private var lastDrivePosition = 0.meters

private var shouldInvert = false

private val steeringkP =
Expand Down Expand Up @@ -110,11 +114,30 @@ class SwerveModule(val io: SwerveModuleIO) {
drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI)
drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD)
}

}

fun updateInputs() {
io.updateInputs(inputs)
}

fun periodic() {
io.updateInputs(inputs)

val deltaCount =
Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size)

for (i in 0..deltaCount) {
val newDrivePosition = inputs.odometryDrivePositions[i]
val newSteeringAngle = inputs.odometrySteeringPositions[i]
positionDeltas.add(
SwerveModulePosition(
(newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds
)
)
lastDrivePosition = newDrivePosition
}

// Updating SwerveModulePosition every loop cycle
modulePosition.distanceMeters = inputs.drivePosition.inMeters
modulePosition.angle = inputs.steeringPosition.inRotation2ds
Expand Down
4 changes: 4 additions & 0 deletions src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@ import org.team4099.lib.units.AngularVelocity
import org.team4099.lib.units.LinearAcceleration
import org.team4099.lib.units.LinearVelocity
import org.team4099.lib.units.Velocity
import org.team4099.lib.units.base.Length
import org.team4099.lib.units.base.Meter
import org.team4099.lib.units.base.amps
import org.team4099.lib.units.base.celsius
Expand Down Expand Up @@ -48,6 +49,9 @@ interface SwerveModuleIO {
var driveTemp = 0.0.celsius
var steeringTemp = 0.0.celsius

var odometryDrivePositions = arrayOf<Length>()
var odometrySteeringPositions = arrayOf<Angle>()

var potentiometerOutputRaw = 0.0
var potentiometerOutputRadians = 0.0.radians

Expand Down
Loading
Loading