diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt index 6820ea13..4f1f2add 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/Constants.kt @@ -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 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 30158bc8..21d50ba4 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 @@ -23,8 +23,6 @@ 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 @@ -99,7 +97,7 @@ object GyroIOPigeon2 : GyroIO { pigeon2Configuration.MountPose.MountPoseRoll = GyroConstants.mountRoll.inRadians yawPositionQueue = - if (phoenixDrive) { + if (Constants.Drivetrain.DRIVETRAIN_TYPE == Constants.Drivetrain.DrivetrainType.PHOENIX_TALON) { PhoenixOdometryThread.getInstance().registerSignal(pigeon2, pigeon2.getYaw()) } else { SparkMaxOdometryThread.getInstance().registerSignal { 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 ea1b492e..ef618350 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,7 +43,7 @@ class SwerveModule(val io: SwerveModuleIO) { var modulePosition = SwerveModulePosition() - var positionDeltas: Array + var positionDeltas = mutableListOf() private var speedSetPoint: LinearVelocity = 0.feet.perSecond private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond @@ -115,7 +115,6 @@ class SwerveModule(val io: SwerveModuleIO) { drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } - positionDeltas = arrayOf() } fun updateInputs() { @@ -127,14 +126,15 @@ class SwerveModule(val io: SwerveModuleIO) { 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] = + positionDeltas.add( SwerveModulePosition( (newDrivePosition - lastDrivePosition).inMeters, newSteeringAngle.inRotation2ds ) + ) lastDrivePosition = newDrivePosition } 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 5def9c60..588d00a4 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 @@ -42,6 +42,7 @@ import org.team4099.lib.units.derived.Volt 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.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond import java.lang.Math.PI @@ -260,9 +261,9 @@ class SwerveModuleIOFalcon( (steeringFalcon.get() * RobotController.getBatteryVoltage()).volts inputs.driveStatorCurrent = driveStatorCurrentSignal.value.amps - inputs.driveSupplyCurrent = driveFalcon.supplyCurrent.value.amps - inputs.steeringStatorCurrent = steeringFalcon.statorCurrent.value.amps - inputs.steeringSupplyCurrent = steeringFalcon.statorCurrent.value.amps + inputs.driveSupplyCurrent = driveSupplyCurrentSignal.value.amps + inputs.steeringStatorCurrent = steeringStatorCurrentSignal.value.amps + inputs.steeringSupplyCurrent = steeringSupplyCurrentSignal.value.amps inputs.drivePosition = driveSensor.position inputs.steeringPosition = steeringSensor.position @@ -271,8 +272,8 @@ class SwerveModuleIOFalcon( inputs.steeringVelocity = steeringSensor.velocity // processor temp is also something we may want to log ? - inputs.driveTemp = driveFalcon.deviceTemp.value.celsius - inputs.steeringTemp = steeringFalcon.deviceTemp.value.celsius + inputs.driveTemp = driveTempSignal.value.celsius + inputs.steeringTemp = steeringTempSignal.value.celsius inputs.odometryDrivePositions = drivePositionQueue @@ -289,8 +290,7 @@ class SwerveModuleIOFalcon( steeringPositionQueue .stream() .map { value: Double -> - Rotation2d.fromRotations(value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO) - .radians + (value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO).rotations } .toArray() as Array @@ -334,7 +334,7 @@ class SwerveModuleIOFalcon( driveSensor.velocityToRawUnits(speed), driveSensor.accelerationToRawUnits(acceleration), DrivetrainConstants.FOC_ENABLED, - feedforward.inVolts / 12.0, + feedforward.inVolts / DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE.inVolts, 0, false ) @@ -370,6 +370,9 @@ class SwerveModuleIOFalcon( else (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) ) ) + + drivePositionQueue.clear() + steeringPositionQueue.clear() Logger.getInstance() .recordOutput("$label/zeroPositionRadians", steeringSensor.position.inRadians) println("Loading Zero for Module $label (${steeringFalcon.position.value})") @@ -377,6 +380,8 @@ class SwerveModuleIOFalcon( override fun zeroDrive() { driveFalcon.setPosition(0.0) + drivePositionQueue.clear() + steeringPositionQueue.clear() } 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 index 2a2cd0b8..e9a0a4a1 100644 --- 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 @@ -103,7 +103,7 @@ public void run() { queues.get(i).offer(signals[i].getValueAsDouble()); } } finally { - Drivetrain.Companion.setOdometryLock(true); + Drivetrain.Companion.setOdometryLock(false); } } } 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 index 36dcedee..07d34186 100644 --- 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 @@ -61,7 +61,7 @@ public Queue registerSignal(DoubleSupplier signal) { } private void periodic() { - Drivetrain.Companion.setOdometryLock(false); + Drivetrain.Companion.setOdometryLock(true); try { for (int i = 0; i < signals.size(); i++) { queues.get(i).offer(signals.get(i).getAsDouble());