From f50d138370b5dc08159618ba7bc080ddf4ca3960 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Wed, 6 Dec 2023 11:53:00 -0500 Subject: [PATCH] finish multithreading --- build.gradle | 2 +- .../config/constants/DrivetrainConstants.kt | 2 + .../subsystems/drivetrain/drive/Drivetrain.kt | 72 ++++---- .../subsystems/drivetrain/gyro/GyroIO.kt | 2 + .../drivetrain/gyro/GyroIOPigeon2.kt | 20 +++ .../drivetrain/swervemodule/SwerveModule.kt | 23 +++ .../drivetrain/swervemodule/SwerveModuleIO.kt | 4 + .../swervemodule/SwerveModuleIOFalcon.kt | 155 +++++++++++++++++- .../threads/PhoenixOdometryThread.java | 110 +++++++++++++ .../threads/SparkMaxOdometryThread.java | 73 +++++++++ .../robot2023/subsystems/falconspin/Motor.kt | 4 +- vendordeps/Phoenix.json | 59 ++++--- 12 files changed, 462 insertions(+), 64 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java diff --git a/build.gradle b/build.gradle index 69ad5ff0..184c545d 100644 --- a/build.gradle +++ b/build.gradle @@ -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" diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt index b944808b..13aac5e5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -31,6 +31,8 @@ object DrivetrainConstants { const val FOC_ENABLED = false const val MINIMIZE_SKEW = false + const val OMOMETRY_UPDATE_FREQUENCY = 250.0 + const val WHEEL_COUNT = 4 val WHEEL_DIAMETER = 3.827.inches val DRIVETRAIN_LENGTH = 22.750.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt index f232f9b4..b31dd0f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/Drivetrain.kt @@ -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() { @@ -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) { + if (Locked) { + odometryLock.lock() + } else { + odometryLock.unlock() + } + } + } + var gyroYawOffset = 0.0.radians val closestAlignmentAngle: Angle @@ -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() } @@ -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) { + // Read wheel deltas from each module + val wheelDeltas = arrayOfNulls(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 @@ -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)) } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt index 1697ec38..d66b7273 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIO.kt @@ -19,6 +19,8 @@ interface GyroIO { var gyroPitchRate = 0.0.radians.perSecond var gyroRollRate = 0.0.radians.perSecond + var odometryYawPositions = arrayOf() + var gyroConnected = false override fun toLog(table: LogTable?) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt index b5323f6a..30158bc8 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt @@ -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 @@ -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 { @@ -20,10 +23,14 @@ object GyroIOPigeon2 : GyroIO { private val isConnected: Boolean = pigeon2.upTime.value > 0.0 + val phoenixDrive = true + var gyroYawOffset: Angle = 0.0.degrees var gyroPitchOffset: Angle = 0.0.degrees var gyroRollOffset: Angle = 0.0.degrees + val yawPositionQueue: Queue + val rawGyro: Angle = 0.0.degrees /** The current angle of the drivetrain. */ @@ -91,6 +98,15 @@ object GyroIOPigeon2 : GyroIO { pigeon2Configuration.MountPose.MountPoseYaw = GyroConstants.mountYaw.inRadians pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians + yawPositionQueue = + if (phoenixDrive) { + PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw()) + } else { + SparkMaxOdometryThread.getInstance().registerSignal { + pigeon2.getYaw().getValueAsDouble() + } + } + // TODO look into more pigeon configuration stuff pigeon2.configurator.apply(pigeon2Configuration) } @@ -109,6 +125,10 @@ object GyroIOPigeon2 : GyroIO { inputs.gyroPitchRate = gyroPitchRate inputs.gyroRollRate = gyroRollRate + inputs.odometryYawPositions = + yawPositionQueue.stream().map { value: Double -> value.degrees }.toArray() as Array + yawPositionQueue.clear() + Logger.getInstance().recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt index bef85758..ea1b492e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt @@ -43,11 +43,15 @@ class SwerveModule(val io: SwerveModuleIO) { var modulePosition = SwerveModulePosition() + var positionDeltas: Array + 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 = @@ -110,11 +114,30 @@ class SwerveModule(val io: SwerveModuleIO) { drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } + + positionDeltas = arrayOf() + } + + fun updateInputs() { + io.updateInputs(inputs) } fun periodic() { io.updateInputs(inputs) + val deltaCount = + Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size) + positionDeltas = arrayOfNulls(deltaCount) + for (i in 0..deltaCount) { + val newDrivePosition = inputs.odometryDrivePositions[i] + val newSteeringAngle = inputs.odometrySteeringPositions[i] + positionDeltas[i] = + SwerveModulePosition( + (newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds + ) + lastDrivePosition = newDrivePosition + } + // Updating SwerveModulePosition every loop cycle modulePosition.distanceMeters = inputs.drivePosition.inMeters modulePosition.angle = inputs.steeringPosition.inRotation2ds diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt index bbc82e41..2453daaf 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt @@ -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 @@ -48,6 +49,9 @@ interface SwerveModuleIO { var driveTemp = 0.0.celsius var steeringTemp = 0.0.celsius + var odometryDrivePositions = arrayOf() + var odometrySteeringPositions = arrayOf() + var potentiometerOutputRaw = 0.0 var potentiometerOutputRadians = 0.0.radians diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt index 8a541c06..5def9c60 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule +import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.MotionMagicConfigs import com.ctre.phoenix6.configs.MotorOutputConfigs import com.ctre.phoenix6.configs.Slot0Configs @@ -11,9 +12,11 @@ import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads.PhoenixOdometryThread import com.team4099.robot2023.subsystems.falconspin.Falcon500 import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import edu.wpi.first.math.geometry.Rotation2d import edu.wpi.first.wpilibj.AnalogInput import edu.wpi.first.wpilibj.RobotController import org.littletonrobotics.junction.Logger @@ -22,6 +25,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 @@ -39,7 +43,9 @@ import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond import java.lang.Math.PI +import java.util.Queue class SwerveModuleIOFalcon( private val steeringFalcon: TalonFX, @@ -55,6 +61,7 @@ class SwerveModuleIOFalcon( DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, DrivetrainConstants.STEERING_COMPENSATION_VOLTAGE ) + private val driveSensor = ctreLinearMechanismSensor( driveFalcon, @@ -151,13 +158,108 @@ class SwerveModuleIOFalcon( ) ) } + val driveStatorCurrentSignal: StatusSignal + val driveSupplyCurrentSignal: StatusSignal + val steeringStatorCurrentSignal: StatusSignal + val steeringSupplyCurrentSignal: StatusSignal + val driveTempSignal: StatusSignal + val steeringTempSignal: StatusSignal + val drivePositionQueue: Queue + val steeringPositionQueue: Queue + + init { + driveFalcon.configurator.apply(TalonFXConfiguration()) + steeringFalcon.configurator.apply(TalonFXConfiguration()) + + driveFalcon.clearStickyFaults() + steeringFalcon.clearStickyFaults() + + steeringConfiguration.Slot0.kP = + steeringSensor.proportionalPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KP) + steeringConfiguration.Slot0.kI = + steeringSensor.integralPositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KI) + steeringConfiguration.Slot0.kD = + steeringSensor.derivativePositionGainToRawUnits(DrivetrainConstants.PID.STEERING_KD) + steeringConfiguration.Slot0.kV = + steeringSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.STEERING_KFF) + steeringConfiguration.MotionMagic.MotionMagicCruiseVelocity = + steeringSensor.velocityToRawUnits(DrivetrainConstants.STEERING_VEL_MAX) + steeringConfiguration.MotionMagic.MotionMagicAcceleration = + steeringSensor.accelerationToRawUnits(DrivetrainConstants.STEERING_ACCEL_MAX) + steeringConfiguration.CurrentLimits.SupplyCurrentLimit = + DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT.inAmperes + steeringConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + + steeringConfiguration.MotorOutput.NeutralMode = + NeutralModeValue.Brake // change back to coast maybe? + steeringFalcon.inverted = true + steeringFalcon.configurator.apply(steeringConfiguration) + + driveConfiguration.Slot0.kP = + driveSensor.proportionalVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KP) + driveConfiguration.Slot0.kI = + driveSensor.integralVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KI) + driveConfiguration.Slot0.kD = + driveSensor.derivativeVelocityGainToRawUnits(DrivetrainConstants.PID.DRIVE_KD) + driveConfiguration.Slot0.kV = 0.05425 + // driveSensor.velocityFeedforwardToRawUnits(DrivetrainConstants.PID.DRIVE_KFF) + driveConfiguration.CurrentLimits.SupplyCurrentLimit = + DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.SupplyCurrentThreshold = + DrivetrainConstants.DRIVE_THRESHOLD_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.SupplyTimeThreshold = + DrivetrainConstants.DRIVE_TRIGGER_THRESHOLD_TIME.inSeconds + driveConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + driveConfiguration.CurrentLimits.StatorCurrentLimit = + DrivetrainConstants.DRIVE_STATOR_CURRENT_LIMIT.inAmperes + driveConfiguration.CurrentLimits.StatorCurrentLimitEnable = false // TODO tune + + driveConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + driveFalcon.configurator.apply(driveConfiguration) + + driveStatorCurrentSignal = driveFalcon.statorCurrent + driveSupplyCurrentSignal = driveFalcon.supplyCurrent + steeringStatorCurrentSignal = steeringFalcon.statorCurrent + steeringSupplyCurrentSignal = steeringFalcon.statorCurrent + driveTempSignal = driveFalcon.deviceTemp + steeringTempSignal = steeringFalcon.deviceTemp + + drivePositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveFalcon, driveFalcon.getPosition()) + steeringPositionQueue = + PhoenixOdometryThread.getInstance().registerSignal(driveFalcon, driveFalcon.getPosition()) + + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(Falcon500(driveFalcon, "$label Drive Motor")), + DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT, + 90.celsius, + DrivetrainConstants.DRIVE_SUPPLY_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(Falcon500(steeringFalcon, "$label Steering Motor")), + DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT, + 90.celsius, + DrivetrainConstants.STEERING_SUPPLY_CURRENT_LIMIT - 10.amps, + 110.celsius + ) + ) + } override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { inputs.driveAppliedVoltage = (driveFalcon.get() * RobotController.getBatteryVoltage()).volts inputs.steeringAppliedVoltage = (steeringFalcon.get() * RobotController.getBatteryVoltage()).volts - inputs.driveStatorCurrent = driveFalcon.statorCurrent.value.amps + inputs.driveStatorCurrent = driveStatorCurrentSignal.value.amps inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.value.amps inputs.steeringStatorCurrent = steeringFalcon.statorCurrent.value.amps inputs.steeringSupplyCurrent = steeringFalcon.statorCurrent.value.amps @@ -172,6 +274,29 @@ class SwerveModuleIOFalcon( inputs.driveTemp = driveFalcon.deviceTemp.value.celsius inputs.steeringTemp = steeringFalcon.deviceTemp.value.celsius + inputs.odometryDrivePositions = + drivePositionQueue + .stream() + .map { value: Double -> + ( + DrivetrainConstants.WHEEL_DIAMETER * 2 * Math.PI * value / + DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO + ) + } + .toArray() as + Array + inputs.odometrySteeringPositions = + steeringPositionQueue + .stream() + .map { value: Double -> + Rotation2d.fromRotations(value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO) + .radians + } + .toArray() as + Array + drivePositionQueue.clear() + steeringPositionQueue.clear() + inputs.potentiometerOutputRaw = potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI inputs.potentiometerOutputRadians = potentiometerOutput.radians @@ -186,7 +311,16 @@ class SwerveModuleIOFalcon( } override fun setSteeringSetpoint(angle: Angle) { - steeringFalcon.setControl(PositionDutyCycle(steeringSensor.positionToRawUnits(angle), DrivetrainConstants.FOC_ENABLED, 0.0, 0, false)) + steeringFalcon.setControl( + PositionDutyCycle( + steeringSensor.positionToRawUnits(angle), + steeringSensor.velocityToRawUnits(0.0.radians.perSecond), + DrivetrainConstants.FOC_ENABLED, + 0.0, + 0, + false + ) + ) } override fun setClosedLoop( @@ -197,7 +331,12 @@ class SwerveModuleIOFalcon( val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign driveFalcon.setControl( VelocityDutyCycle( - driveSensor.velocityToRawUnits(speed), DrivetrainConstants.FOC_ENABLED, feedforward.inVolts / 12.0, 0, false + driveSensor.velocityToRawUnits(speed), + driveSensor.accelerationToRawUnits(acceleration), + DrivetrainConstants.FOC_ENABLED, + feedforward.inVolts / 12.0, + 0, + false ) ) @@ -211,7 +350,11 @@ class SwerveModuleIOFalcon( * @param speed: Desired speed */ override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - driveFalcon.setControl(DutyCycleOut(speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, DrivetrainConstants.FOC_ENABLED, false)) + driveFalcon.setControl( + DutyCycleOut( + speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, DrivetrainConstants.FOC_ENABLED, false + ) + ) setSteeringSetpoint(steering) } @@ -220,7 +363,7 @@ class SwerveModuleIOFalcon( } override fun zeroSteering() { - steeringFalcon.setRotorPosition( + steeringFalcon.setPosition( steeringSensor.positionToRawUnits( if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) (potentiometerOutput.radians) - zeroOffset @@ -233,7 +376,7 @@ class SwerveModuleIOFalcon( } override fun zeroDrive() { - driveFalcon.setRotorPosition(0.0) + driveFalcon.setPosition(0.0) } override fun configureDrivePID( diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java new file mode 100644 index 00000000..2a2cd0b8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java @@ -0,0 +1,110 @@ +// Copyright 2021-2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads; + +import com.ctre.phoenix6.BaseStatusSignal; +import com.ctre.phoenix6.StatusSignal; +import com.ctre.phoenix6.hardware.ParentDevice; +import com.ctre.phoenix6.unmanaged.Unmanaged; +import com.team4099.robot2023.config.constants.DrivetrainConstants; +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain; + +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.concurrent.locks.Lock; +import java.util.concurrent.locks.ReentrantLock; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for Phoenix 6 devices on both the RIO and CANivore buses. When using + * a CANivore, the thread uses the "waitForAll" blocking method to enable more consistent sampling. + * This also allows Phoenix Pro users to benefit from lower latency between devices using CANivore + * time synchronization. + */ +public class PhoenixOdometryThread extends Thread { + private final Lock signalsLock = + new ReentrantLock(); // Prevents conflicts when registering signals + private BaseStatusSignal[] signals = new BaseStatusSignal[0]; + private final List> queues = new ArrayList<>(); + private boolean isCANFD = false; + + private static PhoenixOdometryThread instance = null; + + public static PhoenixOdometryThread getInstance() { + if (instance == null) { + instance = new PhoenixOdometryThread(); + } + return instance; + } + + private PhoenixOdometryThread() { + setName("PhoenixOdometryThread"); + setDaemon(true); + start(); + } + + public Queue registerSignal(ParentDevice device, StatusSignal signal) { + isCANFD = Unmanaged.isNetworkFD(device.getNetwork()); + Queue queue = new ArrayBlockingQueue<>(100); + signalsLock.lock(); + try { + BaseStatusSignal[] newSignals = new BaseStatusSignal[signals.length + 1]; + System.arraycopy(signals, 0, newSignals, 0, signals.length); + newSignals[signals.length] = signal; + signals = newSignals; + queues.add(queue); + + } finally { + signalsLock.unlock(); + } + return queue; + } + + @Override + public void run() { + while (true) { + // Wait for updates from all signals + signalsLock.lock(); + try { + if (isCANFD && signals.length > 0) { + BaseStatusSignal.waitForAll(2.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY, signals); + } else { + // "waitForAll" does not support blocking on multiple + // signals with a bus that is not CAN FD, regardless + // of Pro licensing. No reasoning for this behavior + // is provided by the documentation. + Thread.sleep((long) (1000.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY)); + BaseStatusSignal.refreshAll(signals); + } + } catch (InterruptedException e) { + e.printStackTrace(); + } finally { + signalsLock.unlock(); + } + + // Save new data to queues + Drivetrain.Companion.setOdometryLock(true); + try { + for (int i = 0; i < signals.length; i++) { + queues.get(i).offer(signals[i].getValueAsDouble()); + } + } finally { + Drivetrain.Companion.setOdometryLock(true); + } + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java new file mode 100644 index 00000000..36dcedee --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java @@ -0,0 +1,73 @@ +// Copyright 2021-2023 FRC 6328 +// http://github.com/Mechanical-Advantage +// +// This program is free software; you can redistribute it and/or +// modify it under the terms of the GNU General Public License +// version 3 as published by the Free Software Foundation or +// available in the root directory of this project. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU General Public License for more details. + +package com.team4099.robot2023.subsystems.drivetrain.swervemodule.threads; + +import com.team4099.robot2023.config.constants.DrivetrainConstants; +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain; +import edu.wpi.first.wpilibj.Notifier; +import java.util.ArrayList; +import java.util.List; +import java.util.Queue; +import java.util.concurrent.ArrayBlockingQueue; +import java.util.function.DoubleSupplier; + +/** + * Provides an interface for asynchronously reading high-frequency measurements to a set of queues. + * + *

This version is intended for devices like the SparkMax that require polling rather than a + * blocking thread. A Notifier thread is used to gather samples with consistent timing. + */ +public class SparkMaxOdometryThread { + private List signals = new ArrayList<>(); + private List> queues = new ArrayList<>(); + + private final Notifier notifier; + private static SparkMaxOdometryThread instance = null; + + public static SparkMaxOdometryThread getInstance() { + if (instance == null) { + instance = new SparkMaxOdometryThread(); + } + return instance; + } + + private SparkMaxOdometryThread() { + notifier = new Notifier(this::periodic); + notifier.setName("SparkMaxOdometryThread"); + notifier.startPeriodic(1.0 / DrivetrainConstants.OMOMETRY_UPDATE_FREQUENCY ); + } + + public Queue registerSignal(DoubleSupplier signal) { + Queue queue = new ArrayBlockingQueue<>(100); + Drivetrain.Companion.setOdometryLock(true); + try { + signals.add(signal); + queues.add(queue); + } finally { + Drivetrain.Companion.setOdometryLock(false); + } + return queue; + } + + private void periodic() { + Drivetrain.Companion.setOdometryLock(false); + try { + for (int i = 0; i < signals.size(); i++) { + queues.get(i).offer(signals.get(i).getAsDouble()); + } + } finally { + Drivetrain.Companion.setOdometryLock(false); + } + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt index b3f7a0bd..a738e9ae 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/falconspin/Motor.kt @@ -240,8 +240,8 @@ class Falcon500( if (falcon500.stickyFault_ReverseSoftLimit.value) { retVal.add("ReverseSoftLimit") } - if (falcon500.stickyFault_MissingRemoteSensor.value) { - retVal.add("MissingRemoteSensor") + if (falcon500.stickyFault_MissingDifferentialFX.value) { + retVal.add("MissingDifferentialFX") } if (falcon500.stickyFault_FusedSensorOutOfSync.value) { retVal.add("MissingRemoteSensor") diff --git a/vendordeps/Phoenix.json b/vendordeps/Phoenix.json index a5620fa9..a6983a59 100644 --- a/vendordeps/Phoenix.json +++ b/vendordeps/Phoenix.json @@ -1,25 +1,32 @@ { "fileName": "Phoenix6.json", "name": "CTRE-Phoenix (v6)", - "version": "23.2.2", - "frcYear": 2023, + "version": "24.0.0-beta-3", + "frcYear": 2024, "uuid": "e995de00-2c64-4df5-8831-c1441420ff19", "mavenUrls": [ "https://maven.ctr-electronics.com/release/" ], - "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2023-latest.json", + "jsonUrl": "https://maven.ctr-electronics.com/release/com/ctre/phoenix6/latest/Phoenix6-frc2024-beta-latest.json", + "conflictsWith": [ + { + "uuid": "3fcf3402-e646-4fa6-971e-18afe8173b1a", + "errorMessage": "The combined Phoenix-6-And-5 vendordep is no longer supported. Please remove the vendordep and instead add both the latest Phoenix 6 vendordep and Phoenix 5 vendordep.", + "offlineFileName": "Phoenix6And5.json" + } + ], "javaDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-java", - "version": "23.2.2" + "version": "24.0.0-beta-3" } ], "jniDependencies": [ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -32,7 +39,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -45,7 +52,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -58,7 +65,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -71,7 +78,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -84,7 +91,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -97,7 +104,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -110,7 +117,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -123,7 +130,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -136,7 +143,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.0.0-beta-3", "isJar": false, "skipInvalidPlatforms": true, "validPlatforms": [ @@ -151,7 +158,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "wpiapi-cpp", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_Phoenix6_WPI", "headerClassifier": "headers", "sharedLibrary": true, @@ -166,7 +173,7 @@ { "groupId": "com.ctre.phoenix6", "artifactId": "tools", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_PhoenixTools", "headerClassifier": "headers", "sharedLibrary": true, @@ -181,7 +188,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "wpiapi-cpp-sim", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_Phoenix6_WPISim", "headerClassifier": "headers", "sharedLibrary": true, @@ -196,7 +203,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "tools-sim", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_PhoenixTools_Sim", "headerClassifier": "headers", "sharedLibrary": true, @@ -211,7 +218,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonSRX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimTalonSRX", "headerClassifier": "headers", "sharedLibrary": true, @@ -226,7 +233,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simTalonFX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -241,7 +248,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simVictorSPX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimVictorSPX", "headerClassifier": "headers", "sharedLibrary": true, @@ -256,7 +263,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simPigeonIMU", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimPigeonIMU", "headerClassifier": "headers", "sharedLibrary": true, @@ -271,7 +278,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simCANCoder", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimCANCoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -286,7 +293,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProTalonFX", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProTalonFX", "headerClassifier": "headers", "sharedLibrary": true, @@ -301,7 +308,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProCANcoder", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProCANcoder", "headerClassifier": "headers", "sharedLibrary": true, @@ -316,7 +323,7 @@ { "groupId": "com.ctre.phoenix6.sim", "artifactId": "simProPigeon2", - "version": "23.2.2", + "version": "24.0.0-beta-3", "libName": "CTRE_SimProPigeon2", "headerClassifier": "headers", "sharedLibrary": true,