diff --git a/simgui-ds.json b/simgui-ds.json index 69b1a3cb..1781fd7a 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -92,6 +92,12 @@ "robotJoysticks": [ { "guid": "Keyboard0" + }, + { + "guid": "Keyboard1" + }, + { + "guid": "Keyboard2" } ] } diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index d1f2f817..ea93bde2 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -6,10 +6,10 @@ package com.team4099.robot2023 const val MAVEN_GROUP = "" const val MAVEN_NAME = "Crescendo-2024" const val VERSION = "unspecified" -const val GIT_REVISION = 99 -const val GIT_SHA = "3676e5c0aa32041bab20dd30c8eb185a918d82dc" -const val GIT_DATE = "2024-01-29T15:42:24Z" +const val GIT_REVISION = 100 +const val GIT_SHA = "211385d92d6ac7f542123c55c842c94f23b08db0" +const val GIT_DATE = "2024-01-29T16:34:41Z" const val GIT_BRANCH = "drivetrain" -const val BUILD_DATE = "2024-01-29T16:27:27Z" -const val BUILD_UNIX_TIME = 1706563647813L +const val BUILD_DATE = "2024-01-31T16:13:12Z" +const val BUILD_UNIX_TIME = 1706735592229L const val DIRTY = 1 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 13aac5e5..dcd20adc 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -159,14 +159,14 @@ object DrivetrainConstants { // val DRIVE_KV = 2.2678.volts / 1.0.meters.perSecond // val DRIVE_KA = 0.40499.volts / 1.0.meters.perSecond.perSecond - val SIM_DRIVE_KS = 0.116970.volts + val SIM_DRIVE_KS = 0.0.volts val SIM_DRIVE_KV = 2.7.volts / 1.0.meters.perSecond val SIM_DRIVE_KP = 1.5.volts / 1.meters.perSecond val SIM_DRIVE_KI = 0.0.volts / (1.meters.perSecond * 1.seconds) val SIM_DRIVE_KD = 0.0.volts / 1.meters.perSecond.perSecond - val SIM_STEERING_KP = 0.4.volts.perDegree + val SIM_STEERING_KP = 0.3.volts.perDegree val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } 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 5be39a75..9a7b3c0d 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 @@ -41,10 +41,12 @@ import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians 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.inRadiansPerSecond import org.team4099.lib.units.perSecond import java.util.concurrent.locks.Lock import java.util.concurrent.locks.ReentrantLock @@ -267,7 +269,7 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB Logger.recordOutput("Drivetrain/setPointStates", SwerveModuleState.struct, *setPointStates.toTypedArray()) - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, Pose2dWPILIB.struct, odometryPose.pose2d) + Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians)) Logger.recordOutput( "Odometry/pose3d", Pose3dWPILIB.struct, Pose3d( @@ -305,7 +307,8 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB DrivetrainState.OPEN_LOOP -> { // Outputs setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) - + Logger.recordOutput("Drivetrain/TargetVelocityX", targetedDriveVector.first.inMetersPerSecond) + Logger.recordOutput("Drivetrain/TargetVelocityY", targetedDriveVector.second.inMetersPerSecond) // Transitions nextState = fromRequestToState(currentRequest) } 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 170212dd..adad2ec4 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 @@ -126,6 +126,8 @@ class SwerveModule(val io: SwerveModuleIO) { val deltaCount = Math.min(inputs.odometryDrivePositions.size, inputs.odometrySteeringPositions.size) + positionDeltas.clear() + for (i in 0..deltaCount-1) { val newDrivePosition = inputs.odometryDrivePositions[i] val newSteeringAngle = inputs.odometrySteeringPositions[i] @@ -137,6 +139,13 @@ class SwerveModule(val io: SwerveModuleIO) { lastDrivePosition = newDrivePosition } + if (positionDeltas.size > 0) { + Logger.recordOutput("Drivetrain/PositionDeltas", positionDeltas[0].distanceMeters) + } + else { + Logger.recordOutput("Drivetrain/PositionDeltas", -1337) + } + // 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 8e22b7f9..a17eff62 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 @@ -13,6 +13,7 @@ import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inAmperes import org.team4099.lib.units.base.inCelsius +import org.team4099.lib.units.base.inDecameters import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.Angle @@ -22,6 +23,7 @@ import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Radian import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.radians @@ -73,6 +75,18 @@ interface SwerveModuleIO { table?.put("potentiometerOutputRaw", potentiometerOutputRaw) table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) table?.put("driftPositionMeters", drift.inMeters) + + if (odometryDrivePositions.size > 0) { + table?.put("odometryDrivePositionsMeters", odometryDrivePositions[0].inMeters) + } else { + table?.put("odometryDrivePositionsMeters", 0.0) + } + if (odometrySteeringPositions.size > 0) { + table?.put("odometrySteeringPositionsDegrees", odometrySteeringPositions[0].inDegrees) + } else { + table?.put("odometrySteeringPositionsDegrees", 0.0) + } + } override fun fromLog(table: LogTable?) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt index 2edc5deb..9378e73c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt @@ -121,6 +121,9 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR } + var driveAppliedVolts = 0.0.volts + var turnAppliedVolts = 0.0.volts + override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) @@ -169,7 +172,7 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { inputs.driveVelocity = driveVelocity inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond - inputs.driveAppliedVoltage = (-1337).volts + inputs.driveAppliedVoltage = driveAppliedVolts inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps inputs.driveStatorCurrent = (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator @@ -178,7 +181,7 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { inputs.driveTemp = (-1337).celsius inputs.steeringTemp = (-1337).celsius - inputs.steeringAppliedVoltage = (-1337).volts + inputs.steeringAppliedVoltage = turnAppliedVolts inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps inputs.steeringStatorCurrent = (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator @@ -201,12 +204,12 @@ class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { // helper functions to clamp all inputs and set sim motor voltages properly private fun setDriveVoltage(volts: ElectricalPotential) { - val driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + driveAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) } private fun setSteeringVoltage(volts: ElectricalPotential) { - val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) }