From 92a0dfba0335b3706d1bd23c5b8b44d024c24377 Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Fri, 12 Jan 2024 18:49:53 -0500 Subject: [PATCH 01/38] elevatorIO --- .../subsystems/elevator/elevatorIO.kt | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt new file mode 100644 index 00000000..214b135f --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt @@ -0,0 +1,111 @@ +package com.team4099.robot2023.subsystems.elevator + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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 +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inCelsius +import org.team4099.lib.units.base.inInches +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inInchesPerSecond +import org.team4099.lib.units.perSecond + +interface elevatorIO { + class ElevatorInputs : LoggableInputs { + var elevatorPosition = 0.0.inches + var elevatorVelocity = 0.0.inches.perSecond + + var leaderAppliedVoltage = 0.0.volts + var followerAppliedVoltage = 0.0.volts + + var leaderSupplyCurrent = 0.0.amps + var leaderStatorCurrent = 0.0.amps + + var followerSupplyCurrent = 0.0.amps + var followerStatorCurrent = 0.0.amps + + var leaderTempCelcius = 0.0.celsius + var followerTempCelcius = 0.0.celsius + + var leaderRawPosition = 0.0 + var followerRawPosition = 0.0 + + var isSimulating = false + + override fun toLog(table: LogTable) { + table?.put("elevatorPositionInches", elevatorPosition.inInches) + table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) + table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) + table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) + table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) + table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) + table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) + table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) + table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) + table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) + table?.put("elevatorLeaderRawPosition", leaderRawPosition) + table?.put("elevatorFollowRawPosition", followerRawPosition) + } + + override fun fromLog(table: LogTable?) { + table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { + elevatorPosition = it.inches + } + table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { + elevatorVelocity = it.inches.perSecond + } + table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { + leaderAppliedVoltage = it.volts + } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { + leaderStatorCurrent = it.amps + } + table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { + leaderSupplyCurrent = it.amps + } + table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { + leaderTempCelcius = it.celsius + } + + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { + followerStatorCurrent = it.amps + } + table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { + followerSupplyCurrent = it.amps + } + table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { + followerTempCelcius = it.celsius + } + table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { + leaderRawPosition = it + } + table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { + followerRawPosition = it + } + } + fun updateInputs(inputs: ElevatorInputs) {} + fun setOutputVoltage(voltage: ElectricalPotential) {} + fun setPosition(position: Length, feedForward: ElectricalPotential) {} + fun zeroEncoder() {} + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + } From e6b176c96b3b53b21beed22b67b7d54cc0f72bad Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Sat, 13 Jan 2024 20:03:50 -0500 Subject: [PATCH 02/38] started Elevator.kt file --- .../config/constants/ElevatorConstants.kt | 23 +++++++ .../robot2023/subsystems/elevator/Elevator.kt | 67 +++++++++++++++++++ .../elevator/{elevatorIO.kt => ElevatorIO.kt} | 9 ++- .../subsystems/elevator/ElevatorIOKraken.kt | 4 ++ 4 files changed, 100 insertions(+), 3 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt rename src/main/kotlin/com/team4099/robot2023/subsystems/elevator/{elevatorIO.kt => ElevatorIO.kt} (98%) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt new file mode 100644 index 00000000..7ec75dfb --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -0,0 +1,23 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object ElevatorConstants { + //TODO: change values later + val ELEVATOR_KS = 0.0.volts + val ELEVATOR_KG = 0.0.volts + val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond + val ELEVATOR_KA = 0.0.volts/0.0.inches.perSecond.perSecond + + val ENABLE_ELEVATOR = 1.0 + val ELEVATOR_IDLE_HEIGHT = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches + val SHOOT_SPEAKER_POSITION = 0.0.inches + val SHOOT_AMP_POSITION = 0.0.inches + val SOURCE_NOTE_OFFSET = 0.0.inches + val ELEVATOR_THETA_POS = 0.0.degrees +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt new file mode 100644 index 00000000..d793b6d6 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -0,0 +1,67 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.ElevatorConstants +import org.team4099.lib.controller.ElevatorFeedforward +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perSecond +import org.team4099.lib.units.base.inInches + +class Elevator(val io: ElevatorIO) { + val inputs = ElevatorIO.ElevatorInputs() + private var elevatorFeedforward : ElevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + private val kP = LoggedTunableValue("Elevator/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = LoggedTunableValue( + "Elevator/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + ) + private val kD = LoggedTunableValue( + "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + ) + object TunableElevatorHeights { + val enableElevator = + LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) + val minPosition = + LoggedTunableValue( + "Elevator/minPosition", + ElevatorConstants.ELEVATOR_IDLE_HEIGHT, + Pair({ it.inInches }, { it.inches }) + ) + + val maxPosition = + LoggedTunableValue( + "Elevator/maxPosition", + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, + Pair({ it.inInches }, { it.inches }) + ) + //TODO: change voltages + val openLoopExtendVoltage = + LoggedTunableValue( + "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts }) + ) + + val openLoopRetractVoltage = + LoggedTunableValue( + "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts }) + ) + + val shootSpeakerPosition = LoggedTunableValue("Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION) + val shootAmpPosition = LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION) + val sourceNoteOffset = LoggedTunableValue("Elevator/sourceNoteOffset", ElevatorConstants.SOURCE_NOTE_OFFSET) + val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) + val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) + val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) + val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) + val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) + val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) + val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) + val thetaPos1 = LoggedTunableValue("Elevator/thetaPos1", ElevatorConstants.ELEVATOR_THETA_POS, Pair({ it.inDegrees }, { it.degrees })) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt similarity index 98% rename from src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt rename to src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index 214b135f..9f48ef8d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/elevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -20,7 +20,7 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.perSecond -interface elevatorIO { +interface ElevatorIO { class ElevatorInputs : LoggableInputs { var elevatorPosition = 0.0.inches var elevatorVelocity = 0.0.inches.perSecond @@ -98,7 +98,8 @@ interface elevatorIO { table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { followerRawPosition = it } - } + } + fun updateInputs(inputs: ElevatorInputs) {} fun setOutputVoltage(voltage: ElectricalPotential) {} fun setPosition(position: Length, feedForward: ElectricalPotential) {} @@ -107,5 +108,7 @@ interface elevatorIO { kP: ProportionalGain, kI: IntegralGain, kD: DerivativeGain - ) {} + ) { + } } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt new file mode 100644 index 00000000..67de1ff8 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.elevator + +object ElevatorIOKraken { +} \ No newline at end of file From 1af374c6eb1a3b59ddd2b38e1b09473044ab391f Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Sat, 13 Jan 2024 20:09:50 -0500 Subject: [PATCH 03/38] Reformatted Code --- .../robot2023/subsystems/elevator/Elevator.kt | 52 ++++++++++++++----- 1 file changed, 38 insertions(+), 14 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index d793b6d6..e5bd4b1a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -18,43 +18,64 @@ class Elevator(val io: ElevatorIO) { ElevatorConstants.ELEVATOR_KV, ElevatorConstants.ELEVATOR_KA ) - private val kP = LoggedTunableValue("Elevator/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + + private val kP = LoggedTunableValue( + "Elevator/kP", + Pair({ it.inVoltsPerInch }, { it.volts.perInch }) + ) private val kI = LoggedTunableValue( - "Elevator/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + "Elevator/kI", + Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) private val kD = LoggedTunableValue( - "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + "Elevator/kD", + Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) + object TunableElevatorHeights { val enableElevator = LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) + val minPosition = LoggedTunableValue( - "Elevator/minPosition", - ElevatorConstants.ELEVATOR_IDLE_HEIGHT, - Pair({ it.inInches }, { it.inches }) + "Elevator/minPosition", + ElevatorConstants.ELEVATOR_IDLE_HEIGHT, + Pair({ it.inInches }, { it.inches }) ) - val maxPosition = LoggedTunableValue( "Elevator/maxPosition", ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, Pair({ it.inInches }, { it.inches }) ) + //TODO: change voltages val openLoopExtendVoltage = LoggedTunableValue( - "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts }) + "Elevator/openLoopExtendVoltage", + 8.volts, + Pair({ it.inVolts }, { it.volts }) ) - val openLoopRetractVoltage = LoggedTunableValue( - "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts }) + "Elevator/openLoopRetractVoltage", + -12.0.volts, + Pair({ it.inVolts }, { it.volts }) ) - val shootSpeakerPosition = LoggedTunableValue("Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION) - val shootAmpPosition = LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION) - val sourceNoteOffset = LoggedTunableValue("Elevator/sourceNoteOffset", ElevatorConstants.SOURCE_NOTE_OFFSET) + val shootSpeakerPosition = LoggedTunableValue( + "Elevator/shootSpeakerPosition", + ElevatorConstants.SHOOT_SPEAKER_POSITION + ) + val shootAmpPosition = LoggedTunableValue( + "Elevator/shootAmpPosition", + ElevatorConstants.SHOOT_AMP_POSITION + ) + val sourceNoteOffset = LoggedTunableValue( + "Elevator/sourceNoteOffset", + ElevatorConstants.SOURCE_NOTE_OFFSET + ) + val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) @@ -62,6 +83,9 @@ class Elevator(val io: ElevatorIO) { val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) - val thetaPos1 = LoggedTunableValue("Elevator/thetaPos1", ElevatorConstants.ELEVATOR_THETA_POS, Pair({ it.inDegrees }, { it.degrees })) + val thetaPos1 = LoggedTunableValue("Elevator/thetaPos1", + ElevatorConstants.ELEVATOR_THETA_POS, + Pair({ it.inDegrees }, { it.degrees }) + ) } } \ No newline at end of file From b161b28140ebeffa157368f87dd9860df09863ec Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Mon, 15 Jan 2024 19:40:58 -0500 Subject: [PATCH 04/38] Added Elevator.kt variables --- .../config/constants/ElevatorConstants.kt | 12 ++- .../robot2023/subsystems/elevator/Elevator.kt | 83 +++++++++++++++++++ 2 files changed, 94 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 7ec75dfb..e2ec9497 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -1,13 +1,14 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond object ElevatorConstants { - //TODO: change values later + // TODO: Change values later val ELEVATOR_KS = 0.0.volts val ELEVATOR_KG = 0.0.volts val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond @@ -16,6 +17,15 @@ object ElevatorConstants { val ENABLE_ELEVATOR = 1.0 val ELEVATOR_IDLE_HEIGHT = 0.0.inches val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches + + val ELEVATOR_TOLERANCE = 0.0.inches + + val MAX_VELOCITY = 0.0.meters.perSecond + val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond + val SHOOT_SPEAKER_POSITION = 0.0.inches val SHOOT_AMP_POSITION = 0.0.inches val SOURCE_NOTE_OFFSET = 0.0.inches diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index e5bd4b1a..085b824d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -1,13 +1,18 @@ package com.team4099.robot2023.subsystems.elevator +import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest import org.team4099.lib.controller.ElevatorFeedforward +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.* import org.team4099.lib.units.perSecond import org.team4099.lib.units.base.inInches +import kotlin.time.Duration.Companion.seconds class Elevator(val io: ElevatorIO) { val inputs = ElevatorIO.ElevatorInputs() @@ -88,4 +93,82 @@ class Elevator(val io: ElevatorIO) { Pair({ it.inDegrees }, { it.degrees }) ) } + + val forwardLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + val reversedLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION + + val forwardOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION + val reverseOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION + + var isHomed = false + + var currentState: ElevatorState = ElevatorState.UNINITIALIZED + var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) + set(value) { + when(value) { + is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage + is ElevatorRequest.TargetingPosition -> { + elevatorPositionTarget = value.position + elevatorVelocityTarget = value.finalVelocity + } + else -> {} + } + field = value + } + + var elevatorPositionTarget = 0.0.inches + private set + var elevatorVelocityTarget = 0.0.inches.perSecond + private set + var elevatorVoltageTarget = 0.0.volts + private set + + private var lastRequestedPosition = -9999.inches + private var lastRequestedVelocity = -9999.inches.perSecond + private var lastRequestedVoltage = -9999.volts + + private var timeProfileGeneratedAt = Clock.fpgaTime + + private var lastHomingStatorCurrentTripTime = -9999.seconds + + private var elevatorConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION) + private var elevatorSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + private var elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) + ) + + val isAtTargetedPosition: Boolean + get() = + (currentRequest is ElevatorRequest.TargetingPosition && + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + ElevatorConstants.ELEVATOR_TOLERANCE) || + (TunableElevatorHeights.enableElevator.get() != 1.0) + + val canContinueSafely: Boolean + get() = + currentRequest is ElevatorRequest.TargetingPosition && ( + ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) + ) && lastRequestedPosition == elevatorPositionTarget + + + companion object { + enum class ElevatorState { + UNINITIALIZED, + IDLE, + TARGETING_POSITION, + OPEN_LOOP, + HOME + } + } } \ No newline at end of file From 6946de714761765f9033e01ab046cf8b10d65e11 Mon Sep 17 00:00:00 2001 From: CodingMaster121 Date: Mon, 15 Jan 2024 20:02:33 -0500 Subject: [PATCH 05/38] Added init for Elevator.kt --- .../config/constants/ElevatorConstants.kt | 10 +++++- .../robot2023/subsystems/elevator/Elevator.kt | 34 +++++++++++++++++++ .../subsystems/elevator/ElevatorIO.kt | 21 ++++++------ 3 files changed, 53 insertions(+), 12 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index e2ec9497..d83e6486 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -8,7 +8,15 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond object ElevatorConstants { - // TODO: Change values later + // TODO: Change values later based on CAD + val REAL_KP = 0.0.volts / 1.inches + val REAL_KI = 0.0.volts / (1.inches * 1.seconds) + val REAL_KD = 0.0.volts / (1.inches.perSecond) + + val SIM_KP = 0.0.volts / 1.inches + val SIM_KI = 0.0.volts / (1.inches * 1.seconds) + val SIM_KD = 0.0.volts / (1.inches.perSecond) + val ELEVATOR_KS = 0.0.volts val ELEVATOR_KG = 0.0.volts val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 085b824d..39ea4b90 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.ElevatorConstants +import edu.wpi.first.wpilibj.RobotBase import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest import org.team4099.lib.controller.ElevatorFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -135,6 +136,7 @@ class Elevator(val io: ElevatorIO) { private var lastHomingStatorCurrentTripTime = -9999.seconds + // Trapezoidal Profile Constraints private var elevatorConstraints: TrapezoidProfile.Constraints = TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION) private var elevatorSetpoint: TrapezoidProfile.State = @@ -162,6 +164,38 @@ class Elevator(val io: ElevatorIO) { ) && lastRequestedPosition == elevatorPositionTarget + init { + TunableElevatorHeights + + // Initializes PID constants and changes FF depending on if sim or real + if(RobotBase.isReal()) { + isHomed = false + + kP.initDefault(ElevatorConstants.REAL_KP) + kI.initDefault(ElevatorConstants.REAL_KI) + kD.initDefault(ElevatorConstants.REAL_KD) + } else { + isHomed = true + + kP.initDefault(ElevatorConstants.SIM_KP) + kI.initDefault(ElevatorConstants.SIM_KI) + kD.initDefault(ElevatorConstants.SIM_KD) + } + + elevatorFeedforward = ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + + io.configPID(kP.get(), kI.get(), kD.get()) + } + + fun periodic() { + + } + companion object { enum class ElevatorState { UNINITIALIZED, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index 9f48ef8d..d5c27eb3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -99,16 +99,15 @@ interface ElevatorIO { followerRawPosition = it } } - - fun updateInputs(inputs: ElevatorInputs) {} - fun setOutputVoltage(voltage: ElectricalPotential) {} - fun setPosition(position: Length, feedForward: ElectricalPotential) {} - fun zeroEncoder() {} - fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - } } + + fun updateInputs(inputs: ElevatorInputs) {} + fun setOutputVoltage(voltage: ElectricalPotential) {} + fun setPosition(position: Length, feedForward: ElectricalPotential) {} + fun zeroEncoder() {} + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} } From f5faca7c2dc586065fe422ce056809f7687c082d Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Tue, 16 Jan 2024 20:38:58 -0500 Subject: [PATCH 06/38] finished elevator.kt file --- .../config/constants/ElevatorConstants.kt | 5 + .../robot2023/subsystems/elevator/Elevator.kt | 129 +++++++++++++++++- .../subsystems/superstructure/Request.kt | 2 +- 3 files changed, 131 insertions(+), 5 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index d83e6486..5857342a 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -1,5 +1,6 @@ package com.team4099.robot2023.config.constants +import org.team4099.lib.units.base.amps import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds @@ -38,4 +39,8 @@ object ElevatorConstants { val SHOOT_AMP_POSITION = 0.0.inches val SOURCE_NOTE_OFFSET = 0.0.inches val ELEVATOR_THETA_POS = 0.0.degrees + val HOMING_STATOR_CURRENT = 0.0.amps + val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds + val HOMING_APPLIED_VOLTAGE = 0.0.volts + val ELEVATOR_GROUND_OFFSET = 0.0.inches } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 39ea4b90..34a63bd1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -3,7 +3,9 @@ package com.team4099.robot2023.subsystems.elevator import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants +import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.RobotBase import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest import org.team4099.lib.controller.ElevatorFeedforward @@ -13,6 +15,9 @@ import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.* import org.team4099.lib.units.perSecond import org.team4099.lib.units.base.inInches +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.inInchesPerSecond import kotlin.time.Duration.Companion.seconds class Elevator(val io: ElevatorIO) { @@ -97,7 +102,7 @@ class Elevator(val io: ElevatorIO) { val forwardLimitReached: Boolean get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION - val reversedLimitReached: Boolean + val reverseLimitReached: Boolean get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION val forwardOpenLoopLimitReached: Boolean @@ -134,7 +139,7 @@ class Elevator(val io: ElevatorIO) { private var timeProfileGeneratedAt = Clock.fpgaTime - private var lastHomingStatorCurrentTripTime = -9999.seconds + private var lastHomingStatorCurrentTripTime = Clock.fpgaTime // Trapezoidal Profile Constraints private var elevatorConstraints: TrapezoidProfile.Constraints = @@ -193,16 +198,132 @@ class Elevator(val io: ElevatorIO) { } fun periodic() { + io.updateInputs(inputs) + if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + Logger.processInputs("Elevator", inputs) + Logger.recordOutput("Elevator/currentState", currentState.name) + Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) + Logger.recordOutput("Elevator/elevatorHeight", inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET) + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("Elevator/isHomed", isHomed) + Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) + + Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) + Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) + + Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) + Logger.recordOutput("Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond) + Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) + + Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) + Logger.recordOutput( + "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) + + Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) + Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + } + var nextState = currentState + when (currentState) { + ElevatorState.UNINITIALIZED -> { + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.OPEN_LOOP -> { + setOutputVoltage(elevatorVoltageTarget) + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.TARGETING_POSITION -> { + if ((elevatorPositionTarget != lastRequestedPosition) || (elevatorVelocityTarget != lastRequestedVelocity)) { + val preProfileGenerate = Clock.realTimestamp + elevatorProfile = TrapezoidProfile(elevatorConstraints, TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity)) + val postProfileGenerate = Clock.realTimestamp + Logger.recordOutput("/Elevator/profileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) + Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) + Logger.recordOutput("Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond) + timeProfileGeneratedAt = Clock.fpgaTime + lastRequestedPosition = elevatorPositionTarget + lastRequestedVelocity = elevatorVelocityTarget + } + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val profilePosition = elevatorProfile.calculate(timeElapsed) + setPosition(profilePosition) + Logger.recordOutput("Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed)) + Logger.recordOutput("Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond) + Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) + nextState = fromElevatorRequestToState(currentRequest) + if (! (currentState.equivalentToRequest(currentRequest))) { + lastRequestedVelocity = -999.inches.perSecond + lastRequestedPosition = -999.inches + } + } + ElevatorState.HOME -> { + if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { + lastHomingStatorCurrentTripTime = Clock.fpgaTime + } + if (! inputs.isSimulating && (! isHomed && inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && Clock.fpgaTime - lastHomingStatorCurrentTripTime < ElevatorConstants.HOMING_STALL_TIME_THRESHOLD)) { + setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + } + else { + zeroEncoder() + isHomed = true + } + if (isHomed) { + nextState = fromElevatorRequestToState(currentRequest) + } + } + } + currentState = nextState + } + + fun setOutputVoltage(voltage: ElectricalPotential) { + if ((forwardLimitReached) && (voltage > 0.volts) || (reverseLimitReached) && (voltage < 0.volts)) { + io.setOutputVoltage(0.volts) + } + else { + io.setOutputVoltage(voltage) + } + } + + fun setHomeVoltage(voltage: ElectricalPotential) { + io.setOutputVoltage(voltage) + } + fun zeroEncoder() { + io.zeroEncoder() + } + + fun setPosition(setpoint : TrapezoidProfile.State < Meter >) { + val elevatorAcceleration = (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + elevatorSetpoint = setpoint + val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) + if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition)) { + io.setOutputVoltage(0.volts) + } + else { + io.setPosition(setpoint.position, feedForward) + } } companion object { enum class ElevatorState { UNINITIALIZED, - IDLE, TARGETING_POSITION, OPEN_LOOP, - HOME + HOME; + inline fun equivalentToRequest(request : ElevatorRequest) : Boolean { + return (request is ElevatorRequest.Home && this == HOME) || (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) + } + } + + inline fun fromElevatorRequestToState(request : ElevatorRequest) : ElevatorState { + return when(request) { + is ElevatorRequest.Home -> ElevatorState.HOME + is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP + is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION + } } } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 65fe9e78..8b38be20 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -56,7 +56,7 @@ sealed interface Request { val canContinueBuffer: Length = 5.0.inches ) : ElevatorRequest class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest - class Home : ElevatorRequest + class Home() : ElevatorRequest } sealed interface GroundIntakeRequest : Request { From 27a25739eaa8b7d0c3bd69dcd4450d1e8f94b1fa Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Wed, 17 Jan 2024 19:01:38 -0500 Subject: [PATCH 07/38] started hardware code --- .../robot2023/config/constants/ElevatorConstants.kt | 3 +++ .../robot2023/subsystems/elevator/ElevatorIOKraken.kt | 10 +++++++++- 2 files changed, 12 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 5857342a..4c27e1d5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -43,4 +43,7 @@ object ElevatorConstants { val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds val HOMING_APPLIED_VOLTAGE = 0.0.volts val ELEVATOR_GROUND_OFFSET = 0.0.inches + + val LEADER_VOLTAGE = 0.0.volts + val LEADER_GEAR_RATIO = 0.0 } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 67de1ff8..984c3bda 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -1,4 +1,12 @@ package com.team4099.robot2023.subsystems.elevator -object ElevatorIOKraken { +import com.ctre.phoenix6.hardware.TalonFX +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import org.team4099.lib.units.ctreLinearMechanismSensor + +object ElevatorIOKraken: ElevatorIO { + private val elevatorLeaderKraken = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) + private val elevatorFollowerKraken = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) + private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_VOLTAGE) } \ No newline at end of file From 51c66edad7aa62d52cfb124a29556e6f35d921bb Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Thu, 18 Jan 2024 19:16:53 -0500 Subject: [PATCH 08/38] started kraken hardware file --- .../config/constants/ElevatorConstants.kt | 24 +++++++++ .../subsystems/elevator/ElevatorIOKraken.kt | 52 +++++++++++++++++-- 2 files changed, 73 insertions(+), 3 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 4c27e1d5..204f827d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -5,7 +5,10 @@ import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.perInch +import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object ElevatorConstants { @@ -46,4 +49,25 @@ object ElevatorConstants { val LEADER_VOLTAGE = 0.0.volts val LEADER_GEAR_RATIO = 0.0 + val LEADER_SENSOR_CPR = 0 + val LEADER_DIAMETER = 0.0.inches + val LEADER_KP = 0.0.volts/1.0.inches.perSecond + val LEADER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds) + val LEADER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds) + val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + + val FOLLOWER_VOLTAGE = 0.0.volts + val FOLLOWER_GEAR_RATIO = 0.0 + val FOLLOWER_SENSOR_CPR = 0 + val FOLLOWER_DIAMETER = 0.0.inches + val FOLLOWER_KP = 0.0.volts/1.0.inches.perSecond + val FOLLOWER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds) + val FOLLOWER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds) + val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 984c3bda..09a25724 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -1,12 +1,58 @@ package com.team4099.robot2023.subsystems.elevator +import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.hardware.TalonFX import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreLinearMechanismSensor object ElevatorIOKraken: ElevatorIO { - private val elevatorLeaderKraken = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) - private val elevatorFollowerKraken = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) - private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_VOLTAGE) + private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) + private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) + private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_SENSOR_CPR, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_DIAMETER, ElevatorConstants.LEADER_VOLTAGE) + private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.FOLLOWER_SENSOR_CPR, ElevatorConstants.FOLLOWER_GEAR_RATIO, ElevatorConstants.FOLLOWER_DIAMETER, ElevatorConstants.FOLLOWER_VOLTAGE) + private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + lateinit var elevatorLeaderStatorCurrentSignal: StatusSignal + lateinit var elevatorLeaderSupplyCurrentSignal: StatusSignal + lateinit var elevatorLeadertempSignal: StatusSignal + lateinit var elevatorLeaderDutyCycle: StatusSignal + lateinit var elevatorFollowerStatorCurrentSignal: StatusSignal + lateinit var elevatorFollowerSupplyCurrentSignal: StatusSignal + lateinit var elevatorFollowertempSignal: StatusSignal + lateinit var elevatorFollowerDutyCycle: StatusSignal + + init { + elevatorLeaderKraken.clearStickyFaults() + elevatorFollowerKraken.clearStickyFaults() + elevatorLeaderConfiguration.Slot0.kP = leaderSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.LEADER_KP) + elevatorLeaderConfiguration.Slot0.kI = leaderSensor.integralVelocityGainToRawUnits(ElevatorConstants.LEADER_KI) + elevatorLeaderConfiguration.Slot0.kD = leaderSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.LEADER_KD) + + elevatorFollowerConfiguration.Slot0.kP = followerSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KP) + elevatorFollowerConfiguration.Slot0.kI = followerSensor.integralVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KI) + elevatorFollowerConfiguration.Slot0.kD = followerSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KD) + + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) + elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) + + } } \ No newline at end of file From bd74ffef454b48c92c7f6c6eba36dfa0d6bd7538 Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Fri, 19 Jan 2024 19:46:41 -0500 Subject: [PATCH 09/38] finished hardware file --- .../subsystems/elevator/ElevatorIOKraken.kt | 60 ++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 09a25724..1ef1d354 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -1,13 +1,21 @@ package com.team4099.robot2023.subsystems.elevator import com.ctre.phoenix6.StatusSignal +import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration +import com.ctre.phoenix6.controls.PositionDutyCycle 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.ElevatorConstants -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds +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.controller.PIDController +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.* import org.team4099.lib.units.ctreLinearMechanismSensor +import org.team4099.lib.units.derived.* object ElevatorIOKraken: ElevatorIO { private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) @@ -51,8 +59,56 @@ object ElevatorIOKraken: ElevatorIO { elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) + elevatorLeaderKraken.inverted = true + elevatorFollowerKraken.inverted = false + MotorChecker.add("Elevator", "Extension", MotorCollection(mutableListOf( + Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), (Falcon500(elevatorFollowerKraken, "Follower Extension Motor"))), ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, 30.celsius, ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT-30.amps, 110.celsius)) + elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent + elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent + elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp + elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle + elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent + elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent + elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp + elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle + } + + override fun configPID(kP: ProportionalGain, kI: IntegralGain, kD: DerivativeGain) { + val pidController = Slot0Configs() + pidController.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) + pidController.kI = leaderSensor.integralPositionGainToRawUnits(kI) + pidController.kD = leaderSensor.derivativePositionGainToRawUnits(kD) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + inputs.elevatorVelocity = leaderSensor.velocity + inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts + inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts + inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps + inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps + inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps + inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps + inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius + inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius + inputs.leaderRawPosition = leaderSensor.getRawPosition() + inputs.followerRawPosition = followerSensor.getRawPosition() + } + + override fun setOutputVoltage(voltage: ElectricalPotential) { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } + + override fun setPosition(position: Length, feedForward: ElectricalPotential) { + elevatorLeaderKraken.setControl(PositionDutyCycle(leaderSensor.positionToRawUnits(position), 0.0, true, feedForward.inVolts, 0, false, false, false)) + } + override fun zeroEncoder() { + elevatorLeaderKraken.setPosition(0.0) + elevatorFollowerKraken.setPosition(0.0) } } \ No newline at end of file From 3a372084081d54c07f53ba286152dbe3c53cc7c2 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 9 Jan 2024 18:48:00 -0500 Subject: [PATCH 10/38] toLog IO file --- .../subsystems/GroundIntake/GroundIntake.kt | 4 -- .../robot2023/subsystems/intake/Intake.kt | 4 ++ .../robot2023/subsystems/intake/IntakeIO.kt | 38 +++++++++++++++++++ 3 files changed, 42 insertions(+), 4 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt deleted file mode 100644 index 5fdc7677..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/GroundIntake/GroundIntake.kt +++ /dev/null @@ -1,4 +0,0 @@ -package com.team4099.robot2023.subsystems.GroundIntake - -class GroundIntake { -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt new file mode 100644 index 00000000..25f7823f --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -0,0 +1,4 @@ +package com.team4099.robot2023.subsystems.intake + +class Intake { +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt new file mode 100644 index 00000000..ba1bd94c --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -0,0 +1,38 @@ +package com.team4099.robot2023.subsystems.intake + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +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.meters +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerMinute +import org.team4099.lib.units.perMinute +import org.team4099.lib.units.perSecond + +interface IntakeIO { + class IntakeIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + + var rollerAppliedVoltage = 0.0.volts + var rollerSupplyCurrent = 0.0.amps + var rollerStatorCurrent = 0.0.amps + var rollerTemp = 0.0.celsius + + override fun toLog(table: LogTable?) { + table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) + + table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + + table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) + + table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) + + table?.put("rollerTempCelsius", rollerTemp.inCelsius) + } + } +} \ No newline at end of file From d1711e7510706c7b63546e00e308feb9c19212dc Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Tue, 9 Jan 2024 19:34:48 -0500 Subject: [PATCH 11/38] Finished IntakeIO --- .../config/constants/IntakeConstants.kt | 21 ++ .../robot2023/subsystems/Shooter/Shooter.kt | 3 +- .../TelescopingArm/TelescopingArm.kt | 3 +- .../robot2023/subsystems/feeder/Feeder.kt | 3 +- .../robot2023/subsystems/feeder/FeederIO.kt | 3 +- .../robot2023/subsystems/intake/Intake.kt | 3 +- .../robot2023/subsystems/intake/IntakeIO.kt | 124 ++++++++- .../subsystems/intake/IntakeIONEO.kt | 256 ++++++++++++++++++ 8 files changed, 391 insertions(+), 25 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt new file mode 100644 index 00000000..b3be6deb --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -0,0 +1,21 @@ +package com.team4099.robot2023.config.constants + +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts + +object IntakeConstants { + val VOLTAGE_COMPENSATION = 12.0.volts + + // TODO: Add value for encoder offset + val ABSOLUTE_ENCODER_OFFSET = 0.0.degrees + + // TODO: Change gear ratio according to robot + val ROLLER_CURRENT_LIMIT = 50.0.amps + val ARM_CURRENT_LIMIT = 50.0.amps + const val ROLLER_MOTOR_INVERTED = true + const val ARM_MOTOR_INVERTED = false + const val ROLLER_GEAR_RATIO = 36.0 / 18.0 + const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) + const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0 +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt index df493fe8..c1ef2b6f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/Shooter/Shooter.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.Shooter -class Shooter { -} \ No newline at end of file +class Shooter diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt index a1af62e2..53e9e9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/TelescopingArm/TelescopingArm.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.TelescopingArm -class TelescopingArm { -} \ No newline at end of file +class TelescopingArm diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt index bb51b835..f7209b2a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/Feeder.kt @@ -1,6 +1,5 @@ package com.team4099.robot2023.subsystems.feeder import edu.wpi.first.wpilibj2.command.SubsystemBase -class Feeder(val io: FeederIO) : SubsystemBase() { -} \ No newline at end of file +class Feeder(val io: FeederIO) : SubsystemBase() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt index 41ddbf24..780fc386 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/feeder/FeederIO.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.feeder -interface FeederIO { -} \ No newline at end of file +interface FeederIO diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 25f7823f..4e49f3d6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -1,4 +1,3 @@ package com.team4099.robot2023.subsystems.intake -class Intake { -} \ No newline at end of file +class Intake diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index ba1bd94c..b06c5c0a 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -6,33 +6,127 @@ 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.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +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.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond interface IntakeIO { - class IntakeIOInputs : LoggableInputs { - var rollerVelocity = 0.0.rotations.perMinute + class IntakeIOInputs : LoggableInputs { + var rollerVelocity = 0.0.rotations.perMinute + var rollerAppliedVoltage = 0.0.volts - var rollerAppliedVoltage = 0.0.volts - var rollerSupplyCurrent = 0.0.amps - var rollerStatorCurrent = 0.0.amps - var rollerTemp = 0.0.celsius + var rollerSupplyCurrent = 0.0.amps + var rollerStatorCurrent = 0.0.amps + var rollerTemp = 0.0.celsius - override fun toLog(table: LogTable?) { - table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) + var armPosition = 0.0.degrees + var armVelocity = 0.0.degrees.perSecond + var armAbsoluteEncoderPosition = 0.0.degrees - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + var armAppliedVoltage = 0.0.volts + var armSupplyCurrent = 0.0.amps + var armStatorCurrent = 0.0.amps + var armTemp = 0.0.celsius - table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) + override fun toLog(table: LogTable?) { + table?.put("armPositionDegrees", armPosition.inDegrees) + table?.put("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees) + table?.put("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond) + table?.put("armAppliedVoltage", armAppliedVoltage.inVolts) + table?.put("armSupplyCurrentAmps", armSupplyCurrent.inAmperes) + table?.put("armStatorCurrentAmps", armStatorCurrent.inAmperes) + table?.put("armTempCelsius", armTemp.inCelsius) - table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) + table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) + table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) + table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) + table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) + table?.put("rollerTempCelsius", rollerTemp.inCelsius) + } + + override fun fromLog(table: LogTable?) { + table?.get("armPositionDegrees", armPosition.inDegrees)?.let { armPosition = it.degrees } + + table?.get("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees)?.let { + armAbsoluteEncoderPosition = it.degrees + } + + table?.get("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond)?.let { + armVelocity = it.degrees.perSecond + } + + table?.get("armAppliedVoltage", armAppliedVoltage.inVolts)?.let { + armAppliedVoltage = it.volts + } + + table?.get("armSupplyCurrentAmps", armSupplyCurrent.inAmperes)?.let { + armSupplyCurrent = it.amps + } + + table?.get("armStatorCurrentAmps", armStatorCurrent.inAmperes)?.let { + armStatorCurrent = it.amps + } + + table?.get("armTempCelsius", armTemp.inCelsius)?.let { armTemp = it.celsius } + + table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let { + rollerVelocity = it.rotations.perSecond + } + + table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { + rollerAppliedVoltage = it.volts + } - table?.put("rollerTempCelsius", rollerTemp.inCelsius) - } + table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let { + rollerSupplyCurrent = it.amps + } + + table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let { + rollerStatorCurrent = it.amps + } + + table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius } } -} \ No newline at end of file + } + + fun updateInputs(io: IntakeIOInputs) {} + + fun setRollerVoltage(voltage: ElectricalPotential) {} + + fun setArmVoltage(voltage: ElectricalPotential) {} + + fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) {} + + /** + * Updates the PID constants using the implementation controller + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + + /** Sets the current encoder position to be the zero value */ + fun zeroEncoder() {} + + fun setRollerBrakeMode(brake: Boolean) {} + + fun setArmBrakeMode(brake: Boolean) +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt new file mode 100644 index 00000000..103faf83 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt @@ -0,0 +1,256 @@ +package com.team4099.robot2023.subsystems.intake + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.revrobotics.SparkMaxPIDController +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.IntakeConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import edu.wpi.first.wpilibj.DutyCycleEncoder +import org.littletonrobotics.junction.Logger +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.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +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.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxAngularMechanismSensor +import kotlin.math.IEEErem +import kotlin.math.absoluteValue + +object IntakeIONEO : IntakeIO { + private val rollerSparkMax = + CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val armSparkMax = + CANSparkMax(Constants.Intake.ARM_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val rollerSensor = + sparkMaxAngularMechanismSensor( + rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION + ) + + private val armSensor = + sparkMaxAngularMechanismSensor( + armSparkMax, IntakeConstants.ARM_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION + ) + + private val armEncoder = armSparkMax.encoder + + private val throughBoreEncoder = DutyCycleEncoder(Constants.Intake.REV_ENCODER_PORT) + + private val armPIDController: SparkMaxPIDController = armSparkMax.pidController + + // gets the reported angle from the through bore encoder + private val encoderAbsolutePosition: Angle + get() { + var output = + ( + (-throughBoreEncoder.absolutePosition.rotations) * + IntakeConstants.ARM_ENCODER_GEAR_RATIO + ) + + if (output in (-55).degrees..0.0.degrees) { + output -= 180.degrees + } + + return output + } + + // uses the absolute encoder position to calculate the arm position + private val armAbsolutePosition: Angle + get() { + return (encoderAbsolutePosition - IntakeConstants.ABSOLUTE_ENCODER_OFFSET).inDegrees.IEEErem( + 360.0 + ) + .degrees + } + + init { + rollerSparkMax.restoreFactoryDefaults() + rollerSparkMax.clearFaults() + + rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) + rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) + rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED + + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + + rollerSparkMax.openLoopRampRate = 0.0 + rollerSparkMax.burnFlash() + + armSparkMax.restoreFactoryDefaults() + armSparkMax.clearFaults() + + armSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) + armSparkMax.setSmartCurrentLimit(IntakeConstants.ARM_CURRENT_LIMIT.inAmperes.toInt()) + armSparkMax.inverted = IntakeConstants.ARM_MOTOR_INVERTED + armSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + armSparkMax.burnFlash() + + MotorChecker.add( + "Ground Intake", + "Roller", + MotorCollection( + mutableListOf(Neo(rollerSparkMax, "Roller Motor")), + IntakeConstants.ROLLER_CURRENT_LIMIT, + 70.celsius, + IntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps, + 90.celsius + ), + ) + + MotorChecker.add( + "Ground Intake", + "Pivot", + MotorCollection( + mutableListOf(Neo(armSparkMax, "Pivot Motor")), + IntakeConstants.ARM_CURRENT_LIMIT, + 70.celsius, + IntakeConstants.ARM_CURRENT_LIMIT - 30.amps, + 90.celsius + ) + ) + } + + override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { + inputs.rollerVelocity = rollerSensor.velocity + inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput + inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps + + // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BusVoltage + // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = + // percentOutput * statorCurrent + inputs.rollerSupplyCurrent = + inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue + inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius + + inputs.armPosition = armSensor.position + inputs.armAbsoluteEncoderPosition = armAbsolutePosition + inputs.armVelocity = armSensor.velocity + inputs.armAppliedVoltage = armSparkMax.busVoltage.volts * armSparkMax.appliedOutput + inputs.armStatorCurrent = armSparkMax.outputCurrent.amps + inputs.armTemp = armSparkMax.motorTemperature.celsius + + // same math as rollersupplycurrent + inputs.armSupplyCurrent = inputs.armStatorCurrent * armSparkMax.appliedOutput.absoluteValue + + Logger.recordOutput( + "GroundIntake/absoluteEncoderPositionDegrees", encoderAbsolutePosition.inDegrees + ) + + Logger.recordOutput( + "GroundIntake/rollerMotorOvercurrentFault", + rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) + ) + Logger.recordOutput("GroundIntake/busVoltage", rollerSparkMax.busVoltage) + + Logger.recordOutput( + "GroundIntake/armSensorVelocity", + armSparkMax.encoder.velocity * IntakeConstants.ARM_GEAR_RATIO + ) + } + + /** + * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the roller motor to + */ + override fun setRollerVoltage(voltage: ElectricalPotential) { + rollerSparkMax.setVoltage( + clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION) + .inVolts + ) + } + + /** + * Sets the arm motor voltage, ensures the voltage is limited to battery voltage compensation + * + * @param voltage the voltage to set the arm motor to + */ + override fun setArmVoltage(voltage: ElectricalPotential) { + armSparkMax.setVoltage(voltage.inVolts) + + Logger.recordOutput("GroundIntake/commandedVoltage", voltage.inVolts) + } + + /** + * Sets the arm to a desired angle, uses feedforward to account for external forces in the system + * The armPIDController uses the previously set PID constants and ff to calculate how to get to + * the desired position + * + * @param armPosition the desired angle to set the aerm to + * @param feedforward the amount of volts to apply for feedforward + */ + override fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) { + armPIDController.setReference( + armSensor.positionToRawUnits(armPosition), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts + ) + } + + /** + * Updates the PID constants using the implementation controller, uses arm sensor to convert from + * PID constants to motor controller units + * + * @param kP accounts for linear error + * @param kI accounts for integral error + * @param kD accounts for derivative error + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + armPIDController.p = armSensor.proportionalPositionGainToRawUnits(kP) + armPIDController.i = armSensor.integralPositionGainToRawUnits(kI) + armPIDController.d = armSensor.derivativePositionGainToRawUnits(kD) + } + + /** recalculates the current position of the neo encoder using value from the absolute encoder */ + override fun zeroEncoder() { + armEncoder.position = armSensor.positionToRawUnits(armAbsolutePosition) + } + + /** + * Sets the roller motor brake mode + * + * @param brake if it brakes + */ + override fun setRollerBrakeMode(brake: Boolean) { + if (brake) { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } + + /** + * Sets the arm motor brake mode + * + * @param brake if it brakes + */ + override fun setArmBrakeMode(brake: Boolean) { + if (brake) { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + } else { + rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast + } + } +} From 983021a4b44c51004e5267bc0eed7b59b552776a Mon Sep 17 00:00:00 2001 From: Hanish Rajan <141093134+fyrhanish@users.noreply.github.com> Date: Wed, 10 Jan 2024 19:08:52 -0500 Subject: [PATCH 12/38] Began intake file --- .../config/constants/IntakeConstants.kt | 8 ++ .../robot2023/subsystems/intake/Intake.kt | 76 ++++++++++++++++++- 2 files changed, 83 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index b3be6deb..f824e2e2 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -18,4 +18,12 @@ object IntakeConstants { const val ROLLER_GEAR_RATIO = 36.0 / 18.0 const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0 + + // TODO: Enter values + const val ENABLE_ARM = 0.0 + const val ENABLE_ROTATION = 0.0 + val STOWED_UP_ANGLE = 0.0.degrees + val STOWED_DOWN_ANGLE = 0.0.degrees + val INTAKE_VOLTAGE = 0.0.volts + val NEUTRAL_VOLTAGE = 0.0.volts } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 4e49f3d6..70a11a35 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -1,3 +1,77 @@ package com.team4099.robot2023.subsystems.intake -class Intake +import com.team4099.lib.hal.Clock +import com.team4099.lib.logging.LoggedTunableNumber +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.IntakeConstants +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ArmFeedforward +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerDegree +import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond +import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond + +class Intake(private val io: IntakeIO) { + val inputs = IntakeIO.IntakeIOInputs() + + lateinit var armFeedforward: ArmFeedforward + + private val kP = LoggedTunableValue("Intake/kP", Pair({it.inVoltsPerDegree}, {it.volts.perDegree})) + private val kI = LoggedTunableValue("Intake/kI", Pair({it.inVoltsPerDegreeSeconds}, {it.volts.perDegreeSeconds})) + private val kD = LoggedTunableValue("Intake/kD", Pair({it.inVoltsPerDegreePerSecond}, {it.volts.perDegreePerSecond})) + + object TunableGroundIntakeStates { + val enableArm = + LoggedTunableNumber( + "Intake/enableArmIntake", + IntakeConstants.ENABLE_ARM + ) + val enableRotation = + LoggedTunableNumber( + "Intake/enableRotationIntake", + IntakeConstants.ENABLE_ROTATION + ) + val stowedUpAngle = + LoggedTunableValue( + "Intake/stowedUpAngle", + IntakeConstants.STOWED_UP_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) + val stowedDownAngle = + LoggedTunableValue( + "Intake/stowedDownAngle", + IntakeConstants.STOWED_DOWN_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) + val intakeVoltage = + LoggedTunableValue( + "Intake/intakeVoltage", + IntakeConstants.INTAKE_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) + val neutralVoltage = + LoggedTunableValue( + "Intake/neutralVoltage", + IntakeConstants.NEUTRAL_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) + } +} From 4a0a407b0f5cc86af82f0b3e8bfde5fef5732a5b Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Thu, 11 Jan 2024 20:23:15 -0500 Subject: [PATCH 13/38] Added state machine code --- .../config/constants/IntakeConstants.kt | 34 +- .../robot2023/subsystems/intake/Intake.kt | 364 +++++++++++++++++- .../robot2023/subsystems/intake/IntakeIO.kt | 2 + .../subsystems/superstructure/Request.kt | 11 +- 4 files changed, 404 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index f824e2e2..0add7bf8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -1,8 +1,12 @@ package com.team4099.robot2023.config.constants +import com.fasterxml.jackson.annotation.JsonAutoDetect.Value +import edu.wpi.first.wpilibj.DoubleSolenoid +import org.team4099.lib.units.Fraction +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perSecond object IntakeConstants { val VOLTAGE_COMPENSATION = 12.0.volts @@ -22,8 +26,34 @@ object IntakeConstants { // TODO: Enter values const val ENABLE_ARM = 0.0 const val ENABLE_ROTATION = 0.0 + val INTAKE_ANGLE = 0.0.degrees + val OUTTAKE_ANGLE = 0.0.degrees val STOWED_UP_ANGLE = 0.0.degrees val STOWED_DOWN_ANGLE = 0.0.degrees val INTAKE_VOLTAGE = 0.0.volts + val OUTTAKE_VOLTAGE = 0.0.volts val NEUTRAL_VOLTAGE = 0.0.volts + + val ARM_MAX_ROTATION = 0.0.degrees + val ARM_MIN_ROTATION = 0.0.degrees + + val MAX_ARM_VELOCITY = 0.0.radians.perSecond + val MAX_ARM_ACCELERATION = 0.0.radians.perSecond.perSecond + + val ARM_TOLERANCE = 0.0.radians + + object PID { + val NEO_KP = 1.0.volts.perRadian + val NEO_KI = 1.0.volts.perRadianSeconds + val NEO_KD = 1.0.volts.perRadianPerSecond + + val SIM_KP = 1.0.volts.perRadian + val SIM_KI = 1.0.volts.perRadianSeconds + val SIM_KD = 1.0.volts.perRadianPerSecond + + val ARM_KS = 1.0.volts + val ARM_KG = 1.0.volts + val ARM_KV = 1.0.volts / 1.0.radians.perSecond + val ARM_KA = 1.0.volts / 1.0.radians.perSecond.perSecond + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 70a11a35..8eb1ef10 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -38,7 +38,7 @@ class Intake(private val io: IntakeIO) { private val kI = LoggedTunableValue("Intake/kI", Pair({it.inVoltsPerDegreeSeconds}, {it.volts.perDegreeSeconds})) private val kD = LoggedTunableValue("Intake/kD", Pair({it.inVoltsPerDegreePerSecond}, {it.volts.perDegreePerSecond})) - object TunableGroundIntakeStates { + object TunableIntakeStates { val enableArm = LoggedTunableNumber( "Intake/enableArmIntake", @@ -49,6 +49,18 @@ class Intake(private val io: IntakeIO) { "Intake/enableRotationIntake", IntakeConstants.ENABLE_ROTATION ) + val intakeAngle = + LoggedTunableValue( + "Intake/intakeAngle", + IntakeConstants.INTAKE_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) + val outtakeAngle = + LoggedTunableValue( + "Intake/outtakeAngle", + IntakeConstants.OUTTAKE_ANGLE, + Pair({ it.inDegrees }, { it.degrees }) + ) val stowedUpAngle = LoggedTunableValue( "Intake/stowedUpAngle", @@ -73,5 +85,355 @@ class Intake(private val io: IntakeIO) { IntakeConstants.NEUTRAL_VOLTAGE, Pair({ it.inVolts }, { it.volts }) ) + val outtakeVoltage = + LoggedTunableValue( + "Intake/outtakeVoltage", + IntakeConstants.OUTTAKE_VOLTAGE, + Pair({ it.inVolts }, { it.volts }) + ) + } + + var armPositionTarget: Angle = 0.0.degrees + var armVoltageTarget: ElectricalPotential = 0.0.volts + var rollerVoltageTarget: ElectricalPotential = 0.0.volts + + var isZeroed = false + + private var lastArmPositionTarget: Angle = 0.0.degrees + private var lastArmVoltage: ElectricalPotential = 0.0.volts + + val forwardLimitReached: Boolean + get() = inputs.armPosition >= IntakeConstants.ARM_MAX_ROTATION + val reverseLimitReached: Boolean + get() = inputs.armPosition <= IntakeConstants.ARM_MIN_ROTATION + + var lastIntakeRuntime = Clock.fpgaTime + var currentState: IntakeStake = IntakeStake.UNINITIALIZED + + var currentRequest: Request.IntakeRequest = Request.IntakeRequest.ZeroArm() + set(value) { + when (value) { + is Request.IntakeRequest.OpenLoop -> { + armVoltageTarget = value.voltage + rollerVoltageTarget = value.rollerVoltage + } + + is Request.IntakeRequest.TargetingPosition -> { + armPositionTarget = value.position + rollerVoltageTarget = value.rollerVoltage + } + + else -> {} + } + + field = value + } + + private var armConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + IntakeConstants.MAX_ARM_VELOCITY, IntakeConstants.MAX_ARM_ACCELERATION + ) + + private var armProfile = + TrapezoidProfile( + armConstraints, + TrapezoidProfile.State(armPositionTarget, 0.0.degrees.perSecond), + TrapezoidProfile.State(inputs.armPosition, inputs.armVelocity) + ) + + private var timeProfileGeneratedAt = Clock.fpgaTime + + private var prevArmSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.armPosition, inputs.armVelocity) + + val isAtTargetedPosition: Boolean + get() = + ( + currentState == IntakeStake.TARGETING_POSITION && + armProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.armPosition - armPositionTarget).absoluteValue <= + IntakeConstants.ARM_TOLERANCE + ) || + (TunableIntakeStates.enableArm.get() != 1.0) + + val canContinueSafely: Boolean + get() = + currentRequest is Request.IntakeRequest.TargetingPosition && + ( + ((Clock.fpgaTime - timeProfileGeneratedAt) - armProfile.totalTime() < 1.0.seconds) || + armProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) + ) && + (inputs.armPosition - armPositionTarget).absoluteValue <= 5.degrees + + init { + + if (RobotBase.isReal()) { + kP.initDefault(IntakeConstants.PID.NEO_KP) + kI.initDefault(IntakeConstants.PID.NEO_KI) + kD.initDefault(IntakeConstants.PID.NEO_KD) + + armFeedforward = + ArmFeedforward( + IntakeConstants.PID.ARM_KS, + IntakeConstants.PID.ARM_KG, + IntakeConstants.PID.ARM_KV, + IntakeConstants.PID.ARM_KA + ) + } else { + kP.initDefault(IntakeConstants.PID.SIM_KP) + kI.initDefault(IntakeConstants.PID.SIM_KI) + kD.initDefault(IntakeConstants.PID.SIM_KD) + + armFeedforward = + ArmFeedforward( + 0.0.volts, + IntakeConstants.PID.ARM_KG, + IntakeConstants.PID.ARM_KV, + IntakeConstants.PID.ARM_KA + ) + } + } + + fun periodic() { + io.updateInputs(inputs) + + if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + + Logger.processInputs("GroundIntake", inputs) + + Logger.recordOutput("GroundIntake/currentState", currentState.name) + + Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("GroundIntake/isAtTargetedPosition", isAtTargetedPosition) + + Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) + + Logger.recordOutput("GroundIntake/isZeroed", isZeroed) + + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput( + "GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) + ) + + Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) + + Logger.recordOutput("GroundIntake/armPositionTarget", armPositionTarget.inDegrees) + + Logger.recordOutput("GroundIntake/armVoltageTarget", armVoltageTarget.inVolts) + + Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) + + Logger.recordOutput("GroundIntake/lastCommandedAngle", lastArmPositionTarget.inDegrees) + + Logger.recordOutput("GroundIntake/forwardLimitReached", forwardLimitReached) + + Logger.recordOutput("GroundIntake/reverseLimitReached", reverseLimitReached) + } + + var nextState = currentState + when (currentState) { + IntakeStake.UNINITIALIZED -> { + // Outputs + // No designated output functionality because targeting position will take care of it next + // loop cycle + + // Transitions + nextState = IntakeStake.ZEROING_ARM + } + IntakeStake.ZEROING_ARM -> { + zeroArm() + + if (inputs.isSimulated || + (inputs.armPosition - inputs.armAbsoluteEncoderPosition).absoluteValue <= 1.degrees + ) { + isZeroed = true + lastArmPositionTarget = -1337.degrees + } + + // Transitions + nextState = fromRequestToState(currentRequest) + } + IntakeStake.OPEN_LOOP_REQUEST -> { + // Outputs + if (armVoltageTarget != lastArmVoltage) { + lastIntakeRuntime = Clock.fpgaTime + } + + setArmVoltage(armVoltageTarget) + setRollerVoltage(rollerVoltageTarget) + + // Transitions + nextState = fromRequestToState(currentRequest) + + // See related comment in targeting position to see why we do this + if (!(currentState.equivalentToRequest(currentRequest))) { + lastArmVoltage = -1337.volts + } + } + IntakeStake.TARGETING_POSITION -> { + // Outputs + if (armPositionTarget != lastArmPositionTarget) { + val preProfileGenerate = Clock.realTimestamp + armProfile = + TrapezoidProfile( + armConstraints, + TrapezoidProfile.State(armPositionTarget, 0.0.degrees.perSecond), + TrapezoidProfile.State(inputs.armPosition, 0.0.degrees.perSecond) + ) + val postProfileGenerate = Clock.realTimestamp + Logger + .recordOutput( + "/GroundIntake/ProfileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + timeProfileGeneratedAt = Clock.fpgaTime + + // This statement is only run when the armPositionTarget is first noticed to be different + // than the previous setpoint the arm went to. + lastArmPositionTarget = armPositionTarget + lastIntakeRuntime = Clock.fpgaTime + } + + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + + val profileOutput = armProfile.calculate(timeElapsed) + + if (armProfile.isFinished(timeElapsed)) { + setRollerVoltage(rollerVoltageTarget) + } + + setArmPosition(profileOutput) + + Logger + .recordOutput("GroundIntake/completedMotionProfile", armProfile.isFinished(timeElapsed)) + + Logger + .recordOutput("GroundIntake/profilePositionDegrees", profileOutput.position.inDegrees) + Logger + .recordOutput( + "GroundIntake/profileVelocityDegreesPerSecond", + profileOutput.velocity.inDegreesPerSecond + ) + + // Transitions + nextState = fromRequestToState(currentRequest) + + // if we're transitioning out of targeting position, we want to make sure the next time we + // enter targeting position, we regenerate profile (even if the arm setpoint is the same as + // the previous time we ran it) + if (!(currentState.equivalentToRequest(currentRequest))) { + // setting the last target to something unreasonable so the profile is generated next loop + // cycle + lastArmPositionTarget = (-1337).degrees + } + } + } + + // The next loop cycle, we want to run ground intake at the state that was requested. setting + // current state to the next state ensures that we run the logic for the state we want in the + // next loop cycle. + currentState = nextState + } + + /** @param appliedVoltage Represents the applied voltage of the roller motor */ + fun setRollerVoltage(appliedVoltage: ElectricalPotential) { + io.setRollerVoltage(appliedVoltage) + } + + /** + * Sets the break/idle mode of the arm + * + * @param brake The value that break mode for the arm will be set as + */ + fun setArmBrakeMode(brake: Boolean) { + io.setArmBrakeMode(brake) + Logger.recordOutput("GroundIntake/armBrakeModeEnabled", brake) + } + + fun zeroArm() { + io.zeroEncoder() + } + + fun regenerateProfileNextLoopCycle() { + lastArmVoltage = -3337.volts + lastArmPositionTarget = -3337.degrees + lastIntakeRuntime = -3337.seconds + } + + fun setArmVoltage(voltage: ElectricalPotential) { + // if ((openLoopForwardLimitReached && voltage > 0.0.volts) || + // (openLoopReverseLimitReached && voltage < 0.0.volts) + // ) { + // io.setArmVoltage(0.0.volts) + // } else { + io.setArmVoltage(voltage) + // } + } + + /** + * Sets the arm position using the trapezoidal profile state + * + * @param setpoint.position Represents the position the arm should go to + * @param setpoint.velocity Represents the velocity the arm should be at + */ + private fun setArmPosition(setpoint: TrapezoidProfile.State) { + + // Calculating the acceleration of the arm + val armAngularAcceleration = + (setpoint.velocity - prevArmSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + prevArmSetpoint = setpoint + + // Set up the feed forward variable + val feedforward = + armFeedforward.calculate(setpoint.position, setpoint.velocity, armAngularAcceleration) + + // When the forward or reverse limit is reached, set the voltage to 0 + // Else move the arm to the setpoint position + if (isOutOfBounds(setpoint.velocity)) { + io.setArmVoltage(armFeedforward.calculate(inputs.armPosition, 0.degrees.perSecond)) + } else { + io.setArmPosition(setpoint.position, feedforward) + } + + Logger + .recordOutput("GroundIntake/profileIsOutOfBounds", isOutOfBounds(setpoint.velocity)) + Logger.recordOutput("GroundIntake/armFeedForward", feedforward.inVolts) + Logger.recordOutput("GroundIntake/armTargetPosition", setpoint.position.inDegrees) + Logger + .recordOutput("GroundIntake/armTargetVelocity", setpoint.velocity.inDegreesPerSecond) + } + + private fun isOutOfBounds(velocity: AngularVelocity): Boolean { + return (velocity > 0.0.degrees.perSecond && forwardLimitReached) || + (velocity < 0.0.degrees.perSecond && reverseLimitReached) + } + + companion object { + enum class IntakeStake { + UNINITIALIZED, + IDLE, + ZEROING_ARM, + TARGETING_POSITION, + OPEN_LOOP_REQUEST; + + inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { + return ( + (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || + (request is Request.IntakeRequest.TargetingPosition && this == TARGETING_POSITION) + ) + } + } + + inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { + return when (request) { + is Request.IntakeRequest.TargetingPosition -> IntakeStake.TARGETING_POSITION + is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST + is Request.IntakeRequest.ZeroArm -> IntakeStake.ZEROING_ARM + is Request.IntakeRequest.Idle -> IntakeStake.IDLE + } + } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index b06c5c0a..1c5099f3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -41,6 +41,8 @@ interface IntakeIO { var armStatorCurrent = 0.0.amps var armTemp = 0.0.celsius + var isSimulated = false + override fun toLog(table: LogTable?) { table?.put("armPositionDegrees", armPosition.inDegrees) table?.put("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 8b38be20..521cad35 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -59,12 +59,15 @@ sealed interface Request { class Home() : ElevatorRequest } - sealed interface GroundIntakeRequest : Request { + sealed interface IntakeRequest : Request { class TargetingPosition(val position: Angle, val rollerVoltage: ElectricalPotential) : - GroundIntakeRequest + IntakeRequest class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - GroundIntakeRequest - class ZeroArm() : GroundIntakeRequest + IntakeRequest + class ZeroArm() : + IntakeRequest + class Idle() : + IntakeRequest } sealed interface DrivetrainRequest : Request { From 176d5a092e3c756291629bc00cca14e83612e08a Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Fri, 12 Jan 2024 19:01:07 -0500 Subject: [PATCH 14/38] Removed arm from intake code --- .../config/constants/IntakeConstants.kt | 35 +- .../robot2023/subsystems/intake/Intake.kt | 321 +----------------- .../subsystems/superstructure/Request.kt | 6 +- 3 files changed, 13 insertions(+), 349 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 0add7bf8..69610f74 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -23,37 +23,6 @@ object IntakeConstants { const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0 - // TODO: Enter values - const val ENABLE_ARM = 0.0 - const val ENABLE_ROTATION = 0.0 - val INTAKE_ANGLE = 0.0.degrees - val OUTTAKE_ANGLE = 0.0.degrees - val STOWED_UP_ANGLE = 0.0.degrees - val STOWED_DOWN_ANGLE = 0.0.degrees - val INTAKE_VOLTAGE = 0.0.volts - val OUTTAKE_VOLTAGE = 0.0.volts - val NEUTRAL_VOLTAGE = 0.0.volts - - val ARM_MAX_ROTATION = 0.0.degrees - val ARM_MIN_ROTATION = 0.0.degrees - - val MAX_ARM_VELOCITY = 0.0.radians.perSecond - val MAX_ARM_ACCELERATION = 0.0.radians.perSecond.perSecond - - val ARM_TOLERANCE = 0.0.radians - - object PID { - val NEO_KP = 1.0.volts.perRadian - val NEO_KI = 1.0.volts.perRadianSeconds - val NEO_KD = 1.0.volts.perRadianPerSecond - - val SIM_KP = 1.0.volts.perRadian - val SIM_KI = 1.0.volts.perRadianSeconds - val SIM_KD = 1.0.volts.perRadianPerSecond - - val ARM_KS = 1.0.volts - val ARM_KG = 1.0.volts - val ARM_KV = 1.0.volts / 1.0.radians.perSecond - val ARM_KA = 1.0.volts / 1.0.radians.perSecond.perSecond - } + // TODO: Update value + val IDLE_ROLLER_VOLTAGE = 2.0.volts } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 8eb1ef10..134a693f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -32,205 +32,47 @@ import org.team4099.lib.units.perSecond class Intake(private val io: IntakeIO) { val inputs = IntakeIO.IntakeIOInputs() - lateinit var armFeedforward: ArmFeedforward - - private val kP = LoggedTunableValue("Intake/kP", Pair({it.inVoltsPerDegree}, {it.volts.perDegree})) - private val kI = LoggedTunableValue("Intake/kI", Pair({it.inVoltsPerDegreeSeconds}, {it.volts.perDegreeSeconds})) - private val kD = LoggedTunableValue("Intake/kD", Pair({it.inVoltsPerDegreePerSecond}, {it.volts.perDegreePerSecond})) - - object TunableIntakeStates { - val enableArm = - LoggedTunableNumber( - "Intake/enableArmIntake", - IntakeConstants.ENABLE_ARM - ) - val enableRotation = - LoggedTunableNumber( - "Intake/enableRotationIntake", - IntakeConstants.ENABLE_ROTATION - ) - val intakeAngle = - LoggedTunableValue( - "Intake/intakeAngle", - IntakeConstants.INTAKE_ANGLE, - Pair({ it.inDegrees }, { it.degrees }) - ) - val outtakeAngle = - LoggedTunableValue( - "Intake/outtakeAngle", - IntakeConstants.OUTTAKE_ANGLE, - Pair({ it.inDegrees }, { it.degrees }) - ) - val stowedUpAngle = - LoggedTunableValue( - "Intake/stowedUpAngle", - IntakeConstants.STOWED_UP_ANGLE, - Pair({ it.inDegrees }, { it.degrees }) - ) - val stowedDownAngle = - LoggedTunableValue( - "Intake/stowedDownAngle", - IntakeConstants.STOWED_DOWN_ANGLE, - Pair({ it.inDegrees }, { it.degrees }) - ) - val intakeVoltage = - LoggedTunableValue( - "Intake/intakeVoltage", - IntakeConstants.INTAKE_VOLTAGE, - Pair({ it.inVolts }, { it.volts }) - ) - val neutralVoltage = - LoggedTunableValue( - "Intake/neutralVoltage", - IntakeConstants.NEUTRAL_VOLTAGE, - Pair({ it.inVolts }, { it.volts }) - ) - val outtakeVoltage = - LoggedTunableValue( - "Intake/outtakeVoltage", - IntakeConstants.OUTTAKE_VOLTAGE, - Pair({ it.inVolts }, { it.volts }) - ) - } - - var armPositionTarget: Angle = 0.0.degrees - var armVoltageTarget: ElectricalPotential = 0.0.volts var rollerVoltageTarget: ElectricalPotential = 0.0.volts var isZeroed = false - private var lastArmPositionTarget: Angle = 0.0.degrees - private var lastArmVoltage: ElectricalPotential = 0.0.volts - - val forwardLimitReached: Boolean - get() = inputs.armPosition >= IntakeConstants.ARM_MAX_ROTATION - val reverseLimitReached: Boolean - get() = inputs.armPosition <= IntakeConstants.ARM_MIN_ROTATION - var lastIntakeRuntime = Clock.fpgaTime var currentState: IntakeStake = IntakeStake.UNINITIALIZED - var currentRequest: Request.IntakeRequest = Request.IntakeRequest.ZeroArm() + var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() set(value) { - when (value) { + rollerVoltageTarget = when (value) { is Request.IntakeRequest.OpenLoop -> { - armVoltageTarget = value.voltage - rollerVoltageTarget = value.rollerVoltage + value.rollerVoltage } - is Request.IntakeRequest.TargetingPosition -> { - armPositionTarget = value.position - rollerVoltageTarget = value.rollerVoltage + is Request.IntakeRequest.Idle -> { + IntakeConstants.IDLE_ROLLER_VOLTAGE } - - else -> {} } field = value } - private var armConstraints: TrapezoidProfile.Constraints = - TrapezoidProfile.Constraints( - IntakeConstants.MAX_ARM_VELOCITY, IntakeConstants.MAX_ARM_ACCELERATION - ) - - private var armProfile = - TrapezoidProfile( - armConstraints, - TrapezoidProfile.State(armPositionTarget, 0.0.degrees.perSecond), - TrapezoidProfile.State(inputs.armPosition, inputs.armVelocity) - ) - private var timeProfileGeneratedAt = Clock.fpgaTime - private var prevArmSetpoint: TrapezoidProfile.State = - TrapezoidProfile.State(inputs.armPosition, inputs.armVelocity) - - val isAtTargetedPosition: Boolean - get() = - ( - currentState == IntakeStake.TARGETING_POSITION && - armProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && - (inputs.armPosition - armPositionTarget).absoluteValue <= - IntakeConstants.ARM_TOLERANCE - ) || - (TunableIntakeStates.enableArm.get() != 1.0) - - val canContinueSafely: Boolean - get() = - currentRequest is Request.IntakeRequest.TargetingPosition && - ( - ((Clock.fpgaTime - timeProfileGeneratedAt) - armProfile.totalTime() < 1.0.seconds) || - armProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) - ) && - (inputs.armPosition - armPositionTarget).absoluteValue <= 5.degrees - - init { - - if (RobotBase.isReal()) { - kP.initDefault(IntakeConstants.PID.NEO_KP) - kI.initDefault(IntakeConstants.PID.NEO_KI) - kD.initDefault(IntakeConstants.PID.NEO_KD) - - armFeedforward = - ArmFeedforward( - IntakeConstants.PID.ARM_KS, - IntakeConstants.PID.ARM_KG, - IntakeConstants.PID.ARM_KV, - IntakeConstants.PID.ARM_KA - ) - } else { - kP.initDefault(IntakeConstants.PID.SIM_KP) - kI.initDefault(IntakeConstants.PID.SIM_KI) - kD.initDefault(IntakeConstants.PID.SIM_KD) - - armFeedforward = - ArmFeedforward( - 0.0.volts, - IntakeConstants.PID.ARM_KG, - IntakeConstants.PID.ARM_KV, - IntakeConstants.PID.ARM_KA - ) - } - } - fun periodic() { io.updateInputs(inputs) - if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { - io.configPID(kP.get(), kI.get(), kD.get()) - } - Logger.processInputs("GroundIntake", inputs) Logger.recordOutput("GroundIntake/currentState", currentState.name) Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) - Logger.recordOutput("GroundIntake/isAtTargetedPosition", isAtTargetedPosition) - - Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) - Logger.recordOutput("GroundIntake/isZeroed", isZeroed) if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput( - "GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) - ) + Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) - Logger.recordOutput("GroundIntake/armPositionTarget", armPositionTarget.inDegrees) - - Logger.recordOutput("GroundIntake/armVoltageTarget", armVoltageTarget.inVolts) - Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) - - Logger.recordOutput("GroundIntake/lastCommandedAngle", lastArmPositionTarget.inDegrees) - - Logger.recordOutput("GroundIntake/forwardLimitReached", forwardLimitReached) - - Logger.recordOutput("GroundIntake/reverseLimitReached", reverseLimitReached) } var nextState = currentState @@ -241,94 +83,19 @@ class Intake(private val io: IntakeIO) { // loop cycle // Transitions - nextState = IntakeStake.ZEROING_ARM + nextState = IntakeStake.IDLE } - IntakeStake.ZEROING_ARM -> { - zeroArm() - - if (inputs.isSimulated || - (inputs.armPosition - inputs.armAbsoluteEncoderPosition).absoluteValue <= 1.degrees - ) { - isZeroed = true - lastArmPositionTarget = -1337.degrees - } + IntakeStake.IDLE -> { + setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) // Transitions nextState = fromRequestToState(currentRequest) } IntakeStake.OPEN_LOOP_REQUEST -> { - // Outputs - if (armVoltageTarget != lastArmVoltage) { - lastIntakeRuntime = Clock.fpgaTime - } - - setArmVoltage(armVoltageTarget) setRollerVoltage(rollerVoltageTarget) // Transitions nextState = fromRequestToState(currentRequest) - - // See related comment in targeting position to see why we do this - if (!(currentState.equivalentToRequest(currentRequest))) { - lastArmVoltage = -1337.volts - } - } - IntakeStake.TARGETING_POSITION -> { - // Outputs - if (armPositionTarget != lastArmPositionTarget) { - val preProfileGenerate = Clock.realTimestamp - armProfile = - TrapezoidProfile( - armConstraints, - TrapezoidProfile.State(armPositionTarget, 0.0.degrees.perSecond), - TrapezoidProfile.State(inputs.armPosition, 0.0.degrees.perSecond) - ) - val postProfileGenerate = Clock.realTimestamp - Logger - .recordOutput( - "/GroundIntake/ProfileGenerationMS", - postProfileGenerate.inSeconds - preProfileGenerate.inSeconds - ) - timeProfileGeneratedAt = Clock.fpgaTime - - // This statement is only run when the armPositionTarget is first noticed to be different - // than the previous setpoint the arm went to. - lastArmPositionTarget = armPositionTarget - lastIntakeRuntime = Clock.fpgaTime - } - - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - - val profileOutput = armProfile.calculate(timeElapsed) - - if (armProfile.isFinished(timeElapsed)) { - setRollerVoltage(rollerVoltageTarget) - } - - setArmPosition(profileOutput) - - Logger - .recordOutput("GroundIntake/completedMotionProfile", armProfile.isFinished(timeElapsed)) - - Logger - .recordOutput("GroundIntake/profilePositionDegrees", profileOutput.position.inDegrees) - Logger - .recordOutput( - "GroundIntake/profileVelocityDegreesPerSecond", - profileOutput.velocity.inDegreesPerSecond - ) - - // Transitions - nextState = fromRequestToState(currentRequest) - - // if we're transitioning out of targeting position, we want to make sure the next time we - // enter targeting position, we regenerate profile (even if the arm setpoint is the same as - // the previous time we ran it) - if (!(currentState.equivalentToRequest(currentRequest))) { - // setting the last target to something unreasonable so the profile is generated next loop - // cycle - lastArmPositionTarget = (-1337).degrees - } } } @@ -343,95 +110,27 @@ class Intake(private val io: IntakeIO) { io.setRollerVoltage(appliedVoltage) } - /** - * Sets the break/idle mode of the arm - * - * @param brake The value that break mode for the arm will be set as - */ - fun setArmBrakeMode(brake: Boolean) { - io.setArmBrakeMode(brake) - Logger.recordOutput("GroundIntake/armBrakeModeEnabled", brake) - } - fun zeroArm() { io.zeroEncoder() } - fun regenerateProfileNextLoopCycle() { - lastArmVoltage = -3337.volts - lastArmPositionTarget = -3337.degrees - lastIntakeRuntime = -3337.seconds - } - - fun setArmVoltage(voltage: ElectricalPotential) { - // if ((openLoopForwardLimitReached && voltage > 0.0.volts) || - // (openLoopReverseLimitReached && voltage < 0.0.volts) - // ) { - // io.setArmVoltage(0.0.volts) - // } else { - io.setArmVoltage(voltage) - // } - } - - /** - * Sets the arm position using the trapezoidal profile state - * - * @param setpoint.position Represents the position the arm should go to - * @param setpoint.velocity Represents the velocity the arm should be at - */ - private fun setArmPosition(setpoint: TrapezoidProfile.State) { - - // Calculating the acceleration of the arm - val armAngularAcceleration = - (setpoint.velocity - prevArmSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME - prevArmSetpoint = setpoint - - // Set up the feed forward variable - val feedforward = - armFeedforward.calculate(setpoint.position, setpoint.velocity, armAngularAcceleration) - - // When the forward or reverse limit is reached, set the voltage to 0 - // Else move the arm to the setpoint position - if (isOutOfBounds(setpoint.velocity)) { - io.setArmVoltage(armFeedforward.calculate(inputs.armPosition, 0.degrees.perSecond)) - } else { - io.setArmPosition(setpoint.position, feedforward) - } - - Logger - .recordOutput("GroundIntake/profileIsOutOfBounds", isOutOfBounds(setpoint.velocity)) - Logger.recordOutput("GroundIntake/armFeedForward", feedforward.inVolts) - Logger.recordOutput("GroundIntake/armTargetPosition", setpoint.position.inDegrees) - Logger - .recordOutput("GroundIntake/armTargetVelocity", setpoint.velocity.inDegreesPerSecond) - } - - private fun isOutOfBounds(velocity: AngularVelocity): Boolean { - return (velocity > 0.0.degrees.perSecond && forwardLimitReached) || - (velocity < 0.0.degrees.perSecond && reverseLimitReached) - } - companion object { enum class IntakeStake { UNINITIALIZED, IDLE, - ZEROING_ARM, - TARGETING_POSITION, OPEN_LOOP_REQUEST; inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { return ( (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || - (request is Request.IntakeRequest.TargetingPosition && this == TARGETING_POSITION) + (request is Request.IntakeRequest.Idle && this == IDLE) ) } } inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { return when (request) { - is Request.IntakeRequest.TargetingPosition -> IntakeStake.TARGETING_POSITION is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST - is Request.IntakeRequest.ZeroArm -> IntakeStake.ZEROING_ARM is Request.IntakeRequest.Idle -> IntakeStake.IDLE } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 521cad35..12ba3bcd 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -60,11 +60,7 @@ sealed interface Request { } sealed interface IntakeRequest : Request { - class TargetingPosition(val position: Angle, val rollerVoltage: ElectricalPotential) : - IntakeRequest - class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - IntakeRequest - class ZeroArm() : + class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest class Idle() : IntakeRequest From 2bed455c14cd727f3b28ddb05057e54bb19315ab Mon Sep 17 00:00:00 2001 From: Shom770 Date: Fri, 12 Jan 2024 19:16:11 -0500 Subject: [PATCH 15/38] intake code is finished --- .../com/team4099/robot2023/BuildConstants.kt | 14 +- .../config/constants/IntakeConstants.kt | 12 +- .../robot2023/subsystems/intake/Intake.kt | 184 ++++++++---------- .../robot2023/subsystems/intake/IntakeIO.kt | 78 +------- .../subsystems/intake/IntakeIONEO.kt | 160 +-------------- .../subsystems/superstructure/Request.kt | 7 +- 6 files changed, 95 insertions(+), 360 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 126e75c1..a5a1a8af 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 = -1 -const val GIT_SHA = "UNKNOWN" -const val GIT_DATE = "UNKNOWN" -const val GIT_BRANCH = "UNKNOWN" -const val BUILD_DATE = "2023-12-16T19:11:21Z" -const val BUILD_UNIX_TIME = 1702771881467L -const val DIRTY = 129 +const val GIT_REVISION = 20 +const val GIT_SHA = "4571cd4f3189ad1982f91f7dde0d0c7d7ec30341" +const val GIT_DATE = "2024-01-12T19:01:07Z" +const val GIT_BRANCH = "intake" +const val BUILD_DATE = "2024-01-12T19:14:59Z" +const val BUILD_UNIX_TIME = 1705104899720L +const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 69610f74..3e02f6b4 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -1,12 +1,8 @@ package com.team4099.robot2023.config.constants -import com.fasterxml.jackson.annotation.JsonAutoDetect.Value -import edu.wpi.first.wpilibj.DoubleSolenoid -import org.team4099.lib.units.Fraction -import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.* -import org.team4099.lib.units.perSecond +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.volts object IntakeConstants { val VOLTAGE_COMPENSATION = 12.0.volts @@ -16,12 +12,8 @@ object IntakeConstants { // TODO: Change gear ratio according to robot val ROLLER_CURRENT_LIMIT = 50.0.amps - val ARM_CURRENT_LIMIT = 50.0.amps const val ROLLER_MOTOR_INVERTED = true - const val ARM_MOTOR_INVERTED = false const val ROLLER_GEAR_RATIO = 36.0 / 18.0 - const val ARM_GEAR_RATIO = ((60.0 / 12.0) * (80.0 / 18.0) * (32.0 / 16.0)) - const val ARM_ENCODER_GEAR_RATIO = 36.0 / 18.0 // TODO: Update value val IDLE_ROLLER_VOLTAGE = 2.0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 134a693f..8f422b26 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -1,138 +1,110 @@ package com.team4099.robot2023.subsystems.intake -import com.team4099.lib.hal.Clock -import com.team4099.lib.logging.LoggedTunableNumber -import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj.RobotBase import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.ArmFeedforward -import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.units.AngularVelocity -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.inVoltsPerDegree -import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond -import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inDegreesPerSecond -import org.team4099.lib.units.perSecond class Intake(private val io: IntakeIO) { - val inputs = IntakeIO.IntakeIOInputs() + val inputs = IntakeIO.IntakeIOInputs() - var rollerVoltageTarget: ElectricalPotential = 0.0.volts + var rollerVoltageTarget: ElectricalPotential = 0.0.volts - var isZeroed = false + var isZeroed = false - var lastIntakeRuntime = Clock.fpgaTime - var currentState: IntakeStake = IntakeStake.UNINITIALIZED + var currentState: IntakeStake = IntakeStake.UNINITIALIZED - var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() - set(value) { - rollerVoltageTarget = when (value) { - is Request.IntakeRequest.OpenLoop -> { - value.rollerVoltage - } - - is Request.IntakeRequest.Idle -> { - IntakeConstants.IDLE_ROLLER_VOLTAGE - } - } - - field = value + var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() + set(value) { + rollerVoltageTarget = + when (value) { + is Request.IntakeRequest.OpenLoop -> { + value.rollerVoltage + } + is Request.IntakeRequest.Idle -> { + IntakeConstants.IDLE_ROLLER_VOLTAGE + } } - private var timeProfileGeneratedAt = Clock.fpgaTime - - fun periodic() { - io.updateInputs(inputs) - - Logger.processInputs("GroundIntake", inputs) + field = value + } - Logger.recordOutput("GroundIntake/currentState", currentState.name) + fun periodic() { + io.updateInputs(inputs) - Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) + Logger.processInputs("Intake", inputs) - Logger.recordOutput("GroundIntake/isZeroed", isZeroed) + Logger.recordOutput("Intake/currentState", currentState.name) - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) + Logger.recordOutput("Intake/requestedState", currentRequest.javaClass.simpleName) - Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) + Logger.recordOutput("Intake/isZeroed", isZeroed) - Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) - } + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput( + "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) + ) - var nextState = currentState - when (currentState) { - IntakeStake.UNINITIALIZED -> { - // Outputs - // No designated output functionality because targeting position will take care of it next - // loop cycle - - // Transitions - nextState = IntakeStake.IDLE - } - IntakeStake.IDLE -> { - setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - IntakeStake.OPEN_LOOP_REQUEST -> { - setRollerVoltage(rollerVoltageTarget) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - } - - // The next loop cycle, we want to run ground intake at the state that was requested. setting - // current state to the next state ensures that we run the logic for the state we want in the - // next loop cycle. - currentState = nextState + Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) } - /** @param appliedVoltage Represents the applied voltage of the roller motor */ - fun setRollerVoltage(appliedVoltage: ElectricalPotential) { - io.setRollerVoltage(appliedVoltage) + var nextState = currentState + when (currentState) { + IntakeStake.UNINITIALIZED -> { + // Outputs + // No designated output functionality because targeting position will take care of it next + // loop cycle + + // Transitions + nextState = IntakeStake.IDLE + } + IntakeStake.IDLE -> { + setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + IntakeStake.OPEN_LOOP -> { + setRollerVoltage(rollerVoltageTarget) + + // Transitions + nextState = fromRequestToState(currentRequest) + } } - fun zeroArm() { - io.zeroEncoder() + // The next loop cycle, we want to run ground intake at the state that was requested. setting + // current state to the next state ensures that we run the logic for the state we want in the + // next loop cycle. + currentState = nextState + } + + /** @param appliedVoltage Represents the applied voltage of the roller motor */ + fun setRollerVoltage(appliedVoltage: ElectricalPotential) { + io.setRollerVoltage(appliedVoltage) + } + + companion object { + enum class IntakeStake { + UNINITIALIZED, + IDLE, + OPEN_LOOP; + + inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { + return ( + (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || + (request is Request.IntakeRequest.Idle && this == IDLE) + ) + } } - companion object { - enum class IntakeStake { - UNINITIALIZED, - IDLE, - OPEN_LOOP_REQUEST; - - inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { - return ( - (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || - (request is Request.IntakeRequest.Idle && this == IDLE) - ) - } - } - - inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { - return when (request) { - is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST - is Request.IntakeRequest.Idle -> IntakeStake.IDLE - } - } + inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { + return when (request) { + is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP + is Request.IntakeRequest.Idle -> IntakeStake.IDLE + } } + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index 1c5099f3..91cdee66 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -6,19 +6,10 @@ 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.derived.Angle -import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.IntegralGain -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.inVolts import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.inRotationsPerMinute import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond @@ -32,25 +23,10 @@ interface IntakeIO { var rollerStatorCurrent = 0.0.amps var rollerTemp = 0.0.celsius - var armPosition = 0.0.degrees - var armVelocity = 0.0.degrees.perSecond - var armAbsoluteEncoderPosition = 0.0.degrees - - var armAppliedVoltage = 0.0.volts - var armSupplyCurrent = 0.0.amps - var armStatorCurrent = 0.0.amps - var armTemp = 0.0.celsius - var isSimulated = false override fun toLog(table: LogTable?) { - table?.put("armPositionDegrees", armPosition.inDegrees) - table?.put("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees) - table?.put("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond) - table?.put("armAppliedVoltage", armAppliedVoltage.inVolts) - table?.put("armSupplyCurrentAmps", armSupplyCurrent.inAmperes) - table?.put("armStatorCurrentAmps", armStatorCurrent.inAmperes) - table?.put("armTempCelsius", armTemp.inCelsius) + table?.put("isSimulated", isSimulated) table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) @@ -60,30 +36,6 @@ interface IntakeIO { } override fun fromLog(table: LogTable?) { - table?.get("armPositionDegrees", armPosition.inDegrees)?.let { armPosition = it.degrees } - - table?.get("armAbsoluteEncoderPositionDegrees", armAbsoluteEncoderPosition.inDegrees)?.let { - armAbsoluteEncoderPosition = it.degrees - } - - table?.get("armVelocityDegreesPerSec", armVelocity.inDegreesPerSecond)?.let { - armVelocity = it.degrees.perSecond - } - - table?.get("armAppliedVoltage", armAppliedVoltage.inVolts)?.let { - armAppliedVoltage = it.volts - } - - table?.get("armSupplyCurrentAmps", armSupplyCurrent.inAmperes)?.let { - armSupplyCurrent = it.amps - } - - table?.get("armStatorCurrentAmps", armStatorCurrent.inAmperes)?.let { - armStatorCurrent = it.amps - } - - table?.get("armTempCelsius", armTemp.inCelsius)?.let { armTemp = it.celsius } - table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let { rollerVelocity = it.rotations.perSecond } @@ -104,31 +56,9 @@ interface IntakeIO { } } - fun updateInputs(io: IntakeIOInputs) {} - - fun setRollerVoltage(voltage: ElectricalPotential) {} - - fun setArmVoltage(voltage: ElectricalPotential) {} - - fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) {} - - /** - * Updates the PID constants using the implementation controller - * - * @param kP accounts for linear error - * @param kI accounts for integral error - * @param kD accounts for derivative error - */ - fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} - - /** Sets the current encoder position to be the zero value */ - fun zeroEncoder() {} + fun updateInputs(io: IntakeIOInputs) - fun setRollerBrakeMode(brake: Boolean) {} + fun setRollerVoltage(voltage: ElectricalPotential) - fun setArmBrakeMode(brake: Boolean) + fun setRollerBrakeMode(brake: Boolean) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt index 103faf83..dadcd0db 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt @@ -2,82 +2,31 @@ package com.team4099.robot2023.subsystems.intake import com.revrobotics.CANSparkMax import com.revrobotics.CANSparkMaxLowLevel -import com.revrobotics.SparkMaxPIDController import com.team4099.lib.math.clamp import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.falconspin.MotorChecker import com.team4099.robot2023.subsystems.falconspin.MotorCollection import com.team4099.robot2023.subsystems.falconspin.Neo -import edu.wpi.first.wpilibj.DutyCycleEncoder import org.littletonrobotics.junction.Logger 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.derived.Angle -import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.IntegralGain -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.inVolts -import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxAngularMechanismSensor -import kotlin.math.IEEErem import kotlin.math.absoluteValue object IntakeIONEO : IntakeIO { private val rollerSparkMax = CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val armSparkMax = - CANSparkMax(Constants.Intake.ARM_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - private val rollerSensor = sparkMaxAngularMechanismSensor( rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION ) - private val armSensor = - sparkMaxAngularMechanismSensor( - armSparkMax, IntakeConstants.ARM_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION - ) - - private val armEncoder = armSparkMax.encoder - - private val throughBoreEncoder = DutyCycleEncoder(Constants.Intake.REV_ENCODER_PORT) - - private val armPIDController: SparkMaxPIDController = armSparkMax.pidController - - // gets the reported angle from the through bore encoder - private val encoderAbsolutePosition: Angle - get() { - var output = - ( - (-throughBoreEncoder.absolutePosition.rotations) * - IntakeConstants.ARM_ENCODER_GEAR_RATIO - ) - - if (output in (-55).degrees..0.0.degrees) { - output -= 180.degrees - } - - return output - } - - // uses the absolute encoder position to calculate the arm position - private val armAbsolutePosition: Angle - get() { - return (encoderAbsolutePosition - IntakeConstants.ABSOLUTE_ENCODER_OFFSET).inDegrees.IEEErem( - 360.0 - ) - .degrees - } - init { rollerSparkMax.restoreFactoryDefaults() rollerSparkMax.clearFaults() @@ -91,16 +40,6 @@ object IntakeIONEO : IntakeIO { rollerSparkMax.openLoopRampRate = 0.0 rollerSparkMax.burnFlash() - armSparkMax.restoreFactoryDefaults() - armSparkMax.clearFaults() - - armSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - armSparkMax.setSmartCurrentLimit(IntakeConstants.ARM_CURRENT_LIMIT.inAmperes.toInt()) - armSparkMax.inverted = IntakeConstants.ARM_MOTOR_INVERTED - armSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - - armSparkMax.burnFlash() - MotorChecker.add( "Ground Intake", "Roller", @@ -112,18 +51,6 @@ object IntakeIONEO : IntakeIO { 90.celsius ), ) - - MotorChecker.add( - "Ground Intake", - "Pivot", - MotorCollection( - mutableListOf(Neo(armSparkMax, "Pivot Motor")), - IntakeConstants.ARM_CURRENT_LIMIT, - 70.celsius, - IntakeConstants.ARM_CURRENT_LIMIT - 30.amps, - 90.celsius - ) - ) } override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { @@ -139,30 +66,11 @@ object IntakeIONEO : IntakeIO { inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius - inputs.armPosition = armSensor.position - inputs.armAbsoluteEncoderPosition = armAbsolutePosition - inputs.armVelocity = armSensor.velocity - inputs.armAppliedVoltage = armSparkMax.busVoltage.volts * armSparkMax.appliedOutput - inputs.armStatorCurrent = armSparkMax.outputCurrent.amps - inputs.armTemp = armSparkMax.motorTemperature.celsius - - // same math as rollersupplycurrent - inputs.armSupplyCurrent = inputs.armStatorCurrent * armSparkMax.appliedOutput.absoluteValue - - Logger.recordOutput( - "GroundIntake/absoluteEncoderPositionDegrees", encoderAbsolutePosition.inDegrees - ) - Logger.recordOutput( - "GroundIntake/rollerMotorOvercurrentFault", + "Intake/rollerMotorOvercurrentFault", rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) ) - Logger.recordOutput("GroundIntake/busVoltage", rollerSparkMax.busVoltage) - - Logger.recordOutput( - "GroundIntake/armSensorVelocity", - armSparkMax.encoder.velocity * IntakeConstants.ARM_GEAR_RATIO - ) + Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage) } /** @@ -177,57 +85,6 @@ object IntakeIONEO : IntakeIO { ) } - /** - * Sets the arm motor voltage, ensures the voltage is limited to battery voltage compensation - * - * @param voltage the voltage to set the arm motor to - */ - override fun setArmVoltage(voltage: ElectricalPotential) { - armSparkMax.setVoltage(voltage.inVolts) - - Logger.recordOutput("GroundIntake/commandedVoltage", voltage.inVolts) - } - - /** - * Sets the arm to a desired angle, uses feedforward to account for external forces in the system - * The armPIDController uses the previously set PID constants and ff to calculate how to get to - * the desired position - * - * @param armPosition the desired angle to set the aerm to - * @param feedforward the amount of volts to apply for feedforward - */ - override fun setArmPosition(armPosition: Angle, feedforward: ElectricalPotential) { - armPIDController.setReference( - armSensor.positionToRawUnits(armPosition), - CANSparkMax.ControlType.kPosition, - 0, - feedforward.inVolts - ) - } - - /** - * Updates the PID constants using the implementation controller, uses arm sensor to convert from - * PID constants to motor controller units - * - * @param kP accounts for linear error - * @param kI accounts for integral error - * @param kD accounts for derivative error - */ - override fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - armPIDController.p = armSensor.proportionalPositionGainToRawUnits(kP) - armPIDController.i = armSensor.integralPositionGainToRawUnits(kI) - armPIDController.d = armSensor.derivativePositionGainToRawUnits(kD) - } - - /** recalculates the current position of the neo encoder using value from the absolute encoder */ - override fun zeroEncoder() { - armEncoder.position = armSensor.positionToRawUnits(armAbsolutePosition) - } - /** * Sets the roller motor brake mode * @@ -240,17 +97,4 @@ object IntakeIONEO : IntakeIO { rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast } } - - /** - * Sets the arm motor brake mode - * - * @param brake if it brakes - */ - override fun setArmBrakeMode(brake: Boolean) { - if (brake) { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - } else { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - } - } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index 12ba3bcd..e905aaef 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -7,7 +7,6 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.inches -import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.perSecond @@ -60,10 +59,8 @@ sealed interface Request { } sealed interface IntakeRequest : Request { - class OpenLoop(val rollerVoltage: ElectricalPotential) : - IntakeRequest - class Idle() : - IntakeRequest + class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest + class Idle() : IntakeRequest } sealed interface DrivetrainRequest : Request { From d7203ca4b039a2b30bc189cfdb705896bfc09fe8 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Fri, 12 Jan 2024 19:16:30 -0500 Subject: [PATCH 16/38] remove encoder offset --- .../com/team4099/robot2023/config/constants/IntakeConstants.kt | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 3e02f6b4..7c27d526 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -7,9 +7,6 @@ import org.team4099.lib.units.derived.volts object IntakeConstants { val VOLTAGE_COMPENSATION = 12.0.volts - // TODO: Add value for encoder offset - val ABSOLUTE_ENCODER_OFFSET = 0.0.degrees - // TODO: Change gear ratio according to robot val ROLLER_CURRENT_LIMIT = 50.0.amps const val ROLLER_MOTOR_INVERTED = true From cc19b014b6b2541b66ef9c09331fb31f751275c1 Mon Sep 17 00:00:00 2001 From: lakelandspark Date: Sat, 13 Jan 2024 19:27:54 -0500 Subject: [PATCH 17/38] Removed zero state --- .../robot2023/subsystems/intake/Intake.kt | 170 +++++++++--------- .../robot2023/subsystems/intake/IntakeIO.kt | 11 +- 2 files changed, 87 insertions(+), 94 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 8f422b26..d020e99d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -1,110 +1,106 @@ package com.team4099.robot2023.subsystems.intake +import com.team4099.lib.hal.Clock import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts -class Intake(private val io: IntakeIO) { - val inputs = IntakeIO.IntakeIOInputs() - - var rollerVoltageTarget: ElectricalPotential = 0.0.volts - - var isZeroed = false - - var currentState: IntakeStake = IntakeStake.UNINITIALIZED - - var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() - set(value) { - rollerVoltageTarget = - when (value) { - is Request.IntakeRequest.OpenLoop -> { - value.rollerVoltage - } - is Request.IntakeRequest.Idle -> { - IntakeConstants.IDLE_ROLLER_VOLTAGE - } +class Intake(val io: IntakeIO) { + val inputs = IntakeIO.IntakeIOInputs() + var rollerVoltageTarget: ElectricalPotential = 0.0.volts + var isZeroed = false + var currentState: IntakeStake = IntakeStake.UNINITIALIZED + + var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() + set(value) { + rollerVoltageTarget = when (value) { + is Request.IntakeRequest.OpenLoop -> { + value.rollerVoltage + } + + is Request.IntakeRequest.Idle -> { + IntakeConstants.IDLE_ROLLER_VOLTAGE + } + } + + field = value } - field = value - } - - fun periodic() { - io.updateInputs(inputs) + private var timeProfileGeneratedAt = Clock.fpgaTime - Logger.processInputs("Intake", inputs) + fun periodic() { + io.updateInputs(inputs) - Logger.recordOutput("Intake/currentState", currentState.name) + Logger.processInputs("GroundIntake", inputs) + Logger.recordOutput("GroundIntake/currentState", currentState.name) + Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) + Logger.recordOutput("GroundIntake/isZeroed", isZeroed) - Logger.recordOutput("Intake/requestedState", currentRequest.javaClass.simpleName) - - Logger.recordOutput("Intake/isZeroed", isZeroed) + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) + Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) + Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) + } - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput( - "Intake/isAtCommandedState", currentState.equivalentToRequest(currentRequest) - ) + var nextState = currentState + when (currentState) { + IntakeStake.UNINITIALIZED -> { + // Outputs + // No designated output functionality because targeting position will take care of it next + // loop cycle + + // Transitions + nextState = IntakeStake.IDLE + } + IntakeStake.IDLE -> { + setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + IntakeStake.OPEN_LOOP_REQUEST -> { + setRollerVoltage(rollerVoltageTarget) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + } - Logger.recordOutput("Intake/rollerVoltageTarget", rollerVoltageTarget.inVolts) + // The next loop cycle, we want to run ground intake at the state that was requested. setting + // current state to the next state ensures that we run the logic for the state we want in the + // next loop cycle. + currentState = nextState } - var nextState = currentState - when (currentState) { - IntakeStake.UNINITIALIZED -> { - // Outputs - // No designated output functionality because targeting position will take care of it next - // loop cycle - - // Transitions - nextState = IntakeStake.IDLE - } - IntakeStake.IDLE -> { - setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - IntakeStake.OPEN_LOOP -> { - setRollerVoltage(rollerVoltageTarget) - - // Transitions - nextState = fromRequestToState(currentRequest) - } + /** @param appliedVoltage Represents the applied voltage of the roller motor */ + fun setRollerVoltage(appliedVoltage: ElectricalPotential) { + io.setRollerVoltage(appliedVoltage) } - // The next loop cycle, we want to run ground intake at the state that was requested. setting - // current state to the next state ensures that we run the logic for the state we want in the - // next loop cycle. - currentState = nextState - } - - /** @param appliedVoltage Represents the applied voltage of the roller motor */ - fun setRollerVoltage(appliedVoltage: ElectricalPotential) { - io.setRollerVoltage(appliedVoltage) - } - - companion object { - enum class IntakeStake { - UNINITIALIZED, - IDLE, - OPEN_LOOP; - - inline fun equivalentToRequest(request: Request.IntakeRequest): Boolean { - return ( - (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || - (request is Request.IntakeRequest.Idle && this == IDLE) - ) - } - } + companion object { + enum class IntakeStake { + UNINITIALIZED, + IDLE, + OPEN_LOOP_REQUEST; + + fun equivalentToRequest(request: Request.IntakeRequest): Boolean { + return ( + (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || + (request is Request.IntakeRequest.Idle && this == IDLE) + ) + } + } - inline fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { - return when (request) { - is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP - is Request.IntakeRequest.Idle -> IntakeStake.IDLE - } + fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { + return when (request) { + is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST + is Request.IntakeRequest.Idle -> IntakeStake.IDLE + } + } } - } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index 91cdee66..9a7bba5b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -21,13 +21,10 @@ interface IntakeIO { var rollerSupplyCurrent = 0.0.amps var rollerStatorCurrent = 0.0.amps - var rollerTemp = 0.0.celsius - var isSimulated = false + var rollerTemp = 0.0.celsius override fun toLog(table: LogTable?) { - table?.put("isSimulated", isSimulated) - table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) @@ -56,9 +53,9 @@ interface IntakeIO { } } - fun updateInputs(io: IntakeIOInputs) + fun updateInputs(io: IntakeIOInputs) {} - fun setRollerVoltage(voltage: ElectricalPotential) + fun setRollerVoltage(voltage: ElectricalPotential) {} - fun setRollerBrakeMode(brake: Boolean) + fun setRollerBrakeMode(brake: Boolean) {} } From 1e2913802f6e5128f49095908dfa2b99031c01c5 Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Sat, 13 Jan 2024 21:10:27 -0500 Subject: [PATCH 18/38] Initial commit for IntakeIOSim plus syntax changes stake --> state and OPEN_LOOP_REQUEST --> OPEN_LOOP --- .../config/constants/IntakeConstants.kt | 4 +- .../robot2023/subsystems/intake/Intake.kt | 22 ++++----- .../robot2023/subsystems/intake/IntakeIO.kt | 2 + .../subsystems/intake/IntakeIOSim.kt | 49 +++++++++++++++++++ 4 files changed, 64 insertions(+), 13 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 7c27d526..79a2b2a8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.config.constants import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.volts object IntakeConstants { - val VOLTAGE_COMPENSATION = 12.0.volts + val ROLLER_INERTIA = 0.0 + val VOLTAGE_COMPENSATION = 12.0.volts // TODO: Change gear ratio according to robot val ROLLER_CURRENT_LIMIT = 50.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index d020e99d..fe7d059d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -14,7 +14,7 @@ class Intake(val io: IntakeIO) { val inputs = IntakeIO.IntakeIOInputs() var rollerVoltageTarget: ElectricalPotential = 0.0.volts var isZeroed = false - var currentState: IntakeStake = IntakeStake.UNINITIALIZED + var currentState: IntakeState = IntakeState.UNINITIALIZED var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() set(value) { @@ -49,21 +49,21 @@ class Intake(val io: IntakeIO) { var nextState = currentState when (currentState) { - IntakeStake.UNINITIALIZED -> { + IntakeState.UNINITIALIZED -> { // Outputs // No designated output functionality because targeting position will take care of it next // loop cycle // Transitions - nextState = IntakeStake.IDLE + nextState = IntakeState.IDLE } - IntakeStake.IDLE -> { + IntakeState.IDLE -> { setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) // Transitions nextState = fromRequestToState(currentRequest) } - IntakeStake.OPEN_LOOP_REQUEST -> { + IntakeState.OPEN_LOOP -> { setRollerVoltage(rollerVoltageTarget) // Transitions @@ -83,23 +83,23 @@ class Intake(val io: IntakeIO) { } companion object { - enum class IntakeStake { + enum class IntakeState { UNINITIALIZED, IDLE, - OPEN_LOOP_REQUEST; + OPEN_LOOP; fun equivalentToRequest(request: Request.IntakeRequest): Boolean { return ( - (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP_REQUEST) || + (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || (request is Request.IntakeRequest.Idle && this == IDLE) ) } } - fun fromRequestToState(request: Request.IntakeRequest): IntakeStake { + fun fromRequestToState(request: Request.IntakeRequest): IntakeState { return when (request) { - is Request.IntakeRequest.OpenLoop -> IntakeStake.OPEN_LOOP_REQUEST - is Request.IntakeRequest.Idle -> IntakeStake.IDLE + is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP + is Request.IntakeRequest.Idle -> IntakeState.IDLE } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt index 9a7bba5b..6ba96d7e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt @@ -24,6 +24,8 @@ interface IntakeIO { var rollerTemp = 0.0.celsius + var isSimulated = false + override fun toLog(table: LogTable?) { table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt new file mode 100644 index 00000000..f9f202e2 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt @@ -0,0 +1,49 @@ +package com.team4099.robot2023.subsystems.intake + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.IntakeConstants +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.celsius +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perMinute + +class IntakeIOSim : IntakeIO { + private val rollerSim: FlywheelSim = FlywheelSim( + DCMotor.getNEO(1), + IntakeConstants.ROLLER_GEAR_RATIO, + IntakeConstants.ROLLER_INERTIA + ) + + init{} + + override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { + + rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute + inputs.rollerAppliedVoltage = 0.volts + inputs.rollerSupplyCurrent = 0.amps + inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps + inputs.rollerTemp = 0.0.celsius + + inputs.isSimulated = true + } + + override fun setRollerVoltage(voltage: ElectricalPotential) { + rollerSim.setInputVoltage( + clamp( + voltage, + -IntakeConstants.VOLTAGE_COMPENSATION, + IntakeConstants.VOLTAGE_COMPENSATION + ) + .inVolts + ) + } +} From 8898d8e3986841e8c4c0fd06c31a137a0f5b0b4c Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Mon, 15 Jan 2024 19:28:22 -0500 Subject: [PATCH 19/38] update constants --- .../team4099/robot2023/config/constants/IntakeConstants.kt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 79a2b2a8..3846be7c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -4,13 +4,13 @@ import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object IntakeConstants { - val ROLLER_INERTIA = 0.0 + val ROLLER_INERTIA = 0.0014948033 // this one has been updated val VOLTAGE_COMPENSATION = 12.0.volts // TODO: Change gear ratio according to robot val ROLLER_CURRENT_LIMIT = 50.0.amps const val ROLLER_MOTOR_INVERTED = true - const val ROLLER_GEAR_RATIO = 36.0 / 18.0 + const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated // TODO: Update value val IDLE_ROLLER_VOLTAGE = 2.0.volts From dc978d6a42bce6de63bb5edd33801763314bf15e Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Mon, 15 Jan 2024 19:49:07 -0500 Subject: [PATCH 20/38] stuff --- simgui-ds.json | 92 +++++++++++++++++++ simgui.json | 9 ++ .../com/team4099/robot2023/BuildConstants.kt | 10 +- .../com/team4099/robot2023/RobotContainer.kt | 7 ++ .../team4099/robot2023/config/ControlBoard.kt | 2 + .../config/constants/IntakeConstants.kt | 2 +- .../robot2023/subsystems/intake/Intake.kt | 7 ++ .../subsystems/intake/IntakeIOSim.kt | 8 +- 8 files changed, 129 insertions(+), 8 deletions(-) create mode 100644 simgui-ds.json create mode 100644 simgui.json diff --git a/simgui-ds.json b/simgui-ds.json new file mode 100644 index 00000000..73cc713c --- /dev/null +++ b/simgui-ds.json @@ -0,0 +1,92 @@ +{ + "keyboardJoysticks": [ + { + "axisConfig": [ + { + "decKey": 65, + "incKey": 68 + }, + { + "decKey": 87, + "incKey": 83 + }, + { + "decKey": 69, + "decayRate": 0.0, + "incKey": 82, + "keyRate": 0.009999999776482582 + } + ], + "axisCount": 3, + "buttonCount": 4, + "buttonKeys": [ + 90, + 88, + 67, + 86 + ], + "povConfig": [ + { + "key0": 328, + "key135": 323, + "key180": 322, + "key225": 321, + "key270": 324, + "key315": 327, + "key45": 329, + "key90": 326 + } + ], + "povCount": 1 + }, + { + "axisConfig": [ + { + "decKey": 74, + "incKey": 76 + }, + { + "decKey": 73, + "incKey": 75 + } + ], + "axisCount": 2, + "buttonCount": 4, + "buttonKeys": [ + 77, + 44, + 46, + 47 + ], + "povCount": 0 + }, + { + "axisConfig": [ + { + "decKey": 263, + "incKey": 262 + }, + { + "decKey": 265, + "incKey": 264 + } + ], + "axisCount": 2, + "buttonCount": 6, + "buttonKeys": [ + 260, + 268, + 266, + 261, + 269, + 267 + ], + "povCount": 0 + }, + { + "axisCount": 0, + "buttonCount": 0, + "povCount": 0 + } + ] +} diff --git a/simgui.json b/simgui.json new file mode 100644 index 00000000..21c47b41 --- /dev/null +++ b/simgui.json @@ -0,0 +1,9 @@ +{ + "NTProvider": { + "types": { + "/FMSInfo": "FMSInfo", + "/Shuffleboard/Pre-match/Mode": "String Chooser", + "/SmartDashboard/AutonomousMode": "String Chooser" + } + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index a5a1a8af..ff557a29 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 = 20 -const val GIT_SHA = "4571cd4f3189ad1982f91f7dde0d0c7d7ec30341" -const val GIT_DATE = "2024-01-12T19:01:07Z" +const val GIT_REVISION = 25 +const val GIT_SHA = "b4edbcbc86bce4f445e9dcbf3dc9d1361b02aca3" +const val GIT_DATE = "2024-01-15T19:28:22Z" const val GIT_BRANCH = "intake" -const val BUILD_DATE = "2024-01-12T19:14:59Z" -const val BUILD_UNIX_TIME = 1705104899720L +const val BUILD_DATE = "2024-01-15T19:47:46Z" +const val BUILD_UNIX_TIME = 1705366066090L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 6bfb14cc..0fae56ae 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -10,6 +10,9 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 +import com.team4099.robot2023.subsystems.intake.Intake +import com.team4099.robot2023.subsystems.intake.IntakeIONEO +import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request @@ -23,6 +26,7 @@ import org.team4099.lib.units.derived.degrees object RobotContainer { private val drivetrain: Drivetrain + private val intake: Intake private val vision: Vision private val limelight: LimelightVision @@ -31,6 +35,7 @@ object RobotContainer { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) + intake = Intake(IntakeIONEO) vision = Vision( // object: CameraIO {} @@ -45,6 +50,7 @@ object RobotContainer { } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) + intake = Intake(IntakeIOSim) vision = Vision( CameraIONorthstar("northstar_1"), @@ -110,6 +116,7 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) + ControlBoard.groundIntakeTest.whileTrue(intake.generateIntakeTestCommand()) // ControlBoard.autoLevel.whileActiveContinuous( // GoToAngle(drivetrain).andThen(AutoLevel(drivetrain)) // ) diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 2a3446ac..d1383fad 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,4 +97,6 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } + + val groundIntakeTest = Trigger { driver.aButton } } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt index 3846be7c..00b49039 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt @@ -4,7 +4,7 @@ import org.team4099.lib.units.base.amps import org.team4099.lib.units.derived.volts object IntakeConstants { - val ROLLER_INERTIA = 0.0014948033 // this one has been updated + val ROLLER_INERTIA = 0.002459315 // this one has been updated val VOLTAGE_COMPENSATION = 12.0.volts // TODO: Change gear ratio according to robot diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index fe7d059d..3614c806 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -4,6 +4,8 @@ import com.team4099.lib.hal.Clock import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands.runOnce import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.derived.ElectricalPotential @@ -82,6 +84,11 @@ class Intake(val io: IntakeIO) { io.setRollerVoltage(appliedVoltage) } + fun generateIntakeTestCommand(): Command { + val returnCommand = runOnce({ currentRequest = Request.IntakeRequest.OpenLoop(12.volts) }) + return returnCommand + } + companion object { enum class IntakeState { UNINITIALIZED, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt index f9f202e2..801b2e84 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt @@ -14,13 +14,14 @@ import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perMinute -class IntakeIOSim : IntakeIO { +object IntakeIOSim : IntakeIO { private val rollerSim: FlywheelSim = FlywheelSim( DCMotor.getNEO(1), IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.ROLLER_INERTIA ) + private var appliedVoltage = 0.volts; init{} override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { @@ -28,7 +29,7 @@ class IntakeIOSim : IntakeIO { rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute - inputs.rollerAppliedVoltage = 0.volts + inputs.rollerAppliedVoltage = appliedVoltage inputs.rollerSupplyCurrent = 0.amps inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps inputs.rollerTemp = 0.0.celsius @@ -37,6 +38,7 @@ class IntakeIOSim : IntakeIO { } override fun setRollerVoltage(voltage: ElectricalPotential) { + appliedVoltage = voltage rollerSim.setInputVoltage( clamp( voltage, @@ -46,4 +48,6 @@ class IntakeIOSim : IntakeIO { .inVolts ) } + + override fun setRollerBrakeMode(brake: Boolean) {} } From 9244539121dd39cfcef3d13323e6eb3c1b245995 Mon Sep 17 00:00:00 2001 From: NeonCoal <52942449+NeonCoal@users.noreply.github.com> Date: Mon, 15 Jan 2024 20:47:37 -0500 Subject: [PATCH 21/38] fixing sim --- .../com/team4099/robot2023/BuildConstants.kt | 10 +- .../com/team4099/robot2023/RobotContainer.kt | 8 +- .../robot2023/commands/SysIdCommand.kt | 4 +- .../drivetrain/DriveBrakeModeCommand.kt | 8 +- .../commands/drivetrain/DrivePathCommand.kt | 48 +- .../commands/drivetrain/GoToAngle.kt | 23 +- .../commands/drivetrain/GyroAutoLevel.kt | 25 +- .../drivetrain/OpenLoopReverseCommand.kt | 8 +- .../drivetrain/ResetGyroPitchCommand.kt | 4 +- .../drivetrain/ResetGyroYawCommand.kt | 4 +- .../commands/drivetrain/ResetPoseCommand.kt | 4 +- .../commands/drivetrain/ResetZeroCommand.kt | 4 +- .../drivetrain/SwerveModuleTuningCommand.kt | 4 +- .../commands/drivetrain/TeleopDriveCommand.kt | 7 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 7 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 993 ++++++++---------- .../drivetrain/drive/DrivetrainIO.kt | 42 +- .../drivetrain/drive/DrivetrainIOReal.kt | 55 - .../drivetrain/drive/DrivetrainIOSim.kt | 18 +- .../subsystems/drivetrain/gyro/GyroIO.kt | 90 +- .../subsystems/drivetrain/gyro/GyroIONavx.kt | 124 +-- .../drivetrain/gyro/GyroIOPigeon2.kt | 144 --- .../drivetrain/servemodule/SwerveModule.kt | 276 +++++ .../drivetrain/servemodule/SwerveModuleIO.kt | 122 +++ .../servemodule/SwerveModuleIOSim.kt | 247 +++++ .../drivetrain/swervemodule/SwerveModule.kt | 330 ------ .../drivetrain/swervemodule/SwerveModuleIO.kt | 146 --- .../swervemodule/SwerveModuleIOFalcon.kt | 458 -------- .../swervemodule/SwerveModuleIOSim.kt | 274 ----- .../threads/PhoenixOdometryThread.java | 110 -- .../threads/SparkMaxOdometryThread.java | 73 -- .../robot2023/subsystems/intake/Intake.kt | 6 +- 32 files changed, 1278 insertions(+), 2398 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index ff557a29..65645c9c 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 = 25 -const val GIT_SHA = "b4edbcbc86bce4f445e9dcbf3dc9d1361b02aca3" -const val GIT_DATE = "2024-01-15T19:28:22Z" +const val GIT_REVISION = 26 +const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea" +const val GIT_DATE = "2024-01-15T19:49:07Z" const val GIT_BRANCH = "intake" -const val BUILD_DATE = "2024-01-15T19:47:46Z" -const val BUILD_UNIX_TIME = 1705366066090L +const val BUILD_DATE = "2024-01-15T20:41:51Z" +const val BUILD_UNIX_TIME = 1705369311220L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 0fae56ae..f648bf94 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -6,16 +6,14 @@ import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOReal +import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIOPigeon2 import com.team4099.robot2023.subsystems.intake.Intake import com.team4099.robot2023.subsystems.intake.IntakeIONEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan @@ -34,7 +32,7 @@ object RobotContainer { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} - drivetrain = Drivetrain(GyroIOPigeon2, DrivetrainIOReal) + drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {}) intake = Intake(IntakeIONEO) vision = Vision( @@ -92,7 +90,7 @@ object RobotContainer { } fun zeroSensors() { - drivetrain.currentRequest = Request.DrivetrainRequest.ZeroSensors() + drivetrain.zeroSensors() } fun zeroAngle(toAngle: Angle) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt index 61999fe2..bd56d9ed 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/SysIdCommand.kt @@ -2,14 +2,14 @@ package com.team4099.robot2023.commands import edu.wpi.first.wpilibj.Timer import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Subsystem import org.littletonrobotics.junction.Logger import java.util.function.BiConsumer import java.util.function.Consumer import java.util.function.Supplier -class SysIdCommand : CommandBase { +class SysIdCommand : Command { private val isDriveTrain: Boolean private lateinit var driveTrainSetter: BiConsumer private lateinit var mechanismSetter: Consumer diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 8d3024d0..d7c4c6dd 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,20 +1,18 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond -class DriveBrakeModeCommand(val drivetrain: Drivetrain) : CommandBase() { +class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } override fun execute() { - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 92bf07df..51438b3b 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -15,44 +14,16 @@ import edu.wpi.first.math.trajectory.TrajectoryParameterizer.TrajectoryGeneratio import edu.wpi.first.math.trajectory.constraint.CentripetalAccelerationConstraint import edu.wpi.first.math.trajectory.constraint.TrajectoryConstraint import edu.wpi.first.wpilibj.DriverStation -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.Meter -import org.team4099.lib.units.base.inMeters -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.inches -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.cos -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.inMetersPerSecondPerMeter -import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds -import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond -import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds -import org.team4099.lib.units.derived.perMeter -import org.team4099.lib.units.derived.perMeterSeconds -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.derived.sin -import org.team4099.lib.units.inDegreesPerSecond -import org.team4099.lib.units.inMetersPerSecond -import org.team4099.lib.units.inMetersPerSecondPerSecond -import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.inRadiansPerSecondPerSecond -import org.team4099.lib.units.perSecond +import org.team4099.lib.units.* +import org.team4099.lib.units.base.* +import org.team4099.lib.units.derived.* import java.util.function.Supplier import kotlin.math.PI @@ -65,7 +36,7 @@ class DrivePathCommand( val endPathOnceAtReference: Boolean = true, val leaveOutYAdjustment: Boolean = false, val endVelocity: Velocity2d = Velocity2d(), -) : CommandBase() { +) : Command() { private val xPID: PIDController> private val yPID: PIDController> @@ -244,8 +215,7 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.currentRequest = - Request.DrivetrainRequest.ClosedLoop( + drivetrain.setClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -310,16 +280,14 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index f46e6956..0779ea8f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,30 +3,19 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds -import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.* import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond import kotlin.math.PI -class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { +class GoToAngle(val drivetrain: Drivetrain) : Command() { private val thetaPID: ProfiledPIDController> val thetakP = @@ -92,8 +81,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -109,8 +97,7 @@ class GoToAngle(val drivetrain: Drivetrain) : CommandBase() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index 7978de7f..5d22049e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,26 +4,17 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.units.Fraction -import org.team4099.lib.units.Value -import org.team4099.lib.units.Velocity +import org.team4099.lib.units.* import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.Second import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.inMetersPerSecond -import org.team4099.lib.units.perSecond - -class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() { +import org.team4099.lib.units.derived.* + +class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { private val gyroPID: ProfiledPIDController> var alignmentAngle = drivetrain.odometryPose.rotation @@ -100,8 +91,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.0.radians.perSecond, gyroFeedback, fieldOriented = true ) @@ -129,8 +119,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : CommandBase() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index 8267b398..c4847a4d 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,20 +1,18 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.perSecond -class OpenLoopReverseCommand(val drivetrain: Drivetrain) : CommandBase() { +class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } override fun execute() { - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( + drivetrain.setOpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt index 26ae9102..09f48993 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroPitchCommand.kt @@ -1,13 +1,13 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees class ResetGyroPitchCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt index d4f34fb9..1c84301e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetGyroYawCommand.kt @@ -1,13 +1,13 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees class ResetGyroYawCommand(val drivetrain: Drivetrain, val toAngle: Angle = 0.0.degrees) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt index 1ca8a687..eccb3790 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetPoseCommand.kt @@ -2,11 +2,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.util.AllianceFlipUtil -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.geometry.Pose2d -class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : CommandBase() { +class ResetPoseCommand(val drivetrain: Drivetrain, val pose: Pose2d) : Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt index df186518..9e3cd36d 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ResetZeroCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger -class ResetZeroCommand(val drivetrain: Drivetrain) : CommandBase() { +class ResetZeroCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt index 70e0c257..6f0403e0 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/SwerveModuleTuningCommand.kt @@ -2,12 +2,12 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import edu.wpi.first.math.kinematics.SwerveModuleState -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.inRotation2ds class SwerveModuleTuningCommand(val drivetrain: Drivetrain, val steeringPosition: () -> Angle) : - CommandBase() { + Command() { init { addRequirements(drivetrain) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index d369cb5a..849c0b28 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,9 +1,8 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.driver.DriverProfile -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger class TeleopDriveCommand( @@ -13,7 +12,7 @@ class TeleopDriveCommand( val turn: () -> Double, val slowMode: () -> Boolean, val drivetrain: Drivetrain -) : CommandBase() { +) : Command() { init { addRequirements(drivetrain) @@ -24,7 +23,7 @@ class TeleopDriveCommand( override fun execute() { val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) - drivetrain.currentRequest = Request.DrivetrainRequest.OpenLoop(rotation, speed) + drivetrain.setOpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index d47b882c..8654eba7 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,18 +1,17 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import edu.wpi.first.wpilibj2.command.CommandBase +import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest -class ZeroSensorsCommand(val drivetrain: Drivetrain) : CommandBase() { +class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { init { addRequirements(drivetrain) } override fun initialize() { - drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() + drivetrain.zeroSensors() } override fun isFinished(): Boolean { 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 484c9b0f..eae04997 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 @@ -5,7 +5,6 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.superstructure.Request import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -32,6 +31,7 @@ import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearAcceleration import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.Length +import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inMilliseconds import org.team4099.lib.units.base.inSeconds @@ -44,626 +44,513 @@ 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 -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = - Alert( - "Gyro is not connected, field relative driving will be significantly worse.", - Alert.AlertType.ERROR - ) - - val gyroInputs = GyroIO.GyroIOInputs() - val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - - var gyroYawOffset = 0.0.radians - - val closestAlignmentAngle: Angle - get() { - for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { - return angle.degrees + val gyroNotConnectedAlert = + Alert( + "Gyro is not connected, field relative driving will be significantly worse.", + Alert.AlertType.ERROR + ) + + val gyroInputs = GyroIO.GyroIOInputs() + val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR + + var gyroYawOffset = 0.0.radians + + val closestAlignmentAngle: Angle + get() { + for (angle in -180..90 step 90) { + if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + return angle.degrees + } + } + return 0.0.degrees } - } - return 0.0.degrees - } - var canMoveSafely = Supplier { false } + var canMoveSafely = Supplier { false } - var elevatorHeightSupplier = Supplier { 0.0.inches } + var elevatorHeightSupplier = Supplier { 0.0.inches } - var velocityTarget = 0.degrees.perSecond + init { - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + // Wheel speeds + zeroSteering() + } - var fieldOrientation = true // true denotes that when driving, it'll be field oriented. + private val frontLeftWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val frontRightWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backLeftWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backRightWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + val moduleTranslations = + listOf( + frontLeftWheelLocation, + frontRightWheelLocation, + backLeftWheelLocation, + backRightWheelLocation + ) - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + val swerveDriveKinematics = + SwerveDriveKinematics( + frontLeftWheelLocation.translation2d, + frontRightWheelLocation.translation2d, + backLeftWheelLocation.translation2d, + backRightWheelLocation.translation2d + ) - var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) - var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() - set(value) { - when (value) { - is DrivetrainRequest.OpenLoop -> { - velocityTarget = value.angularVelocity - targetedDriveVector = value.driveVector - fieldOrientation = value.fieldOriented - } - is DrivetrainRequest.ClosedLoop -> { - targetedChassisSpeeds = value.chassisSpeeds - targetedChassisAccels = value.chassisAccels - } - else -> {} - } - field = value - } + var swerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) - init { - - // Wheel speeds - zeroSteering() - } - - private val frontLeftWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val frontRightWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backLeftWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backRightWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - - val moduleTranslations = - listOf( - frontLeftWheelLocation, - frontRightWheelLocation, - backLeftWheelLocation, - backRightWheelLocation - ) - - val swerveDriveKinematics = - SwerveDriveKinematics( - frontLeftWheelLocation.translation2d, - frontRightWheelLocation.translation2d, - backLeftWheelLocation.translation2d, - backRightWheelLocation.translation2d - ) - - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.0001)) - - var swerveDriveOdometry = - SwerveDriveOdometry( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray() - ) - - var setPointStates = - mutableListOf( - SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() - ) - - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - // get() { - // return Pose2d( - // 42.875.inches + 79.centi.meters + 14.inches + 2.75.inches, - // 113.25.inches + 1.inches, - // 180.degrees - // ) - // } - set(value) { - swerveDrivePoseEstimator.resetPose(value) - - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + var setPointStates = + mutableListOf( + SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() + ) - var rawGyroAngle = odometryPose.rotation + var odometryPose: Pose2d + get() = swerveDrivePoseEstimator.getLatestPose() + set(value) { + swerveDrivePoseEstimator.resetPose(value) - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) - set(value) { - swerveDriveOdometry.resetPosition( - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - value.pose2d - ) - } + if (RobotBase.isReal()) {} else { + undriftedPose = odometryPose + swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 + } + } - var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) + var rawGyroAngle = odometryPose.rotation - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + var undriftedPose: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) + set(value) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + value.pose2d + ) + } - var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) - var omegaVelocity = 0.0.radians.perSecond + var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var lastModulePositions = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) + var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - var lastGyroYaw = 0.0.radians + var omegaVelocity = 0.0.radians.perSecond - override fun periodic() { - val startTime = Clock.realTimestamp + var lastModulePositions = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) - odometryLock.lock(); // Prevents odometry updates while reading data + var lastGyroYaw = 0.0.radians - gyroIO.updateInputs(gyroInputs) - swerveModules.forEach { it.updateInputs() } + override fun periodic() { + val startTime = Clock.realTimestamp + gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroIO.updateInputs(gyroInputs) - odometryLock.unlock() + swerveModules.forEach { it.periodic() } - gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + // TODO: add logic to reduce setpoint based on elevator/arm position + DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond - swerveModules.forEach { it.periodic() } + Logger.recordOutput( + "Drivetrain/maxSetpointMetersPerSecond", + DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - Logger.recordOutput( - "Drivetrain/maxSetpointMetersPerSecond", - DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond - ) + // Update field velocity + val measuredStates = arrayOfNulls(4) + for (i in 0..3) { + measuredStates[i] = + SwerveModuleState( + swerveModules[i].inputs.driveVelocity.inMetersPerSecond, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } + val chassisState: ChassisSpeeds = + ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) + val fieldVelCalculated = + Translation2d( + chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters + ) + .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + + robotVelocity = Pair(chassisState.vx, chassisState.vy) + fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) + + omegaVelocity = chassisState.omega + if (!gyroInputs.gyroConnected) { + gyroInputs.gyroYawRate = omegaVelocity + rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate + gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset + } - // Update field velocity - val measuredStates = arrayOfNulls(4) - for (i in 0..3) { - measuredStates[i] = - SwerveModuleState( - swerveModules[i].inputs.driveVelocity.inMetersPerSecond, - swerveModules[i].inputs.steeringPosition.inRotation2ds + // updating odometry every loop cycle + updateOdometry() + + Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) + Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) + + Logger.processInputs("Drivetrain/Gyro", gyroInputs) + Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) + Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) + + Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians)) + Logger.recordOutput( + "Odometry/pose3d", + Pose3d( + odometryPose.x, + odometryPose.y, + 1.0.meters, + Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) + ) + .pose3d + ) + Logger.recordOutput( + "Odometry/targetPose", + doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) ) - } - val chassisState: ChassisSpeeds = - ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) - val fieldVelCalculated = - Translation2d( - chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters - ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig - - robotVelocity = Pair(chassisState.vx, chassisState.vy) - fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) - - omegaVelocity = chassisState.omega - if (!gyroInputs.gyroConnected) { - gyroInputs.gyroYawRate = omegaVelocity - rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate - gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset - } - // updating odometry every loop cycle - updateOdometry() - - Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) - Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) - - Logger.processInputs("Drivetrain/Gyro", gyroInputs) - Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) - Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) - - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, odometryPose.pose2d) - Logger.recordOutput( - "Odometry/pose3d", - Pose3d( - odometryPose.x, - odometryPose.y, - 1.0.meters, - Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) - ) - .pose3d - ) - Logger.recordOutput( - "Odometry/targetPose", - doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) - ) - - Logger.recordOutput( - "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", - (Clock.realTimestamp - startTime).inMilliseconds - ) - - var nextState = currentState - - when (currentState) { - DrivetrainState.UNINITIALIZED -> { - // Transitions - nextState = DrivetrainState.ZEROING_SENSORS - } - DrivetrainState.ZEROING_SENSORS -> { - zeroSensors() - - // Transitions - currentRequest = DrivetrainRequest.Idle() - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.OPEN_LOOP -> { - // Outputs - setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.CLOSED_LOOP -> { - // Outputs - setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.IDLE -> { - nextState = fromRequestToState(currentRequest) - } + Logger.recordOutput( + "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) } - currentState = nextState + 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 + ) + } - // Log the current state - Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) - } + var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - private fun updateOdometry() { + if (gyroInputs.gyroConnected) { + driveTwist = + edu.wpi.first.math.geometry.Twist2d( + driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians + ) + lastGyroYaw = gyroInputs.rawGyroYaw + } - 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) - - 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 + if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { + val undriftedModules = arrayOfNulls(4) + for (i in 0 until 4) { + undriftedModules[i] = + SwerveModulePosition( + ( + swerveModules[i].modulePosition.distanceMeters.meters - + swerveModules[i].inputs.drift + ) + .inMeters, + swerveModules[i].modulePosition.angle + ) + } + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) + + drift = undriftedPose.minus(odometryPose) + + Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) + } - // reversing the drift to store the ground truth pose - if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { - val undriftedModules = arrayOfNulls(4) - for (i in 0 until 4) { - undriftedModules[i] = - SwerveModulePosition( - ( - swerveModules[i].modulePosition.distanceMeters.meters - - swerveModules[i].inputs.drift - ) - .inMeters, - swerveModules[i].modulePosition.angle - ) - } - swerveDriveOdometry.update((gyroInputs.gyroYaw).inRotation2ds, undriftedModules) - - drift = undriftedPose.minus(odometryPose) - - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) - } - } - - /** - * @param angularVelocity Represents the angular velocity of the chassis - * @param driveVector Pair of linear velocities: First is X vel, second is Y vel - * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) - */ - fun setOpenLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - fieldOriented: Boolean = true - ) { - val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 - val allianceFlippedDriveVector = - Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) - - val swerveModuleStates: Array - var desiredChassisSpeeds: ChassisSpeeds - - if (fieldOriented) { - desiredChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - odometryPose.rotation - ) - } else { - desiredChassisSpeeds = - ChassisSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - ) + swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) } - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy - ), - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega - ) + /** + * @param angularVelocity Represents the angular velocity of the chassis + * @param driveVector Pair of linear velocities: First is X vel, second is Y vel + * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) + */ + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + fieldOriented: Boolean = true + ) { + val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 + val allianceFlippedDriveVector = + Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + if (fieldOriented) { + Logger.recordOutput("drivetrain/isFieldOriented", true) + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + odometryPose.rotation + ) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + ) + } - val twistToNextPose: Twist2d = velocityTransform.log() + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy + ), + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega + ) + + val twistToNextPose: Twist2d = velocityTransform.log() + + desiredChassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + } + + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - desiredChassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond ) - } - swerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) + setPointStates = swerveModuleStates.toMutableList() - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond - ) + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) + } + } - setPointStates = swerveModuleStates.toMutableList() + /** + * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular + * and linear acceleration. Calculates both angular and linear velocities and acceleration and + * calls setPositionClosedLoop for each SwerveModule object. + * + * @param angularVelocity The angular velocity of a specified drive + * @param driveVector.first The linear velocity on the X axis + * @param driveVector.second The linear velocity on the Y axis + * @param angularAcceleration The angular acceleration of a specified drive + * @param driveAcceleration.first The linear acceleration on the X axis + * @param driveAcceleration.second The linear acceleration on the Y axis + * @param fieldOriented Defines whether module states are calculated relative to field + */ + fun setClosedLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, + driveAcceleration: Pair = + Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), + fieldOriented: Boolean = true, + ) { + val velSwerveModuleStates: Array? + val accelSwerveModuleStates: Array? + + if (fieldOriented) { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + // This is with respect to the field, meaning all velocity and acceleration vectors are + // adjusted to be relative to the field instead of relative to the robot. + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation + ) + .chassisSpeedsWPILIB + ) + + // Although this isn't perfect, calculating acceleration states using the same math as + // velocity can get us "good enough" accel states to minimize skew + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels.fromFieldRelativeAccels( + driveAcceleration.first, + driveAcceleration.second, + angularAcceleration, + odometryPose.rotation + ) + .chassisAccelsWPILIB + ) + } else { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) + .chassisSpeedsWPILIB + ) + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) + .chassisAccelsWPILIB + ) + } - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) - } - } - - /** - * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular - * and linear acceleration. Calculates both angular and linear velocities and acceleration and - * calls setPositionClosedLoop for each SwerveModule object. - * - * @param angularVelocity The angular velocity of a specified drive - * @param driveVector.first The linear velocity on the X axis - * @param driveVector.second The linear velocity on the Y axis - * @param angularAcceleration The angular acceleration of a specified drive - * @param driveAcceleration.first The linear acceleration on the X axis - * @param driveAcceleration.second The linear acceleration on the Y axis - * @param fieldOriented Defines whether module states are calculated relative to field - */ - fun setClosedLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, - driveAcceleration: Pair = - Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), - fieldOriented: Boolean = true, - ) { - val velSwerveModuleStates: Array? - val accelSwerveModuleStates: Array? - - if (fieldOriented) { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - // This is with respect to the field, meaning all velocity and acceleration vectors are - // adjusted to be relative to the field instead of relative to the robot. - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation - ) - .chassisSpeedsWPILIB + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond ) - // Although this isn't perfect, calculating acceleration states using the same math as - // velocity can get us "good enough" accel states to minimize skew - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels.fromFieldRelativeAccels( - driveAcceleration.first, - driveAcceleration.second, - angularAcceleration, - odometryPose.rotation - ) - .chassisAccelsWPILIB - ) - } else { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) - .chassisSpeedsWPILIB - ) - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) - .chassisAccelsWPILIB - ) + setPointStates = velSwerveModuleStates.toMutableList() + + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) + } } - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond - ) + fun setClosedLoop( + chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, + chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + ) { + var chassisSpeeds = chassisSpeeds + + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vxMetersPerSecond.meters.perSecond, + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vyMetersPerSecond.meters.perSecond + ), + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.omegaRadiansPerSecond.radians.perSecond + ) + + val twistToNextPose: Twist2d = velocityTransform.log() + + chassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + .chassisSpeedsWPILIB + } - setPointStates = velSwerveModuleStates.toMutableList() + val velSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } - } - - fun setClosedLoop( - chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, - chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = - edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - ) { - var chassisSpeeds = chassisSpeeds - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vxMetersPerSecond.meters.perSecond, - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vyMetersPerSecond.meters.perSecond - ), - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.omegaRadiansPerSecond.radians.perSecond + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond ) - val twistToNextPose: Twist2d = velocityTransform.log() + setPointStates = velSwerveModuleStates.toMutableList() - chassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - .chassisSpeedsWPILIB + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) + } } - val velSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) - val accelSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + fun resetModuleZero() { + swerveModules.forEach { it.resetModuleZero() } + } - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond - ) + /** Zeros all the sensors on the drivetrain. */ + fun zeroSensors() { + zeroGyroPitch(0.0.degrees) + zeroGyroRoll() + zeroSteering() + zeroDrive() + } - setPointStates = velSwerveModuleStates.toMutableList() + /** + * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. + * + * @param toAngle Zeros the gyro to the value + */ + fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + gyroIO.zeroGyroYaw(toAngle) + + swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) + + if (RobotBase.isSimulation()) { + swerveDriveOdometry.resetPosition( + toAngle.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + undriftedPose.pose2d + ) + } - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) + if (!(gyroInputs.gyroConnected)) { + gyroYawOffset = toAngle - rawGyroAngle + } } - } - - fun resetModuleZero() { - swerveModules.forEach { it.resetModuleZero() } - } - - /** Zeros all the sensors on the drivetrain. */ - fun zeroSensors() { - zeroGyroPitch(0.0.degrees) - zeroGyroRoll() - zeroSteering() - zeroDrive() - } - - /** - * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. - * - * @param toAngle Zeros the gyro to the value - */ - fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - - if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( - toAngle.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - undriftedPose.pose2d - ) + + fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroPitch(toAngle) } - if (!(gyroInputs.gyroConnected)) { - gyroYawOffset = toAngle - rawGyroAngle + fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroRoll(toAngle) } - } - - fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroPitch(toAngle) - } - - fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroRoll(toAngle) - } - - /** Zeros the steering motors for each swerve module. */ - fun zeroSteering() { - swerveModules.forEach { it.zeroSteering() } - } - - /** Zeros the drive motors for each swerve module. */ - private fun zeroDrive() { - swerveModules.forEach { it.zeroDrive() } - } - - fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) - } - - companion object { - // Drivetrain multithreading - var odometryLock: Lock = ReentrantLock() - fun setOdometryLock(Locked: Boolean) { - if (Locked) { - odometryLock.lock() - } else { - odometryLock.unlock() - } + + /** Zeros the steering motors for each swerve module. */ + fun zeroSteering() { + swerveModules.forEach { it.zeroSteering() } } - // Drivetrain states for state machine. - enum class DrivetrainState { - UNINITIALIZED, - IDLE, - ZEROING_SENSORS, - OPEN_LOOP, - CLOSED_LOOP; - - inline fun equivalentToRequest(request: Request.DrivetrainRequest): Boolean { - return ( - (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || - (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || - (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || - (request is DrivetrainRequest.Idle && this == IDLE) - ) - } + /** Zeros the drive motors for each swerve module. */ + private fun zeroDrive() { + swerveModules.forEach { it.zeroDrive() } } - inline fun fromRequestToState(request: Request.DrivetrainRequest): DrivetrainState { - return when (request) { - is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP - is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP - is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS - is DrivetrainRequest.Idle -> DrivetrainState.IDLE - } + fun addVisionData(visionData: List) { + swerveDrivePoseEstimator.addVisionData(visionData) } - } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt index 419b53e4..8ffd9987 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt @@ -4,24 +4,24 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO interface DrivetrainIO { - fun getSwerveModules(): List { - return listOf( - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Right Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Right Wheel" - }) - ) - } -} + fun getSwerveModules(): List { + return listOf( + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Right Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Right Wheel" + }) + ) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt deleted file mode 100644 index e86fea20..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOReal.kt +++ /dev/null @@ -1,55 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.drive - -import com.ctre.phoenix6.hardware.TalonFX -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.Constants.Universal.CANIVORE_NAME -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule -import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOFalcon -import edu.wpi.first.wpilibj.AnalogInput - -object DrivetrainIOReal : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.FRONT_LEFT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.FRONT_LEFT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.FRONT_LEFT_ANALOG_POTENTIOMETER), - DrivetrainConstants.FRONT_LEFT_MODULE_ZERO, - Constants.Drivetrain.FRONT_LEFT_MODULE_NAME - ) - ), - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.FRONT_RIGHT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.FRONT_RIGHT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.FRONT_RIGHT_ANALOG_POTENTIOMETER), - DrivetrainConstants.FRONT_RIGHT_MODULE_ZERO, - Constants.Drivetrain.FRONT_RIGHT_MODULE_NAME - ) - ), - SwerveModule( - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.BACK_LEFT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.BACK_LEFT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.BACK_LEFT_ANALOG_POTENTIOMETER), - DrivetrainConstants.BACK_LEFT_MODULE_ZERO, - Constants.Drivetrain.BACK_LEFT_MODULE_NAME - ) - ), - SwerveModule( - // object: SwerveModuleIO { - // override val label: String = Constants.Drivetrain.BACK_RIGHT_MODULE_NAME - // } - SwerveModuleIOFalcon( - TalonFX(Constants.Drivetrain.BACK_RIGHT_STEERING_ID, CANIVORE_NAME), - TalonFX(Constants.Drivetrain.BACK_RIGHT_DRIVE_ID, CANIVORE_NAME), - AnalogInput(Constants.Drivetrain.BACK_RIGHT_ANALOG_POTENTIOMETER), - DrivetrainConstants.BACK_RIGHT_MODULE_ZERO, - Constants.Drivetrain.BACK_RIGHT_MODULE_NAME - ) - ) - ) - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt index b77281b3..25e40a71 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt @@ -4,12 +4,12 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOSim object DrivetrainIOSim : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule(SwerveModuleIOSim("Front Left Wheel")), - SwerveModule(SwerveModuleIOSim("Front Right Wheel")), - SwerveModule(SwerveModuleIOSim("Back Left Wheel")), - SwerveModule(SwerveModuleIOSim("Back Right Wheel")) - ) - } -} + override fun getSwerveModules(): List { + return listOf( + SwerveModule(SwerveModuleIOSim("Front Left Wheel")), + SwerveModule(SwerveModuleIOSim("Front Right Wheel")), + SwerveModule(SwerveModuleIOSim("Back Left Wheel")), + SwerveModule(SwerveModuleIOSim("Back Right Wheel")) + ) + } +} \ No newline at end of file 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 89cbd4d9..481e3875 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 @@ -10,52 +10,52 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond interface GyroIO { - class GyroIOInputs : LoggableInputs { - var rawGyroYaw = 0.0.radians - var gyroYaw = 0.0.radians - var gyroPitch = -3.degrees - var gyroRoll = 0.0.radians - var gyroYawRate = 0.0.radians.perSecond - var gyroPitchRate = 0.0.radians.perSecond - var gyroRollRate = 0.0.radians.perSecond - - var odometryYawPositions = arrayOf() - - var gyroConnected = false - - override fun toLog(table: LogTable?) { - table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) - table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) - table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) - table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) - table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) - table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) - table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) - table?.put("gyroConnected", gyroConnected) + class GyroIOInputs : LoggableInputs { + var rawGyroYaw = 0.0.radians + var gyroYaw = 0.0.radians + var gyroPitch = -3.degrees + var gyroRoll = 0.0.radians + var gyroYawRate = 0.0.radians.perSecond + var gyroPitchRate = 0.0.radians.perSecond + var gyroRollRate = 0.0.radians.perSecond + + var odometryYawPositions = arrayOf() + + var gyroConnected = false + + override fun toLog(table: LogTable?) { + table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) + table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) + table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) + table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) + table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) + table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) + table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) + table?.put("gyroConnected", gyroConnected) + } + + override fun fromLog(table: LogTable?) { + table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } + table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } + table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } + table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } + table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { + gyroYawRate = it.degrees.perSecond + } + table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { + gyroPitchRate = it.degrees.perSecond + } + table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { + gyroRollRate = it.degrees.perSecond + } + table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } + } } + fun updateInputs(inputs: GyroIOInputs) {} - override fun fromLog(table: LogTable?) { - table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } - table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } - table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } - table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } - table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { - gyroYawRate = it.degrees.perSecond - } - table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { - gyroPitchRate = it.degrees.perSecond - } - table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { - gyroRollRate = it.degrees.perSecond - } - table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } - } - } - fun updateInputs(inputs: GyroIOInputs) {} - - fun zeroGyroYaw(toAngle: Angle) {} + fun zeroGyroYaw(toAngle: Angle) {} - fun zeroGyroPitch(toAngle: Angle) {} + fun zeroGyroPitch(toAngle: Angle) {} - fun zeroGyroRoll(toAngle: Angle) {} -} + fun zeroGyroRoll(toAngle: Angle) {} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt index ce8a4d09..c08c5395 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt @@ -11,75 +11,77 @@ import org.team4099.lib.units.perSecond import kotlin.math.IEEErem object GyroIONavx : GyroIO { - private val gyro = AHRS(SerialPort.Port.kMXP) + private val gyro = AHRS(SerialPort.Port.kMXP) - init { - gyro.zeroYaw() - } + init { + gyro.zeroYaw() + } - var gyroYawOffset: Angle = 0.0.degrees - var gyroPitchOffset: Angle = 0.0.degrees - var gyroRollOffset: Angle = 0.0.degrees + var gyroYawOffset: Angle = 0.0.degrees + var gyroPitchOffset: Angle = 0.0.degrees + var gyroRollOffset: Angle = 0.0.degrees - /** The current angle of the drivetrain. */ - val gyroYaw: Angle - get() { - if (gyro.isConnected) { - var rawYaw = gyro.angle + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } + /** The current angle of the drivetrain. */ + val gyroYaw: Angle + get() { + if (gyro.isConnected) { + var rawYaw = gyro.angle + gyroYawOffset.inDegrees + rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate + return rawYaw.IEEErem(360.0).degrees + } else { + return (-1.337).degrees + } + } - val gyroPitch: Angle - get() { - if (gyro.isConnected) { - val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroPitch: Angle + get() { + if (gyro.isConnected) { + val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees + return rawPitch.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroRoll: Angle - get() { - if (gyro.isConnected) { - val rawRoll = gyro.roll + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroRoll: Angle + get() { + if (gyro.isConnected) { + val rawRoll = gyro.roll + gyroRollOffset.inDegrees + return rawRoll.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroYawRate: AngularVelocity - get() { - if (gyro.isConnected) { - return gyro.rate.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } + val gyroYawRate: AngularVelocity + get() { + if (gyro.isConnected) { + return gyro.rate.degrees.perSecond + } else { + return -1.337.degrees.perSecond + } + } - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - inputs.gyroYaw = gyroYaw - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll + override fun updateInputs(inputs: GyroIO.GyroIOInputs) { + inputs.rawGyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyroYaw + inputs.gyroYawRate = gyroYawRate + inputs.gyroPitch = gyroPitch + inputs.gyroRoll = gyroRoll - inputs.gyroConnected = gyro.isConnected - } + inputs.gyroConnected = gyro.isConnected + } - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees - } + override fun zeroGyroYaw(toAngle: Angle) { + gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees + } - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees - } + override fun zeroGyroPitch(toAngle: Angle) { + gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees + } - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees - } -} + override fun zeroGyroRoll(toAngle: Angle) { + gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees + } +} \ No newline at end of file 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 deleted file mode 100644 index 6a493d64..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIOPigeon2.kt +++ /dev/null @@ -1,144 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.gyro - -import com.ctre.phoenix6.configs.Pigeon2Configuration -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 -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.inDegreesPerSecond -import org.team4099.lib.units.perSecond -import java.util.Queue -import kotlin.math.IEEErem - -object GyroIOPigeon2 : GyroIO { - private var pigeon2 = Pigeon2(Constants.Gyro.PIGEON_2_ID, Constants.Universal.CANIVORE_NAME) - - private val isConnected: Boolean = pigeon2.upTime.value > 0.0 - - 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. */ - val gyroYaw: Angle - get() { - if (isConnected) { - var rawYaw = pigeon2.yaw.value + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyroYawRate.inDegreesPerSecond - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } - - val gyroPitch: Angle - get() { - if (isConnected) { - val rawPitch = pigeon2.pitch.value + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } - - val gyroRoll: Angle - get() { - if (isConnected) { - val rawRoll = pigeon2.roll.value + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } - - val gyroYawRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityZWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - val gyroPitchRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityYWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - val gyroRollRate: AngularVelocity - get() { - if (isConnected) { - return pigeon2.angularVelocityXWorld.value.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } - - init { - val pigeon2Configuration = Pigeon2Configuration() - pigeon2Configuration.MountPose.MountPosePitch = GyroConstants.mountPitch.inRadians - 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.yaw.getValueAsDouble() } - } - - // TODO look into more pigeon configuration stuff - pigeon2.configurator.apply(pigeon2Configuration) - } - - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - - inputs.rawGyroYaw = pigeon2.yaw.value.degrees - - inputs.gyroConnected = isConnected - - inputs.gyroYaw = gyroYaw - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll - - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitchRate = gyroPitchRate - inputs.gyroRollRate = gyroRollRate - - inputs.odometryYawPositions = - yawPositionQueue.stream().map { value: Double -> value.degrees }.toArray() as Array - yawPositionQueue.clear() - - Logger.recordOutput("Gyro/rawYawDegrees", pigeon2.yaw.value) - } - - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = toAngle - pigeon2.yaw.value.IEEErem(360.0).degrees - } - - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = toAngle - pigeon2.pitch.value.IEEErem(360.0).degrees - } - - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = toAngle - pigeon2.roll.value.IEEErem(360.0).degrees - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt new file mode 100644 index 00000000..77060f32 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -0,0 +1,276 @@ +package com.team4099.robot2023.subsystems.drivetrain.swervemodule + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import edu.wpi.first.math.kinematics.SwerveModulePosition +import edu.wpi.first.math.kinematics.SwerveModuleState +import edu.wpi.first.wpilibj.RobotBase.isReal +import org.littletonrobotics.junction.Logger +import org.team4099.lib.units.LinearAcceleration +import org.team4099.lib.units.LinearVelocity +import org.team4099.lib.units.base.feet +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.* +import org.team4099.lib.units.perSecond +import kotlin.math.IEEErem +import kotlin.math.withSign + +class SwerveModule(val io: SwerveModuleIO) { + val inputs = SwerveModuleIO.SwerveModuleIOInputs() + + var modulePosition = SwerveModulePosition() + + private var speedSetPoint: LinearVelocity = 0.feet.perSecond + private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond + + private var steeringSetPoint: Angle = 0.degrees + + private var shouldInvert = false + + private val steeringkP = + LoggedTunableValue( + "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) + ) + private val steeringkI = + LoggedTunableValue( + "Drivetrain/moduleSteeringkI", + Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val steeringkD = + LoggedTunableValue( + "Drivetrain/moduleSteeringkD", + Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + private val steeringMaxVel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX + ) + private val steeringMaxAccel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX + ) + + private val drivekP = + LoggedTunableValue( + "Drivetrain/moduleDrivekP", + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) + ) + + private val drivekI = + LoggedTunableValue( + "Drivetrain/moduleDrivekI", + Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) + ) + + private val drivekD = + LoggedTunableValue( + "Drivetrain/moduleDrivekD", + Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) + ) + + init { + if (isReal()) { + steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) + } else { + steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) + } + } + + fun periodic() { + io.updateInputs(inputs) + + // Updating SwerveModulePosition every loop cycle + modulePosition.distanceMeters = inputs.drivePosition.inMeters + modulePosition.angle = inputs.steeringPosition.inRotation2ds + + if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { + io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) + } + + if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { + io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) + } + + if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { + io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) + } + + Logger.processInputs(io.label, inputs) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveSpeedSetpointMetersPerSecond", + // if (!shouldInvert) speedSetPoint.inMetersPerSecond + // else -speedSetPoint.inMetersPerSecond + // ) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveAccelSetpointMetersPerSecondSq", + // accelerationSetPoint.inMetersPerSecondPerSecond + // ) + // Logger.getInstance() + // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/steeringValueDegreesWithMod", + // inputs.steeringPosition.inDegrees.IEEErem(360.0) + // ) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param steering The angular position desired for the swerve module to be set to + * @param speed The speed desired for the swerve module to reach + * @param acceleration The linear acceleration used to calculate how to reach the desired speed + */ + fun set( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, + optimize: Boolean = true + ) { + if (speed == 0.feet.perSecond) { + io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) + return + } + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians + + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians + } + + speedSetPoint = + if (shouldInvert) { + speed * -1 + } else { + speed + } + accelerationSetPoint = + if (shouldInvert) { + acceleration * -1 + } else { + acceleration + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + + // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + } + + fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians + + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians + } + + val outputSpeed = + if (shouldInvert) { + speed * -1 + } else { + speed + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + io.setOpenLoop(steeringSetPoint, outputSpeed) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using open loop control. + * + * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + */ + fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { + if (optimize) { + val optimizedState = + SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) + io.setOpenLoop( + optimizedState.angle.angle, + optimizedState + .speedMetersPerSecond + .meters + .perSecond // consider desaturating wheel speeds here if it doesn't work + // from drivetrain + ) + Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) + } else { + io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) + Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) + } + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration + * vectors. + * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) + */ + fun setPositionClosedLoop( + desiredVelState: SwerveModuleState, + desiredAccelState: SwerveModuleState, + optimize: Boolean = true + ) { + if (optimize) { + val optimizedVelState = + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + val optimizedAccelState = + SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) + io.setClosedLoop( + optimizedVelState.angle.angle, + optimizedVelState.speedMetersPerSecond.meters.perSecond, + optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) + } else { + io.setClosedLoop( + desiredVelState.angle.angle, + desiredVelState.speedMetersPerSecond.meters.perSecond, + desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) + } + } + + /** Creates event of the current potentiometer value as needs to be manually readjusted. */ + fun resetModuleZero() { + io.resetModuleZero() + } + + /** Zeros the steering motor */ + fun zeroSteering() { + io.zeroSteering() + } + + /** Zeros the drive motor */ + fun zeroDrive() { + io.zeroDrive() + } + + fun setDriveBrakeMode(brake: Boolean) { + io.setDriveBrakeMode(brake) + } + + fun setSteeringBrakeMode(brake: Boolean) { + io.setSteeringBrakeMode(brake) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt new file mode 100644 index 00000000..f40da16e --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -0,0 +1,122 @@ +package com.team4099.robot2023.subsystems.drivetrain.swervemodule + +import org.littletonrobotics.junction.LogTable +import org.littletonrobotics.junction.inputs.LoggableInputs +import org.team4099.lib.units.* +import org.team4099.lib.units.base.* +import org.team4099.lib.units.derived.* + +interface SwerveModuleIO { + class SwerveModuleIOInputs : LoggableInputs { + var driveAppliedVoltage = 0.0.volts + var steeringAppliedVoltage = 0.0.volts + + var driveStatorCurrent = 0.0.amps + var driveSupplyCurrent = 0.0.amps + var steeringStatorCurrent = 0.0.amps + var steeringSupplyCurrent = 0.0.amps + + var drivePosition = 0.0.meters + var steeringPosition = 0.0.degrees + + var driveVelocity = 0.0.meters.perSecond + var steeringVelocity = 0.0.degrees.perSecond + + var driveTemp = 0.0.celsius + var steeringTemp = 0.0.celsius + + var potentiometerOutputRaw = 0.0 + var potentiometerOutputRadians = 0.0.radians + + var drift = 0.0.meters + + override fun toLog(table: LogTable?) { + table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) + table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) + table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) + table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) + table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) + table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) + table?.put("drivePositionMeters", drivePosition.inMeters) + table?.put("steeringPositionRadians", steeringPosition.inRadians) + table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) + table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + table?.put("driveTempCelcius", driveTemp.inCelsius) + table?.put("steeringTempCelcius", steeringTemp.inCelsius) + table?.put("potentiometerOutputRaw", potentiometerOutputRaw) + table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) + table?.put("driftPositionMeters", drift.inMeters) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { + driveAppliedVoltage = it.volts + } + table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { + steeringAppliedVoltage = it.volts + } + table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { + driveStatorCurrent = it.amps + } + table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { + driveSupplyCurrent = it.amps + } + table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { + steeringStatorCurrent = it.amps + } + table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { + steeringSupplyCurrent = it.amps + } + table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { + drivePosition = it.meters + } + table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { + steeringPosition = it.radians + } + table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { + driveVelocity = it.meters.perSecond + } + table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + ?.let { steeringVelocity = it.radians.perSecond } + table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } + table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { + steeringTemp = it.celsius + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { + potentiometerOutputRaw = it + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { + potentiometerOutputRadians = it.radians + } + table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } + } + } + + val label: String + + fun updateInputs(inputs: SwerveModuleIOInputs) {} + + fun setSteeringSetpoint(angle: Angle) {} + fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} + fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} + + fun resetModuleZero() {} + fun zeroSteering() {} + fun zeroDrive() {} + + fun setDriveBrakeMode(brake: Boolean) {} + + fun setSteeringBrakeMode(brake: Boolean) {} + + fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) {} + fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt new file mode 100644 index 00000000..49074abd --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt @@ -0,0 +1,247 @@ +package com.team4099.robot2023.subsystems.drivetrain.swervemodule + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.BatterySim +import edu.wpi.first.wpilibj.simulation.FlywheelSim +import edu.wpi.first.wpilibj.simulation.RoboRioSim +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.PIDController +import org.team4099.lib.controller.SimpleMotorFeedforward +import org.team4099.lib.units.* +import org.team4099.lib.units.base.* +import org.team4099.lib.units.derived.* +import kotlin.random.Random + +class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { + // Use inverses of gear ratios because our standard is <1 is reduction + val driveMotorSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, + DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + val steerMotorSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, + DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + init { + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + } + + var turnRelativePosition = 0.0.radians + var turnAbsolutePosition = + (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to + var driveVelocity = 0.0.meters.perSecond + var drift: Length = 0.0.meters + + private val driveFeedback = + PIDController( + DrivetrainConstants.PID.SIM_DRIVE_KP, + DrivetrainConstants.PID.SIM_DRIVE_KI, + DrivetrainConstants.PID.SIM_DRIVE_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + private val driveFeedForward = + SimpleMotorFeedforward( + DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV + ) + + private val steeringFeedback = + PIDController( + DrivetrainConstants.PID.SIM_STEERING_KP, + DrivetrainConstants.PID.SIM_STEERING_KI, + DrivetrainConstants.PID.SIM_STEERING_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + + init { + steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) + steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + } + + override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { + driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + val angleDifference: Angle = + (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + .radians + turnAbsolutePosition += angleDifference + turnRelativePosition += angleDifference + + // constrains it to 2pi radians + while (turnAbsolutePosition < 0.radians) { + turnAbsolutePosition += (2.0 * Math.PI).radians + } + while (turnAbsolutePosition > (2.0 * Math.PI).radians) { + turnAbsolutePosition -= (2.0 * Math.PI).radians + } + + // s = r * theta -> d/2 * rad/s = m/s + driveVelocity = + (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond + + // simming drift + var loopCycleDrift = 0.0.meters + if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { + loopCycleDrift = + (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) + .meters // 0.0005 is just a nice number that ended up working out for drift + } + drift += loopCycleDrift + + // pi * d * rotations = distance travelled + inputs.drivePosition = + inputs.drivePosition + + DrivetrainConstants.WHEEL_DIAMETER * + Math.PI * + ( + driveMotorSim.angularVelocityRadPerSec * + Constants.Universal.LOOP_PERIOD_TIME.inSeconds + ) + .radians + .inRotations + + loopCycleDrift // adding a random amount of drift + inputs.steeringPosition = turnAbsolutePosition + inputs.drift = drift + + inputs.driveVelocity = driveVelocity + inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond + + inputs.driveAppliedVoltage = (-1337).volts + inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps + inputs.driveStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.driveTemp = (-1337).celsius + inputs.steeringTemp = (-1337).celsius + + inputs.steeringAppliedVoltage = (-1337).volts + inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps + inputs.steeringStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.potentiometerOutputRadians = turnAbsolutePosition + inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians + + // Setting a more accurate simulated voltage under load + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage( + inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes + ) + ) + } + + // 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) + driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) + } + + private fun setSteeringVoltage(volts: ElectricalPotential) { + val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + } + + override fun setSteeringSetpoint(angle: Angle) { + val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) + Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) + Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) + Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) + Logger.recordOutput( + "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + ) + setSteeringVoltage(feedback) + } + + override fun setClosedLoop( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration + ) { + Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) + val feedforward = driveFeedForward.calculate(speed, acceleration) + setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) + + setSteeringSetpoint(steering) + } + + override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { + setDriveVoltage( + RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) + ) + setSteeringSetpoint(steering) + } + + override fun resetModuleZero() { + println("Resetting your module's 0 doesn't do anything meaningful in sim :(") + } + + override fun zeroDrive() { + println("Zero drive do anything meaningful in sim") + } + + override fun zeroSteering() { + turnAbsolutePosition = 0.0.radians + } + + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + driveFeedback.setPID(kP, kI, kD) + } + + override fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + steeringFeedback.setPID(kP, kI, kD) + } + + override fun setDriveBrakeMode(brake: Boolean) { + println("Can't set brake mode in simulation") + } + + override fun configureSteeringMotionMagic( + maxVel: AngularVelocity, + maxAccel: AngularAcceleration + ) { + println("Can't configure motion magic in simulation") + } +} \ No newline at end of file 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 deleted file mode 100644 index 34d71a78..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModule.kt +++ /dev/null @@ -1,330 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.swervemodule - -import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.DrivetrainConstants -import edu.wpi.first.math.kinematics.SwerveModulePosition -import edu.wpi.first.math.kinematics.SwerveModuleState -import edu.wpi.first.wpilibj.RobotBase.isReal -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.LinearAcceleration -import org.team4099.lib.units.LinearVelocity -import org.team4099.lib.units.base.feet -import org.team4099.lib.units.base.inMeters -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.Angle -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.inVoltsPerDegree -import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond -import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds -import org.team4099.lib.units.derived.inVoltsPerMeters -import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond -import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds -import org.team4099.lib.units.derived.perMeterPerSecond -import org.team4099.lib.units.derived.perMeterPerSecondPerSecond -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inMetersPerSecond -import org.team4099.lib.units.inMetersPerSecondPerSecond -import org.team4099.lib.units.perSecond -import kotlin.math.IEEErem -import kotlin.math.abs -import kotlin.math.withSign - -class SwerveModule(val io: SwerveModuleIO) { - val inputs = SwerveModuleIO.SwerveModuleIOInputs() - - var modulePosition = SwerveModulePosition() - - var positionDeltas = mutableListOf() - - 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 = - LoggedTunableValue( - "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) - ) - private val steeringkI = - LoggedTunableValue( - "Drivetrain/moduleSteeringkI", - Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) - ) - private val steeringkD = - LoggedTunableValue( - "Drivetrain/moduleSteeringkD", - Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) - ) - - private val steeringMaxVel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX - ) - private val steeringMaxAccel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX - ) - - private val drivekP = - LoggedTunableValue( - "Drivetrain/moduleDrivekP", - Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) - ) - - private val drivekI = - LoggedTunableValue( - "Drivetrain/moduleDrivekI", - Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) - ) - - private val drivekD = - LoggedTunableValue( - "Drivetrain/moduleDrivekD", - Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) - ) - - init { - if (isReal()) { - steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) - } else { - steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) - 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 - - if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { - io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) - } - - if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { - io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) - } - - if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { - io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) - } - - Logger.processInputs(io.label, inputs) - Logger.recordOutput( - "${io.label}/driveSpeedSetpointMetersPerSecond", - if (!shouldInvert) speedSetPoint.inMetersPerSecond else -speedSetPoint.inMetersPerSecond - ) - Logger.recordOutput( - "${io.label}/driveAccelSetpointMetersPerSecondSq", - accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.recordOutput("${io.label}/steeringSetpointRadians", steeringSetPoint.inRadians) - Logger.recordOutput( - "${io.label}/steeringValueDegreesWithMod", inputs.steeringPosition.inDegrees.IEEErem(360.0) - ) - - Logger.recordOutput("SwerveModule/SpeedSetPoint", speedSetPoint.inMetersPerSecond) - Logger.recordOutput("SwerveModule/SteeringSetPoint", steeringSetPoint.inRadians) - Logger.recordOutput( - "SwerveModule/AccelerationSetPoint", accelerationSetPoint.inMetersPerSecondPerSecond - ) - Logger.recordOutput( - "SwerveModule/SteeringError", (steeringSetPoint - inputs.steeringPosition).inRadians - ) - } - - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param steering The angular position desired for the swerve module to be set to - * @param speed The speed desired for the swerve module to reach - * @param acceleration The linear acceleration used to calculate how to reach the desired speed - */ - fun set( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, - optimize: Boolean = true - ) { - if (speed == 0.feet.perSecond) { - io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) - return - } - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - speedSetPoint = - if (shouldInvert) { - speed * -1 - } else { - speed - } - accelerationSetPoint = - if (shouldInvert) { - acceleration * -1 - } else { - acceleration - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - - // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } - - fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - val outputSpeed = - if (shouldInvert) { - speed * -1 - } else { - speed - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - io.setOpenLoop(steeringSetPoint, outputSpeed) - } - - /** - * Sets the swerve module to the specified angular and X & Y velocities using open loop control. - * - * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - */ - fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { - if (optimize) { - val optimizedState = - SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) - io.setOpenLoop( - optimizedState.angle.angle, - optimizedState.speedMetersPerSecond.meters.perSecond * - Math.cos( - abs( - (optimizedState.angle.angle - inputs.steeringPosition) - .inRadians - ) - ) // consider desaturating wheel speeds here if it doesn't work - // from drivetrain - ) - Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) - } else { - io.setOpenLoop( - desiredState.angle.angle, - desiredState.speedMetersPerSecond.meters.perSecond * - Math.cos(abs((desiredState.angle.angle - inputs.steeringPosition).inRadians)) - ) - Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) - } - } - - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration - * vectors. - * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) - */ - fun setPositionClosedLoop( - desiredVelState: SwerveModuleState, - desiredAccelState: SwerveModuleState, - optimize: Boolean = true - ) { - if (optimize) { - val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) - val optimizedAccelState = - SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) - - steeringSetPoint = optimizedVelState.angle.angle - speedSetPoint = optimizedVelState.speedMetersPerSecond.meters.perSecond - accelerationSetPoint = optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } else { - steeringSetPoint = desiredVelState.angle.angle - speedSetPoint = desiredVelState.speedMetersPerSecond.meters.perSecond - accelerationSetPoint = desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } - } - - /** Creates event of the current potentiometer value as needs to be manually readjusted. */ - fun resetModuleZero() { - io.resetModuleZero() - } - - /** Zeros the steering motor */ - fun zeroSteering() { - io.zeroSteering() - } - - /** Zeros the drive motor */ - fun zeroDrive() { - io.zeroDrive() - } - - fun setDriveBrakeMode(brake: Boolean) { - io.setDriveBrakeMode(brake) - } - - fun setSteeringBrakeMode(brake: Boolean) { - io.setSteeringBrakeMode(brake) - } -} 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 deleted file mode 100644 index 95173245..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIO.kt +++ /dev/null @@ -1,146 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.swervemodule - -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.AngularAcceleration -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 -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inCelsius -import org.team4099.lib.units.base.inMeters -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.DerivativeGain -import org.team4099.lib.units.derived.IntegralGain -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.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.inMetersPerSecond -import org.team4099.lib.units.inRadiansPerSecond -import org.team4099.lib.units.perSecond - -interface SwerveModuleIO { - class SwerveModuleIOInputs : LoggableInputs { - var driveAppliedVoltage = 0.0.volts - var steeringAppliedVoltage = 0.0.volts - - var driveStatorCurrent = 0.0.amps - var driveSupplyCurrent = 0.0.amps - var steeringStatorCurrent = 0.0.amps - var steeringSupplyCurrent = 0.0.amps - - var drivePosition = 0.0.meters - var steeringPosition = 0.0.degrees - - var driveVelocity = 0.0.meters.perSecond - var steeringVelocity = 0.0.degrees.perSecond - - 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 - - var drift = 0.0.meters - - override fun toLog(table: LogTable?) { - table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) - table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) - table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) - table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) - table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) - table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) - table?.put("drivePositionMeters", drivePosition.inMeters) - table?.put("steeringPositionRadians", steeringPosition.inRadians) - table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) - table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - table?.put("driveTempCelcius", driveTemp.inCelsius) - table?.put("steeringTempCelcius", steeringTemp.inCelsius) - table?.put("potentiometerOutputRaw", potentiometerOutputRaw) - table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) - table?.put("driftPositionMeters", drift.inMeters) - } - - override fun fromLog(table: LogTable?) { - table?.get("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { - driveAppliedVoltage = it.volts - } - table?.get("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { - steeringAppliedVoltage = it.volts - } - table?.get("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { - driveStatorCurrent = it.amps - } - table?.get("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { - driveSupplyCurrent = it.amps - } - table?.get("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { - steeringStatorCurrent = it.amps - } - table?.get("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { - steeringSupplyCurrent = it.amps - } - table?.get("drivePositionMeters", drivePosition.inMeters)?.let { drivePosition = it.meters } - table?.get("steeringPositionRadians", steeringPosition.inRadians)?.let { - steeringPosition = it.radians - } - table?.get("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { - driveVelocity = it.meters.perSecond - } - table?.get("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond)?.let { - steeringVelocity = it.radians.perSecond - } - table?.get("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } - table?.get("steeringTempCelcius", steeringTemp.inCelsius)?.let { steeringTemp = it.celsius } - table?.get("potentiometerOutputRaw", potentiometerOutputRaw)?.let { - potentiometerOutputRaw = it - } - table?.get("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { - potentiometerOutputRadians = it.radians - } - table?.get("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } - } - } - - val label: String - - fun updateInputs(inputs: SwerveModuleIOInputs) {} - - fun setSteeringSetpoint(angle: Angle) {} - fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} - fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} - - fun resetModuleZero() {} - fun zeroSteering() {} - fun zeroDrive() {} - - fun setDriveBrakeMode(brake: Boolean) {} - - fun setSteeringBrakeMode(brake: Boolean) {} - - fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) {} - fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} - fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} -} 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 deleted file mode 100644 index b655a562..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOFalcon.kt +++ /dev/null @@ -1,458 +0,0 @@ -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 -import com.ctre.phoenix6.configs.TalonFXConfiguration -import com.ctre.phoenix6.controls.DutyCycleOut -import com.ctre.phoenix6.controls.PositionDutyCycle -import com.ctre.phoenix6.controls.VelocityDutyCycle -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.wpilibj.AnalogInput -import edu.wpi.first.wpilibj.RobotController -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.AngularAcceleration -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 -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.ctreAngularMechanismSensor -import org.team4099.lib.units.ctreLinearMechanismSensor -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.DerivativeGain -import org.team4099.lib.units.derived.IntegralGain -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.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 -import java.util.Queue - -class SwerveModuleIOFalcon( - private val steeringFalcon: TalonFX, - private val driveFalcon: TalonFX, - private val potentiometer: AnalogInput, - private val zeroOffset: Angle, - override val label: String -) : SwerveModuleIO { - private val steeringSensor = - ctreAngularMechanismSensor( - steeringFalcon, - DrivetrainConstants.STEERING_SENSOR_CPR, - DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_COMPENSATION_VOLTAGE - ) - - private val driveSensor = - ctreLinearMechanismSensor( - driveFalcon, - DrivetrainConstants.DRIVE_SENSOR_CPR, - DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.WHEEL_DIAMETER, - DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE - ) - - // motor params - private val steeringConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val driveConfiguration: TalonFXConfiguration = TalonFXConfiguration() - - private val potentiometerOutput: Double - get() { - return if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) { - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - } else { - 2 * PI - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - } - } - - 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) - - 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 - ) - ) - } - 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 = driveStatorCurrentSignal.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 - - inputs.driveVelocity = driveSensor.velocity - inputs.steeringVelocity = steeringSensor.velocity - - // processor temp is also something we may want to log ? - inputs.driveTemp = driveTempSignal.value.celsius - inputs.steeringTemp = steeringTempSignal.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 -> - (value / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO).rotations - } - .toArray() as - Array - drivePositionQueue.clear() - steeringPositionQueue.clear() - - inputs.potentiometerOutputRaw = - potentiometer.voltage / RobotController.getVoltage5V() * 2.0 * Math.PI - inputs.potentiometerOutputRadians = potentiometerOutput.radians - - Logger.recordOutput( - "$label/potentiometerRadiansWithOffset", - (inputs.potentiometerOutputRadians - zeroOffset).inRadians - ) - - Logger.recordOutput("$label/motorOutput", driveFalcon.get()) - } - - override fun setSteeringSetpoint(angle: Angle) { - steeringFalcon.setControl( - PositionDutyCycle( - steeringSensor.positionToRawUnits(angle), - steeringSensor.velocityToRawUnits(0.0.radians.perSecond), - DrivetrainConstants.FOC_ENABLED, - 0.0, - 0, - false, - false, - false - ) - ) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - val feedforward = DrivetrainConstants.PID.DRIVE_KS * speed.sign - driveFalcon.setControl( - VelocityDutyCycle( - driveSensor.velocityToRawUnits(speed), - driveSensor.accelerationToRawUnits(acceleration), - DrivetrainConstants.FOC_ENABLED, - feedforward.inVolts / DrivetrainConstants.DRIVE_COMPENSATION_VOLTAGE.inVolts, - 0, - false, - false, - false - ) - ) - - setSteeringSetpoint(steering) - } - - /** - * Open Loop Control using PercentOutput control on a Falcon - * - * @param steering: Desired angle - * @param speed: Desired speed - */ - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - driveFalcon.setControl( - DutyCycleOut( - speed / DrivetrainConstants.DRIVE_SETPOINT_MAX, - DrivetrainConstants.FOC_ENABLED, - false, - false, - false - ) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Absolute Potentiometer Value $label ($potentiometerOutput") - } - - override fun zeroSteering() { - steeringFalcon.setPosition( - steeringSensor.positionToRawUnits( - if (label != Constants.Drivetrain.BACK_RIGHT_MODULE_NAME) - (potentiometerOutput.radians) - zeroOffset - else (2 * PI).radians - (potentiometerOutput.radians - zeroOffset) - ) - ) - - drivePositionQueue.clear() - steeringPositionQueue.clear() - Logger.recordOutput("$label/zeroPositionRadians", steeringSensor.position.inRadians) - println("Loading Zero for Module $label (${steeringFalcon.position.value})") - } - - override fun zeroDrive() { - driveFalcon.setPosition(0.0) - drivePositionQueue.clear() - steeringPositionQueue.clear() - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - val PIDConfig = Slot0Configs() - - PIDConfig.kP = driveSensor.proportionalVelocityGainToRawUnits(kP) - PIDConfig.kI = driveSensor.integralVelocityGainToRawUnits(kI) - PIDConfig.kD = driveSensor.derivativeVelocityGainToRawUnits(kD) - PIDConfig.kV = 0.05425 - - driveFalcon.configurator.apply(PIDConfig) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - val PIDConfig = Slot0Configs() - - PIDConfig.kP = steeringSensor.proportionalPositionGainToRawUnits(kP) - PIDConfig.kI = steeringSensor.integralPositionGainToRawUnits(kI) - PIDConfig.kD = steeringSensor.derivativePositionGainToRawUnits(kD) - PIDConfig.kV = 0.05425 - - driveFalcon.configurator.apply(PIDConfig) - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - - val motionMagicConfig = MotionMagicConfigs() - - motionMagicConfig.MotionMagicCruiseVelocity = steeringSensor.velocityToRawUnits(maxVel) - motionMagicConfig.MotionMagicAcceleration = steeringSensor.accelerationToRawUnits(maxAccel) - - steeringFalcon.configurator.apply(motionMagicConfig) - } - - override fun setDriveBrakeMode(brake: Boolean) { - val motorOutputConfig = MotorOutputConfigs() - - if (brake) { - motorOutputConfig.NeutralMode = NeutralModeValue.Brake - } else { - motorOutputConfig.NeutralMode = NeutralModeValue.Coast - } - driveFalcon.configurator.apply(motorOutputConfig) - } - - override fun setSteeringBrakeMode(brake: Boolean) { - val motorOutputConfig = MotorOutputConfigs() - - if (brake) { - motorOutputConfig.NeutralMode = NeutralModeValue.Brake - } else { - motorOutputConfig.NeutralMode = NeutralModeValue.Coast - } - steeringFalcon.configurator.apply(motorOutputConfig) - // motor output configs might overwrite invert? - steeringFalcon.inverted = true - } -} 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 deleted file mode 100644 index 5a219453..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/SwerveModuleIOSim.kt +++ /dev/null @@ -1,274 +0,0 @@ -package com.team4099.robot2023.subsystems.drivetrain.swervemodule - -import com.team4099.lib.math.clamp -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor -import edu.wpi.first.math.system.plant.DCMotor -import edu.wpi.first.wpilibj.simulation.BatterySim -import edu.wpi.first.wpilibj.simulation.FlywheelSim -import edu.wpi.first.wpilibj.simulation.RoboRioSim -import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.PIDController -import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.AngularAcceleration -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 -import org.team4099.lib.units.base.inAmperes -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.DerivativeGain -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.IntegralGain -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.inKilogramsMeterSquared -import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.derived.inRotations -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.inVoltsPerDegree -import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond -import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inMetersPerSecond -import org.team4099.lib.units.perSecond -import kotlin.random.Random - -class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { - // Use inverses of gear ratios because our standard is <1 is reduction - val driveMotorSim: FlywheelSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - val steerMotorSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - init { - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - } - - var turnRelativePosition = 0.0.radians - var turnAbsolutePosition = - (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to - var driveVelocity = 0.0.meters.perSecond - var drift: Length = 0.0.meters - - private val driveFeedback = - PIDController( - DrivetrainConstants.PID.SIM_DRIVE_KP, - DrivetrainConstants.PID.SIM_DRIVE_KI, - DrivetrainConstants.PID.SIM_DRIVE_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - private val driveFeedForward = - SimpleMotorFeedforward( - DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV - ) - - private val steeringFeedback = - PIDController( - DrivetrainConstants.PID.SIM_STEERING_KP, - DrivetrainConstants.PID.SIM_STEERING_KI, - DrivetrainConstants.PID.SIM_STEERING_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - - init { - steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) - steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR - } - - override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - val angleDifference: Angle = - (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - .radians - turnAbsolutePosition += angleDifference - turnRelativePosition += angleDifference - - // constrains it to 2pi radians - while (turnAbsolutePosition < 0.radians) { - turnAbsolutePosition += (2.0 * Math.PI).radians - } - while (turnAbsolutePosition > (2.0 * Math.PI).radians) { - turnAbsolutePosition -= (2.0 * Math.PI).radians - } - - // s = r * theta -> d/2 * rad/s = m/s - driveVelocity = - (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - - // simming drift - var loopCycleDrift = 0.0.meters - if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { - loopCycleDrift = - (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) - .meters // 0.0005 is just a nice number that ended up working out for drift - } - drift += loopCycleDrift - - // pi * d * rotations = distance travelled - inputs.drivePosition = - inputs.drivePosition + - DrivetrainConstants.WHEEL_DIAMETER * - Math.PI * - ( - driveMotorSim.angularVelocityRadPerSec * - Constants.Universal.LOOP_PERIOD_TIME.inSeconds - ) - .radians - .inRotations + - loopCycleDrift // adding a random amount of drift - inputs.steeringPosition = turnAbsolutePosition - inputs.drift = drift - - inputs.driveVelocity = driveVelocity - inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond - - inputs.driveAppliedVoltage = (-1337).volts - inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps - inputs.driveStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.driveTemp = (-1337).celsius - inputs.steeringTemp = (-1337).celsius - - inputs.steeringAppliedVoltage = (-1337).volts - inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps - inputs.steeringStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.potentiometerOutputRadians = turnAbsolutePosition - inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians - - // Setting a more accurate simulated voltage under load - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage( - inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes - ) - ) - } - - // 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) - driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) - } - - private fun setSteeringVoltage(volts: ElectricalPotential) { - val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) - } - - override fun setSteeringSetpoint(angle: Angle) { - val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) - Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) - Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) - Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) - Logger.recordOutput( - "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond - ) - setSteeringVoltage(feedback) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) - val feedforward = driveFeedForward.calculate(speed, acceleration) - setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) - - setSteeringSetpoint(steering) - } - - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - setDriveVoltage( - RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Resetting your module's 0 doesn't do anything meaningful in sim :(") - } - - override fun zeroDrive() { - println("Zero drive do anything meaningful in sim") - } - - override fun zeroSteering() { - turnAbsolutePosition = 0.0.radians - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - driveFeedback.setPID(kP, kI, kD) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - steeringFeedback.setPID(kP, kI, kD) - } - - override fun setDriveBrakeMode(brake: Boolean) { - println("Can't set brake mode in simulation") - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - println("Can't configure motion magic in simulation") - } -} 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 deleted file mode 100644 index e9a0a4a1..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/PhoenixOdometryThread.java +++ /dev/null @@ -1,110 +0,0 @@ -// 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(false); - } - } - } -} \ 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 deleted file mode 100644 index 07d34186..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/swervemodule/threads/SparkMaxOdometryThread.java +++ /dev/null @@ -1,73 +0,0 @@ -// 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(true); - 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/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt index 3614c806..e65f207e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt @@ -5,14 +5,14 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.IntakeConstants import com.team4099.robot2023.subsystems.superstructure.Request import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.volts -class Intake(val io: IntakeIO) { +class Intake(val io: IntakeIO) : SubsystemBase() { val inputs = IntakeIO.IntakeIOInputs() var rollerVoltageTarget: ElectricalPotential = 0.0.volts var isZeroed = false @@ -35,7 +35,7 @@ class Intake(val io: IntakeIO) { private var timeProfileGeneratedAt = Clock.fpgaTime - fun periodic() { + override fun periodic() { io.updateInputs(inputs) Logger.processInputs("GroundIntake", inputs) From e496d457fd52eabccb57609f30dcfd94f3c8caed Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 14:49:50 -0500 Subject: [PATCH 22/38] initial commit --- .../com/team4099/robot2023/BuildConstants.kt | 10 +- .../com/team4099/robot2023/RobotContainer.kt | 3 +- .../drivetrain/DriveBrakeModeCommand.kt | 3 +- .../commands/drivetrain/DrivePathCommand.kt | 15 ++- .../commands/drivetrain/GoToAngle.kt | 9 +- .../commands/drivetrain/GyroAutoLevel.kt | 9 +- .../drivetrain/OpenLoopReverseCommand.kt | 3 +- .../commands/drivetrain/TargetPoseCommand.kt | 120 ++++++++++++++++++ .../commands/drivetrain/TeleopDriveCommand.kt | 3 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 3 +- .../subsystems/drivetrain/drive/Drivetrain.kt | 99 +++++++++++++++ 11 files changed, 252 insertions(+), 25 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 65645c9c..178cd4ce 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 = 26 -const val GIT_SHA = "73f4f3ef5bab17de6b7bbc919ece786ceb7b52ea" -const val GIT_DATE = "2024-01-15T19:49:07Z" +const val GIT_REVISION = 27 +const val GIT_SHA = "e6dd2ad00da75da1686f2dc6e2688aab809838a9" +const val GIT_DATE = "2024-01-15T20:47:37Z" const val GIT_BRANCH = "intake" -const val BUILD_DATE = "2024-01-15T20:41:51Z" -const val BUILD_UNIX_TIME = 1705369311220L +const val BUILD_DATE = "2024-01-16T14:47:03Z" +const val BUILD_UNIX_TIME = 1705434423104L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index f648bf94..dc76fdd6 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -14,6 +14,7 @@ import com.team4099.robot2023.subsystems.intake.IntakeIONEO import com.team4099.robot2023.subsystems.intake.IntakeIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan @@ -90,7 +91,7 @@ object RobotContainer { } fun zeroSensors() { - drivetrain.zeroSensors() + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() } fun zeroAngle(toAngle: Angle) { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index d7c4c6dd..97472978 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians @@ -12,7 +13,7 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 51438b3b..9c6fea93 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,6 +6,7 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -215,7 +216,7 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.setClosedLoop( + drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -280,16 +281,16 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.setOpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.setOpenLoop( - 0.degrees.perSecond, Pair(0.meters.perSecond, 0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 0779ea8f..30d1a3f2 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,6 +3,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -81,7 +82,7 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -97,8 +98,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.setOpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index 5d22049e..e49f994f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,6 +4,7 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController @@ -91,7 +92,7 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, gyroFeedback, fieldOriented = true ) @@ -119,8 +120,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.setOpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index c4847a4d..6f4fa148 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees @@ -12,7 +13,7 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.setOpenLoop( + drivetrain.currentRequest = DrivetrainRequest.OpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt new file mode 100644 index 00000000..9a4a7070 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -0,0 +1,120 @@ +package com.team4099.robot2023.commands.drivetrain + +import com.team4099.lib.logging.LoggedTunableValue +import com.team4099.robot2023.config.constants.DrivetrainConstants +import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request +import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import org.littletonrobotics.junction.Logger +import org.team4099.lib.controller.ProfiledPIDController +import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.geometry.Pose2d +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.perSecond +import kotlin.math.PI +import kotlin.math.tan + +class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { + private val thetaPID: ProfiledPIDController> + + val thetakP = + LoggedTunableValue( + "Pathfollow/thetakP", + Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) + ) + val thetakI = + LoggedTunableValue( + "Pathfollow/thetakI", + Pair( + { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } + ) + ) + val thetakD = + LoggedTunableValue( + "Pathfollow/thetakD", + Pair( + { it.inDegreesPerSecondPerDegreePerSecond }, + { it.degrees.perSecond.perDegreePerSecond } + ) + ) + + val thetaMaxVel = + LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) + val thetaMaxAccel = + LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) + var desiredAngle: Angle = 0.0.degrees + + init { + addRequirements(drivetrain) + + if (RobotBase.isReal()) { + thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) + } else { + thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) + thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) + thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) + } + + thetaPID = + ProfiledPIDController( + thetakP.get(), + thetakI.get(), + thetakD.get(), + TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) + ) + thetaPID.enableContinuousInput(-PI.radians, PI.radians) + } + + override fun initialize() { + thetaPID.reset(drivetrain.odometryPose.rotation) + } + + override fun execute() { + Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) + + val currentPose = drivetrain.odometryPose + desiredAngle = + tan((currentPose.y - targetPose.y).inMeters / (currentPose.x - targetPose.x).inMeters) + .radians + val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) + + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + thetaFeedback, + Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), + fieldOriented = true + ) + + Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) + Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) + Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) + } + + override fun isFinished(): Boolean { + return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < + DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR + } + + override fun end(interrupted: Boolean) { + drivetrain.currentRequest = + Request.DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 849c0b28..18f554e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -23,7 +24,7 @@ class TeleopDriveCommand( override fun execute() { val speed = driver.driveSpeedClampedSupplier(driveX, driveY, slowMode) val rotation = driver.rotationSpeedClampedSupplier(turn, slowMode) - drivetrain.setOpenLoop(rotation, speed) + drivetrain.currentRequest = DrivetrainRequest.OpenLoop(rotation, speed) Logger.recordOutput("ActiveCommands/TeleopDriveCommand", true) } override fun isFinished(): Boolean { diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index 8654eba7..d3b6d267 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,6 +1,7 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -11,7 +12,7 @@ class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { } override fun initialize() { - drivetrain.zeroSensors() + drivetrain.currentRequest = DrivetrainRequest.ZeroSensors() } override fun isFinished(): Boolean { 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 eae04997..870ddbdc 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 @@ -5,6 +5,7 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -168,6 +169,38 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB var lastGyroYaw = 0.0.radians + var velocityTarget = 0.degrees.perSecond + + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + + var fieldOrientation = true + + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + velocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + fieldOrientation = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + else -> {} + } + field = value + } + + init { + zeroSteering() + } + override fun periodic() { val startTime = Clock.realTimestamp gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) @@ -240,6 +273,44 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", (Clock.realTimestamp - startTime).inMilliseconds ) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors() + + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + } + + currentState = nextState + + // Log the current state + Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) } private fun updateOdometry() { @@ -553,4 +624,32 @@ class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemB fun addVisionData(visionData: List) { swerveDrivePoseEstimator.addVisionData(visionData) } + + companion object { + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP; + + inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) + ) + } + } + + inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + } + } + } } \ No newline at end of file From 1b0397f96193ee919d5cce7c2c5ec8d4098cfd87 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 15:40:42 -0500 Subject: [PATCH 23/38] current changes so far --- .../kotlin/com/team4099/robot2023/BuildConstants.kt | 12 ++++++------ .../kotlin/com/team4099/robot2023/RobotContainer.kt | 5 ++++- .../commands/drivetrain/TargetPoseCommand.kt | 9 +++++++++ 3 files changed, 19 insertions(+), 7 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 178cd4ce..39f5c23d 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 = 27 -const val GIT_SHA = "e6dd2ad00da75da1686f2dc6e2688aab809838a9" -const val GIT_DATE = "2024-01-15T20:47:37Z" -const val GIT_BRANCH = "intake" -const val BUILD_DATE = "2024-01-16T14:47:03Z" -const val BUILD_UNIX_TIME = 1705434423104L +const val GIT_REVISION = 28 +const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b" +const val GIT_DATE = "2024-01-16T14:49:50Z" +const val GIT_BRANCH = "targeting-command-with-drivetrain" +const val BUILD_DATE = "2024-01-16T15:27:50Z" +const val BUILD_UNIX_TIME = 1705436870465L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index dc76fdd6..f42e7050 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,6 +2,7 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand +import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -19,7 +20,9 @@ import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase +import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband +import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees @@ -115,7 +118,7 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) - ControlBoard.groundIntakeTest.whileTrue(intake.generateIntakeTestCommand()) + ControlBoard.groundIntakeTest.whileTrue(TargetPoseCommand(drivetrain, Pose2d(27.feet, 13.5.feet, 0.0.degrees))) // ControlBoard.autoLevel.whileActiveContinuous( // GoToAngle(drivetrain).andThen(AutoLevel(drivetrain)) // ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt index 9a4a7070..c1221e2f 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt @@ -19,6 +19,7 @@ import org.team4099.lib.units.derived.inDegrees import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.inRadians import org.team4099.lib.units.derived.perDegree import org.team4099.lib.units.derived.perDegreePerSecond import org.team4099.lib.units.derived.perDegreeSeconds @@ -61,6 +62,8 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co init { addRequirements(drivetrain) + Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) + if (RobotBase.isReal()) { thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) @@ -92,6 +95,12 @@ class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Co desiredAngle = tan((currentPose.y - targetPose.y).inMeters / (currentPose.x - targetPose.x).inMeters) .radians + + // 30.meters, 27.meters is goal, 3 meters / 4 + // 17.5 meters 13.5 meters is goal + + + Logger.recordOutput("AutoLevel/desiredAngleTargetPoseCmd", desiredAngle.inDegrees) val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) drivetrain.currentRequest = From 95035f5c2db4372293cd0d3addbdbd708c319cf7 Mon Sep 17 00:00:00 2001 From: Shom770 Date: Tue, 16 Jan 2024 15:58:16 -0500 Subject: [PATCH 24/38] working dt: --- .../com/team4099/robot2023/RobotContainer.kt | 2 - .../commands/drivetrain/TargetPoseCommand.kt | 129 ------------------ .../team4099/robot2023/config/ControlBoard.kt | 2 - 3 files changed, 133 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index f42e7050..c04891d5 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -2,7 +2,6 @@ package com.team4099.robot2023 import com.team4099.robot2023.auto.AutonomousSelector import com.team4099.robot2023.commands.drivetrain.ResetGyroYawCommand -import com.team4099.robot2023.commands.drivetrain.TargetPoseCommand import com.team4099.robot2023.commands.drivetrain.TeleopDriveCommand import com.team4099.robot2023.config.ControlBoard import com.team4099.robot2023.config.constants.Constants @@ -118,7 +117,6 @@ object RobotContainer { fun mapTeleopControls() { ControlBoard.resetGyro.whileTrue(ResetGyroYawCommand(drivetrain, toAngle = 180.degrees)) - ControlBoard.groundIntakeTest.whileTrue(TargetPoseCommand(drivetrain, Pose2d(27.feet, 13.5.feet, 0.0.degrees))) // ControlBoard.autoLevel.whileActiveContinuous( // GoToAngle(drivetrain).andThen(AutoLevel(drivetrain)) // ) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt deleted file mode 100644 index c1221e2f..00000000 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TargetPoseCommand.kt +++ /dev/null @@ -1,129 +0,0 @@ -package com.team4099.robot2023.commands.drivetrain - -import com.team4099.lib.logging.LoggedTunableValue -import com.team4099.robot2023.config.constants.DrivetrainConstants -import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj.RobotBase -import edu.wpi.first.wpilibj2.command.Command -import org.littletonrobotics.junction.Logger -import org.team4099.lib.controller.ProfiledPIDController -import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.geometry.Pose2d -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.inMeters -import org.team4099.lib.units.derived.Angle -import org.team4099.lib.units.derived.Radian -import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.inDegrees -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond -import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds -import org.team4099.lib.units.derived.inRadians -import org.team4099.lib.units.derived.perDegree -import org.team4099.lib.units.derived.perDegreePerSecond -import org.team4099.lib.units.derived.perDegreeSeconds -import org.team4099.lib.units.derived.radians -import org.team4099.lib.units.inDegreesPerSecond -import org.team4099.lib.units.perSecond -import kotlin.math.PI -import kotlin.math.tan - -class TargetPoseCommand(val drivetrain: Drivetrain, val targetPose: Pose2d) : Command() { - private val thetaPID: ProfiledPIDController> - - val thetakP = - LoggedTunableValue( - "Pathfollow/thetakP", - Pair({ it.inDegreesPerSecondPerDegree }, { it.degrees.perSecond.perDegree }) - ) - val thetakI = - LoggedTunableValue( - "Pathfollow/thetakI", - Pair( - { it.inDegreesPerSecondPerDegreeSeconds }, { it.degrees.perSecond.perDegreeSeconds } - ) - ) - val thetakD = - LoggedTunableValue( - "Pathfollow/thetakD", - Pair( - { it.inDegreesPerSecondPerDegreePerSecond }, - { it.degrees.perSecond.perDegreePerSecond } - ) - ) - - val thetaMaxVel = - LoggedTunableValue("Pathfollow/thetaMaxVel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_VEL) - val thetaMaxAccel = - LoggedTunableValue("Pathfollow/thetaMaxAccel", DrivetrainConstants.PID.MAX_AUTO_ANGULAR_ACCEL) - var desiredAngle: Angle = 0.0.degrees - - init { - addRequirements(drivetrain) - - Logger.recordOutput("Odometry/targetedPose", doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians)) - - if (RobotBase.isReal()) { - thetakP.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KP) - thetakI.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KI) - thetakD.initDefault(DrivetrainConstants.PID.AUTO_THETA_PID_KD) - } else { - thetakP.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KP) - thetakI.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KI) - thetakD.initDefault(DrivetrainConstants.PID.SIM_AUTO_THETA_PID_KD) - } - - thetaPID = - ProfiledPIDController( - thetakP.get(), - thetakI.get(), - thetakD.get(), - TrapezoidProfile.Constraints(thetaMaxVel.get(), thetaMaxAccel.get()) - ) - thetaPID.enableContinuousInput(-PI.radians, PI.radians) - } - - override fun initialize() { - thetaPID.reset(drivetrain.odometryPose.rotation) - } - - override fun execute() { - Logger.recordOutput("ActiveCommands/TargetPoseCommand", true) - - val currentPose = drivetrain.odometryPose - desiredAngle = - tan((currentPose.y - targetPose.y).inMeters / (currentPose.x - targetPose.x).inMeters) - .radians - - // 30.meters, 27.meters is goal, 3 meters / 4 - // 17.5 meters 13.5 meters is goal - - - Logger.recordOutput("AutoLevel/desiredAngleTargetPoseCmd", desiredAngle.inDegrees) - val thetaFeedback = thetaPID.calculate(currentPose.rotation, desiredAngle) - - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - thetaFeedback, - Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y), - fieldOriented = true - ) - - Logger.recordOutput("AutoLevel/CurrentYawDegrees", drivetrain.odometryPose.rotation.inDegrees) - Logger.recordOutput("AutoLevel/DesiredYawDegrees", desiredAngle.inDegrees) - Logger.recordOutput("AutoLevel/thetaFeedbackDPS", thetaFeedback.inDegreesPerSecond) - } - - override fun isFinished(): Boolean { - return (drivetrain.odometryPose.rotation - desiredAngle).absoluteValue < - DrivetrainConstants.PID.AUTO_THETA_ALLOWED_ERROR - } - - override fun end(interrupted: Boolean) { - drivetrain.currentRequest = - Request.DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(drivetrain.fieldVelocity.x, drivetrain.fieldVelocity.y) - ) - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index d1383fad..2a3446ac 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,6 +97,4 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } - - val groundIntakeTest = Trigger { driver.aButton } } From 716474971638d470a0b3d5cea086aaa7895b2cbe Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Fri, 19 Jan 2024 19:31:12 -0500 Subject: [PATCH 25/38] add wrapper class on velocity and position voltage requests --- .../team4099/lib/phoenix6/PositionVoltage.kt | 61 ++++++++++++++++++ .../team4099/lib/phoenix6/VelocityVoltage.kt | 64 +++++++++++++++++++ 2 files changed, 125 insertions(+) create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt create mode 100644 src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt new file mode 100644 index 00000000..51816cb9 --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -0,0 +1,61 @@ +package com.team4099.lib.phoenix6 + +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRotations +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 + +class PositionVoltage( + var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + var enableFOC: Boolean = true, + var feedforward: ElectricalPotential = 0.0.volts, + var slot: Int = 0, + var overrideBrakeDurNeutral: Boolean = false, + var limitForwardMotion: Boolean = false, + var limitReverseMotion: Boolean = false, + var velocity: AngularVelocity = 0.0.degrees.perSecond, +) { + + val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + + fun setPosition(new_position: Angle) { + position = new_position + positionVoltagePhoenix6.Position = new_position.inRotations + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + positionVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + positionVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + positionVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt new file mode 100644 index 00000000..6efd986b --- /dev/null +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -0,0 +1,64 @@ +package com.team4099.lib.phoenix6 + +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 +import org.team4099.lib.units.AngularAcceleration +import org.team4099.lib.units.AngularVelocity +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inRotationsPerSecond +import org.team4099.lib.units.inRotationsPerSecondPerSecond +import org.team4099.lib.units.perSecond + +class VelocityVoltage(var velocity: AngularVelocity, + var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + var enableFOC:Boolean = true, + var feedforward: ElectricalPotential = 0.0.volts, + var slot:Int = 0, + var overrideBrakeDurNeutral: Boolean = false, + var limitForwardMotion: Boolean = false, + var limitReverseMotion: Boolean = false){ + + val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } + + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} \ No newline at end of file From 6de365679bff41b24e4ab9984de79e7e1b82a895 Mon Sep 17 00:00:00 2001 From: Saraansh Wadkar Date: Fri, 19 Jan 2024 20:50:12 -0500 Subject: [PATCH 26/38] update falconutils to 1.1.28 --- build.gradle | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/build.gradle b/build.gradle index a21179a3..ba2238c0 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.28' implementation 'org.apache.commons:commons-collections4:4.0' implementation 'com.google.code.gson:gson:2.10.1' implementation "io.javalin:javalin:5.3.2" From eef9240f379f79b77d232840ba99cf49dd9640a3 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 23:02:06 -0500 Subject: [PATCH 27/38] rebase --- .../config/constants/ElevatorConstants.kt | 25 +------------------ 1 file changed, 1 insertion(+), 24 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 204f827d..d3c6c94c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -46,28 +46,5 @@ object ElevatorConstants { val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds val HOMING_APPLIED_VOLTAGE = 0.0.volts val ELEVATOR_GROUND_OFFSET = 0.0.inches - - val LEADER_VOLTAGE = 0.0.volts - val LEADER_GEAR_RATIO = 0.0 - val LEADER_SENSOR_CPR = 0 - val LEADER_DIAMETER = 0.0.inches - val LEADER_KP = 0.0.volts/1.0.inches.perSecond - val LEADER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds) - val LEADER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds) - val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps - val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps - val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps - - val FOLLOWER_VOLTAGE = 0.0.volts - val FOLLOWER_GEAR_RATIO = 0.0 - val FOLLOWER_SENSOR_CPR = 0 - val FOLLOWER_DIAMETER = 0.0.inches - val FOLLOWER_KP = 0.0.volts/1.0.inches.perSecond - val FOLLOWER_KI = 0.0.volts/(1.0.inches.perSecond*1.0.seconds) - val FOLLOWER_KD = 0.0.volts/(1.0.inches.perSecond/1.0.seconds) - val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps - val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps - val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps + } \ No newline at end of file From 8ed83018a05bb509c8a8e9d9eef42362dbd945e3 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 23:03:46 -0500 Subject: [PATCH 28/38] rebase --- .../config/constants/ElevatorConstants.kt | 12 +- .../subsystems/elevator/ElevatorIOKraken.kt | 20 +- .../subsystems/elevator/ElevatorIONEO.kt | 188 ++++++++++++++++++ 3 files changed, 205 insertions(+), 15 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index d3c6c94c..952536c3 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -1,9 +1,6 @@ package com.team4099.robot2023.config.constants -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.inches -import org.team4099.lib.units.base.meters -import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.base.* import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.perInch import org.team4099.lib.units.derived.rotations @@ -17,6 +14,11 @@ object ElevatorConstants { val REAL_KI = 0.0.volts / (1.inches * 1.seconds) val REAL_KD = 0.0.volts / (1.inches.perSecond) + val RAMP_RATE = 0.5.percent.perSecond + + val LEADER_INVERTED = false + val FOLLOWER_INVERTED = true + val SIM_KP = 0.0.volts / 1.inches val SIM_KI = 0.0.volts / (1.inches * 1.seconds) val SIM_KD = 0.0.volts / (1.inches.perSecond) @@ -46,5 +48,5 @@ object ElevatorConstants { val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds val HOMING_APPLIED_VOLTAGE = 0.0.volts val ELEVATOR_GROUND_OFFSET = 0.0.inches - + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 1ef1d354..e5f5851e 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -20,19 +20,19 @@ import org.team4099.lib.units.derived.* object ElevatorIOKraken: ElevatorIO { private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) - private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.LEADER_SENSOR_CPR, ElevatorConstants.LEADER_GEAR_RATIO, ElevatorConstants.LEADER_DIAMETER, ElevatorConstants.LEADER_VOLTAGE) - private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.FOLLOWER_SENSOR_CPR, ElevatorConstants.FOLLOWER_GEAR_RATIO, ElevatorConstants.FOLLOWER_DIAMETER, ElevatorConstants.FOLLOWER_VOLTAGE) + private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) + private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() - lateinit var elevatorLeaderStatorCurrentSignal: StatusSignal - lateinit var elevatorLeaderSupplyCurrentSignal: StatusSignal - lateinit var elevatorLeadertempSignal: StatusSignal - lateinit var elevatorLeaderDutyCycle: StatusSignal - lateinit var elevatorFollowerStatorCurrentSignal: StatusSignal - lateinit var elevatorFollowerSupplyCurrentSignal: StatusSignal - lateinit var elevatorFollowertempSignal: StatusSignal - lateinit var elevatorFollowerDutyCycle: StatusSignal + var elevatorLeaderStatorCurrentSignal: StatusSignal + var elevatorLeaderSupplyCurrentSignal: StatusSignal + var elevatorLeadertempSignal: StatusSignal + var elevatorLeaderDutyCycle: StatusSignal + var elevatorFollowerStatorCurrentSignal: StatusSignal + var elevatorFollowerSupplyCurrentSignal: StatusSignal + var elevatorFollowertempSignal: StatusSignal + var elevatorFollowerDutyCycle: StatusSignal init { elevatorLeaderKraken.clearStickyFaults() diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt new file mode 100644 index 00000000..659d9bd2 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -0,0 +1,188 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.revrobotics.CANSparkMax +import com.revrobotics.CANSparkMaxLowLevel +import com.revrobotics.SparkMaxPIDController +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.Neo +import org.littletonrobotics.junction.Logger +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 +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inPercentOutputPerSecond +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.sparkMaxLinearMechanismSensor +import kotlin.math.absoluteValue + +object ElevatorIONeo : ElevatorIO { + + private val leaderSparkMax = + CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val leaderSensor = + sparkMaxLinearMechanismSensor( + leaderSparkMax, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + private val followerSparkMax = + CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) + + private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController + + init { + + // reseting motor + leaderSparkMax.restoreFactoryDefaults() + followerSparkMax.restoreFactoryDefaults() + + leaderSparkMax.clearFaults() + followerSparkMax.clearFaults() + + // basic settings + leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + + leaderSparkMax.inverted = true + + leaderSparkMax.setSmartCurrentLimit(ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + followerSparkMax.setSmartCurrentLimit(ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + + leaderSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond + followerSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond + + leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + // makes follower motor output exact same power as leader + followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) + + leaderPIDController.ff = 0.0 + + leaderSparkMax.burnFlash() + followerSparkMax.burnFlash() + + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Neo(leaderSparkMax, "Leader Extension Motor"), + Neo(followerSparkMax, "Follower Extension Motor") + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + + inputs.elevatorVelocity = leaderSensor.velocity + + // voltage in * percent out + inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput + + inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps + + // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + + inputs.leaderSupplyCurrent = + inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue + + inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius + + // voltage in * percent out + inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput + + inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps + + inputs.followerSupplyCurrent = + inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue + + inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius + + inputs.leaderRawPosition = leaderSparkMax.encoder.position + + inputs.followerRawPosition = followerSparkMax.encoder.position + + Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + // divide by 2 cause full power elevator is scary + leaderSparkMax.setVoltage(voltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + leaderPIDController.setReference( + leaderSensor.positionToRawUnits( + clamp( + position, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + ) + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts, + ) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + leaderSparkMax.encoder.position = 0.0 + followerSparkMax.encoder.position = 0.0 + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) + leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) + leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) + } +} \ No newline at end of file From 8da8ad130d688c9f29d94d8c3af3515e877e2c21 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 23:05:59 -0500 Subject: [PATCH 29/38] fix more rebase --- .../com/team4099/robot2023/BuildConstants.kt | 15 -- .../com/team4099/robot2023/RobotContainer.kt | 18 ++- .../team4099/robot2023/config/ControlBoard.kt | 5 + .../config/constants/ElevatorConstants.kt | 18 +++ .../robot2023/subsystems/elevator/Elevator.kt | 29 +++- .../subsystems/elevator/ElevatorIONEO.kt | 2 +- .../subsystems/elevator/ElevatorIOSim.kt | 146 ++++++++++++++++++ .../subsystems/superstructure/Request.kt | 42 +---- 8 files changed, 211 insertions(+), 64 deletions(-) delete mode 100644 src/main/kotlin/com/team4099/robot2023/BuildConstants.kt create mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt deleted file mode 100644 index 39f5c23d..00000000 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ /dev/null @@ -1,15 +0,0 @@ -package com.team4099.robot2023 - -/** - * Automatically generated file containing build version information. - */ -const val MAVEN_GROUP = "" -const val MAVEN_NAME = "Crescendo-2024" -const val VERSION = "unspecified" -const val GIT_REVISION = 28 -const val GIT_SHA = "eca16395eb62f3d6d367479ab32e985e6472149b" -const val GIT_DATE = "2024-01-16T14:49:50Z" -const val GIT_BRANCH = "targeting-command-with-drivetrain" -const val BUILD_DATE = "2024-01-16T15:27:50Z" -const val BUILD_UNIX_TIME = 1705436870465L -const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index c04891d5..598a72ab 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -9,9 +9,9 @@ import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIO import com.team4099.robot2023.subsystems.drivetrain.drive.DrivetrainIOSim import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.intake.Intake -import com.team4099.robot2023.subsystems.intake.IntakeIONEO -import com.team4099.robot2023.subsystems.intake.IntakeIOSim +import com.team4099.robot2023.subsystems.elevator.Elevator +import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO +import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest @@ -27,16 +27,15 @@ import org.team4099.lib.units.derived.degrees object RobotContainer { private val drivetrain: Drivetrain - private val intake: Intake private val vision: Vision private val limelight: LimelightVision + private val elevator: Elevator init { if (RobotBase.isReal()) { // Real Hardware Implementations // drivetrain = Drivetrain(object: GyroIO {},object: DrivetrainIO {} drivetrain = Drivetrain(object : GyroIO {}, object : DrivetrainIO {}) - intake = Intake(IntakeIONEO) vision = Vision( // object: CameraIO {} @@ -48,10 +47,10 @@ object RobotContainer { // CameraIONorthstar("backward") ) limelight = LimelightVision(object : LimelightVisionIO {}) + elevator = Elevator(ElevatorIONEO) } else { // Simulation implementations drivetrain = Drivetrain(object : GyroIO {}, DrivetrainIOSim) - intake = Intake(IntakeIOSim) vision = Vision( CameraIONorthstar("northstar_1"), @@ -59,6 +58,7 @@ object RobotContainer { CameraIONorthstar("northstar_3"), ) limelight = LimelightVision(object : LimelightVisionIO {}) + elevator = Elevator(ElevatorIOSim) } vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) }) @@ -135,6 +135,12 @@ object RobotContainer { // Constants.Universal.Substation.SINGLE_SUBSTATION // ) // ) + + ControlBoard.elevatorOpenLoopExtend.whileTrue(elevator.testElevatorOpenLoopExtendCommand()) + ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand()) + ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) + ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) + } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 2a3446ac..87aeed9c 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -97,4 +97,9 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } + + val elevatorOpenLoopExtend = Trigger {driver.aButton} + val elevatorOpenLoopRetract = Trigger {driver.bButton} + val elevatorClosedLoopHigh = Trigger {driver.xButton} + val elevatorClosedLoopLow = Trigger { driver.yButton} } diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 952536c3..ef75cbf8 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -15,6 +15,10 @@ object ElevatorConstants { val REAL_KD = 0.0.volts / (1.inches.perSecond) val RAMP_RATE = 0.5.percent.perSecond + val CARRIAGE_MASS = 20.pounds + + val ELEVATOR_MAX_RETRACTION = 0.0.inches + val ELEVATOR_MAX_EXTENSION = 18.0.inches val LEADER_INVERTED = false val FOLLOWER_INVERTED = true @@ -49,4 +53,18 @@ object ElevatorConstants { val HOMING_APPLIED_VOLTAGE = 0.0.volts val ELEVATOR_GROUND_OFFSET = 0.0.inches + + val VOLTAGE_COMPENSATION = 12.0.volts + val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 + val SENSOR_CPR = 0 + val SPOOL_DIAMETER = 1.5.inches + + val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + + val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps + } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 34a63bd1..b53531ee 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -7,6 +7,10 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.RobotBase +import edu.wpi.first.wpilibj2.command.Command +import edu.wpi.first.wpilibj2.command.Commands +import edu.wpi.first.wpilibj2.command.Commands.run +import edu.wpi.first.wpilibj2.command.Commands.runOnce import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest import org.team4099.lib.controller.ElevatorFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -119,7 +123,6 @@ class Elevator(val io: ElevatorIO) { is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage is ElevatorRequest.TargetingPosition -> { elevatorPositionTarget = value.position - elevatorVelocityTarget = value.finalVelocity } else -> {} } @@ -326,4 +329,28 @@ class Elevator(val io: ElevatorIO) { } } } + + fun testElevatorOpenLoopRetractCommand(): Command { + return runOnce({ + currentRequest = ElevatorRequest.OpenLoop(-10.volts) + }) + } + + fun testElevatorOpenLoopExtendCommand(): Command { + return runOnce({ + currentRequest = ElevatorRequest.OpenLoop(10.volts) + }) + } + + fun elevatorClosedLoopRetractCommand(): Command { + return runOnce({ + currentRequest = ElevatorRequest.TargetingPosition(12.inches) + }) + } + + fun testElevatorClosedLoopExtendCommand(): Command { + return runOnce({ + currentRequest = ElevatorRequest.TargetingPosition(4.inches) + }) + } } \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 659d9bd2..72cded85 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -26,7 +26,7 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.sparkMaxLinearMechanismSensor import kotlin.math.absoluteValue -object ElevatorIONeo : ElevatorIO { +object ElevatorIONEO : ElevatorIO { private val leaderSparkMax = CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt new file mode 100644 index 00000000..0acf1792 --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -0,0 +1,146 @@ +package com.team4099.robot2023.subsystems.elevator + +import com.team4099.lib.math.clamp +import com.team4099.robot2023.config.constants.Constants +import com.team4099.robot2023.config.constants.ElevatorConstants +import com.team4099.robot2023.subsystems.falconspin.MotorChecker +import com.team4099.robot2023.subsystems.falconspin.MotorCollection +import com.team4099.robot2023.subsystems.falconspin.SimulatedMotor +import edu.wpi.first.math.system.plant.DCMotor +import edu.wpi.first.wpilibj.simulation.BatterySim +import edu.wpi.first.wpilibj.simulation.ElevatorSim +import edu.wpi.first.wpilibj.simulation.RoboRioSim +import org.team4099.lib.controller.PIDController +import org.team4099.lib.units.base.* +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.perSecond + +object ElevatorIOSim : ElevatorIO { + + val elevatorSim: ElevatorSim = ElevatorSim( + DCMotor.getNEO(2), + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.CARRIAGE_MASS.inKilograms, + ElevatorConstants.SPOOL_DIAMETER.inMeters/2, + ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, + ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + SimulatedMotor( + elevatorSim, + "Extension Motor", + ) + ), + baseCurrentLimit = 60.amps, + firstStageTemperatureLimit = 10.celsius, + firstStageCurrentLimit = 45.amps, + motorShutDownThreshold = 20.celsius + ) + ) + } + + private val elevatorController = + PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) + + private var lastAppliedVoltage = 0.0.volts + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.elevatorPosition = elevatorSim.positionMeters.meters + inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond + + inputs.leaderTempCelcius = 0.0.celsius + inputs.leaderStatorCurrent = 0.0.amps + inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.leaderAppliedVoltage = lastAppliedVoltage + + inputs.followerTempCelcius = 0.0.celsius + inputs.followerStatorCurrent = 0.0.amps + inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.followerAppliedVoltage = lastAppliedVoltage + + inputs.leaderRawPosition = 0.0 + inputs.followerRawPosition = 0.0 + + inputs.isSimulating = true + + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) + ) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + val ff = + clamp( + feedforward, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) + + lastAppliedVoltage = ff + feedback + elevatorSim.setInputVoltage((ff + feedback).inVolts) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + println("don't work right now") + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + elevatorController.setPID(kP, kI, kD) + } +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index e905aaef..a1e0e8c6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -10,59 +10,19 @@ import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.perSecond -// typealias GroundIntakeRequest = SuperStructureState.GroundIntakeStructure.GroundIntakeRequest -// typealias GroundIntakeState = SuperStructureState.GroundIntakeStructure.GroundIntakeState -// typealiasing for nested interfaces and sealed classes doesn't work -// https://youtrack.jetbrains.com/issue/KT-34281/Access-nested-classes-including-sealed-class-subclasses-through-typealias sealed interface Request { - sealed interface SuperstructureRequest : Request { - class Idle() : SuperstructureRequest - class Home() : SuperstructureRequest - class GroundIntakeCube() : SuperstructureRequest - class GroundIntakeCone() : SuperstructureRequest - - class EjectGamePiece() : SuperstructureRequest - - class DoubleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - class SingleSubstationIntakePrep(val gamePiece: GamePiece) : SuperstructureRequest - - class DoubleSubstationIntake() : SuperstructureRequest - class SingleSubstationIntake(val gamePiece: GamePiece) : SuperstructureRequest - - class PrepScore(val gamePiece: GamePiece, val nodeTier: NodeTier) : SuperstructureRequest - - class Score() : SuperstructureRequest - - class Tuning() : SuperstructureRequest - } - - // Implements RequestStructure to ensure standardized structure - sealed interface ManipulatorRequest : Request { - class TargetingPosition(val position: Length, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class OpenLoop(val voltage: ElectricalPotential, val rollerVoltage: ElectricalPotential) : - ManipulatorRequest - class Home() : ManipulatorRequest - } sealed interface ElevatorRequest : Request { class TargetingPosition( - val position: Length, - val finalVelocity: LinearVelocity = 0.0.inches.perSecond, - val canContinueBuffer: Length = 5.0.inches + val position: Length ) : ElevatorRequest class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest class Home() : ElevatorRequest } - sealed interface IntakeRequest : Request { - class OpenLoop(val rollerVoltage: ElectricalPotential) : IntakeRequest - class Idle() : IntakeRequest - } - sealed interface DrivetrainRequest : Request { class OpenLoop( val angularVelocity: AngularVelocity, From 81472cb1544cb796d89f61f5f301c55c1c628e16 Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 22:39:43 -0500 Subject: [PATCH 30/38] fix phoenix wrapper issues --- build.gradle | 2 +- .../team4099/lib/phoenix6/PositionVoltage.kt | 14 +-- .../team4099/lib/phoenix6/VelocityVoltage.kt | 16 +-- .../com/team4099/robot2023/BuildConstants.kt | 15 +++ .../config/constants/DrivetrainConstants.kt | 2 +- .../config/constants/IntakeConstants.kt | 17 --- .../robot2023/subsystems/intake/Intake.kt | 113 ------------------ .../robot2023/subsystems/intake/IntakeIO.kt | 63 ---------- .../subsystems/intake/IntakeIONEO.kt | 100 ---------------- .../subsystems/intake/IntakeIOSim.kt | 53 -------- 10 files changed, 32 insertions(+), 363 deletions(-) create mode 100644 src/main/kotlin/com/team4099/robot2023/BuildConstants.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt delete mode 100644 src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt diff --git a/build.gradle b/build.gradle index ba2238c0..a21179a3 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.28' + implementation 'com.github.team4099:FalconUtils:1.1.26' 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/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 51816cb9..5f7c1054 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -12,13 +12,13 @@ import org.team4099.lib.units.perSecond import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 class PositionVoltage( - var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity - var enableFOC: Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot: Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - var limitReverseMotion: Boolean = false, + private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false, var velocity: AngularVelocity = 0.0.degrees.perSecond, ) { diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt index 6efd986b..91339c15 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -11,14 +11,14 @@ import org.team4099.lib.units.inRotationsPerSecond import org.team4099.lib.units.inRotationsPerSecondPerSecond import org.team4099.lib.units.perSecond -class VelocityVoltage(var velocity: AngularVelocity, - var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, - var enableFOC:Boolean = true, - var feedforward: ElectricalPotential = 0.0.volts, - var slot:Int = 0, - var overrideBrakeDurNeutral: Boolean = false, - var limitForwardMotion: Boolean = false, - var limitReverseMotion: Boolean = false){ +class VelocityVoltage(private var velocity: AngularVelocity, + private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + private var enableFOC:Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot:Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false){ val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt new file mode 100644 index 00000000..4bf29e8a --- /dev/null +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -0,0 +1,15 @@ +package com.team4099.robot2023 + +/** + * Automatically generated file containing build version information. + */ +const val MAVEN_GROUP = "" +const val MAVEN_NAME = "Crescendo-2024" +const val VERSION = "unspecified" +const val GIT_REVISION = 45 +const val GIT_SHA = "5e1ed64a8159953426ea83a1f210857ac81b7ef8" +const val GIT_DATE = "2024-01-19T22:05:22Z" +const val GIT_BRANCH = "elevator" +const val BUILD_DATE = "2024-01-19T22:38:32Z" +const val BUILD_UNIX_TIME = 1705721912200L +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..280c2a36 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -170,4 +170,4 @@ object DrivetrainConstants { val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } -} +} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt deleted file mode 100644 index 00b49039..00000000 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/IntakeConstants.kt +++ /dev/null @@ -1,17 +0,0 @@ -package com.team4099.robot2023.config.constants - -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.derived.volts - -object IntakeConstants { - val ROLLER_INERTIA = 0.002459315 // this one has been updated - val VOLTAGE_COMPENSATION = 12.0.volts - - // TODO: Change gear ratio according to robot - val ROLLER_CURRENT_LIMIT = 50.0.amps - const val ROLLER_MOTOR_INVERTED = true - const val ROLLER_GEAR_RATIO = 24.0 / 12.0 // this one has been updated - - // TODO: Update value - val IDLE_ROLLER_VOLTAGE = 2.0.volts -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt deleted file mode 100644 index e65f207e..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/Intake.kt +++ /dev/null @@ -1,113 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.team4099.lib.hal.Clock -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import com.team4099.robot2023.subsystems.superstructure.Request -import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.SubsystemBase -import org.littletonrobotics.junction.Logger -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts - -class Intake(val io: IntakeIO) : SubsystemBase() { - val inputs = IntakeIO.IntakeIOInputs() - var rollerVoltageTarget: ElectricalPotential = 0.0.volts - var isZeroed = false - var currentState: IntakeState = IntakeState.UNINITIALIZED - - var currentRequest: Request.IntakeRequest = Request.IntakeRequest.Idle() - set(value) { - rollerVoltageTarget = when (value) { - is Request.IntakeRequest.OpenLoop -> { - value.rollerVoltage - } - - is Request.IntakeRequest.Idle -> { - IntakeConstants.IDLE_ROLLER_VOLTAGE - } - } - - field = value - } - - private var timeProfileGeneratedAt = Clock.fpgaTime - - override fun periodic() { - io.updateInputs(inputs) - - Logger.processInputs("GroundIntake", inputs) - Logger.recordOutput("GroundIntake/currentState", currentState.name) - Logger.recordOutput("GroundIntake/requestedState", currentRequest.javaClass.simpleName) - Logger.recordOutput("GroundIntake/isZeroed", isZeroed) - - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("GroundIntake/isAtCommandedState", currentState.equivalentToRequest(currentRequest)) - Logger.recordOutput("GroundIntake/timeProfileGeneratedAt", timeProfileGeneratedAt.inSeconds) - Logger.recordOutput("GroundIntake/rollerVoltageTarget", rollerVoltageTarget.inVolts) - } - - var nextState = currentState - when (currentState) { - IntakeState.UNINITIALIZED -> { - // Outputs - // No designated output functionality because targeting position will take care of it next - // loop cycle - - // Transitions - nextState = IntakeState.IDLE - } - IntakeState.IDLE -> { - setRollerVoltage(IntakeConstants.IDLE_ROLLER_VOLTAGE) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - IntakeState.OPEN_LOOP -> { - setRollerVoltage(rollerVoltageTarget) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - } - - // The next loop cycle, we want to run ground intake at the state that was requested. setting - // current state to the next state ensures that we run the logic for the state we want in the - // next loop cycle. - currentState = nextState - } - - /** @param appliedVoltage Represents the applied voltage of the roller motor */ - fun setRollerVoltage(appliedVoltage: ElectricalPotential) { - io.setRollerVoltage(appliedVoltage) - } - - fun generateIntakeTestCommand(): Command { - val returnCommand = runOnce({ currentRequest = Request.IntakeRequest.OpenLoop(12.volts) }) - return returnCommand - } - - companion object { - enum class IntakeState { - UNINITIALIZED, - IDLE, - OPEN_LOOP; - - fun equivalentToRequest(request: Request.IntakeRequest): Boolean { - return ( - (request is Request.IntakeRequest.OpenLoop && this == OPEN_LOOP) || - (request is Request.IntakeRequest.Idle && this == IDLE) - ) - } - } - - fun fromRequestToState(request: Request.IntakeRequest): IntakeState { - return when (request) { - is Request.IntakeRequest.OpenLoop -> IntakeState.OPEN_LOOP - is Request.IntakeRequest.Idle -> IntakeState.IDLE - } - } - } -} \ No newline at end of file diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt deleted file mode 100644 index 6ba96d7e..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIO.kt +++ /dev/null @@ -1,63 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import org.littletonrobotics.junction.LogTable -import org.littletonrobotics.junction.inputs.LoggableInputs -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.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.rotations -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.inRotationsPerMinute -import org.team4099.lib.units.perMinute -import org.team4099.lib.units.perSecond - -interface IntakeIO { - class IntakeIOInputs : LoggableInputs { - var rollerVelocity = 0.0.rotations.perMinute - var rollerAppliedVoltage = 0.0.volts - - var rollerSupplyCurrent = 0.0.amps - var rollerStatorCurrent = 0.0.amps - - var rollerTemp = 0.0.celsius - - var isSimulated = false - - override fun toLog(table: LogTable?) { - table?.put("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute) - table?.put("rollerAppliedVoltage", rollerAppliedVoltage.inVolts) - table?.put("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes) - table?.put("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes) - table?.put("rollerTempCelsius", rollerTemp.inCelsius) - } - - override fun fromLog(table: LogTable?) { - table?.get("rollerVelocityRPM", rollerVelocity.inRotationsPerMinute)?.let { - rollerVelocity = it.rotations.perSecond - } - - table?.get("rollerAppliedVoltage", rollerAppliedVoltage.inVolts)?.let { - rollerAppliedVoltage = it.volts - } - - table?.get("rollerSupplyCurrentAmps", rollerSupplyCurrent.inAmperes)?.let { - rollerSupplyCurrent = it.amps - } - - table?.get("rollerStatorCurrentAmps", rollerStatorCurrent.inAmperes)?.let { - rollerStatorCurrent = it.amps - } - - table?.get("rollerTempCelsius", rollerTemp.inCelsius)?.let { rollerTemp = it.celsius } - } - } - - fun updateInputs(io: IntakeIOInputs) {} - - fun setRollerVoltage(voltage: ElectricalPotential) {} - - fun setRollerBrakeMode(brake: Boolean) {} -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt deleted file mode 100644 index dadcd0db..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIONEO.kt +++ /dev/null @@ -1,100 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.revrobotics.CANSparkMax -import com.revrobotics.CANSparkMaxLowLevel -import com.team4099.lib.math.clamp -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import com.team4099.robot2023.subsystems.falconspin.MotorChecker -import com.team4099.robot2023.subsystems.falconspin.MotorCollection -import com.team4099.robot2023.subsystems.falconspin.Neo -import org.littletonrobotics.junction.Logger -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.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.sparkMaxAngularMechanismSensor -import kotlin.math.absoluteValue - -object IntakeIONEO : IntakeIO { - private val rollerSparkMax = - CANSparkMax(Constants.Intake.ROLLER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val rollerSensor = - sparkMaxAngularMechanismSensor( - rollerSparkMax, IntakeConstants.ROLLER_GEAR_RATIO, IntakeConstants.VOLTAGE_COMPENSATION - ) - - init { - rollerSparkMax.restoreFactoryDefaults() - rollerSparkMax.clearFaults() - - rollerSparkMax.enableVoltageCompensation(IntakeConstants.VOLTAGE_COMPENSATION.inVolts) - rollerSparkMax.setSmartCurrentLimit(IntakeConstants.ROLLER_CURRENT_LIMIT.inAmperes.toInt()) - rollerSparkMax.inverted = IntakeConstants.ROLLER_MOTOR_INVERTED - - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - - rollerSparkMax.openLoopRampRate = 0.0 - rollerSparkMax.burnFlash() - - MotorChecker.add( - "Ground Intake", - "Roller", - MotorCollection( - mutableListOf(Neo(rollerSparkMax, "Roller Motor")), - IntakeConstants.ROLLER_CURRENT_LIMIT, - 70.celsius, - IntakeConstants.ROLLER_CURRENT_LIMIT - 0.amps, - 90.celsius - ), - ) - } - - override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { - inputs.rollerVelocity = rollerSensor.velocity - inputs.rollerAppliedVoltage = rollerSparkMax.busVoltage.volts * rollerSparkMax.appliedOutput - inputs.rollerStatorCurrent = rollerSparkMax.outputCurrent.amps - - // BusVoltage * SupplyCurrent = AppliedVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BusVoltage - // SupplyCurrent = (percentOutput * BusVoltage / BusVoltage) * StatorCurrent = - // percentOutput * statorCurrent - inputs.rollerSupplyCurrent = - inputs.rollerStatorCurrent * rollerSparkMax.appliedOutput.absoluteValue - inputs.rollerTemp = rollerSparkMax.motorTemperature.celsius - - Logger.recordOutput( - "Intake/rollerMotorOvercurrentFault", - rollerSparkMax.getFault(CANSparkMax.FaultID.kOvercurrent) - ) - Logger.recordOutput("Intake/busVoltage", rollerSparkMax.busVoltage) - } - - /** - * Sets the roller motor voltage, ensures the voltage is limited to battery voltage compensation - * - * @param voltage the voltage to set the roller motor to - */ - override fun setRollerVoltage(voltage: ElectricalPotential) { - rollerSparkMax.setVoltage( - clamp(voltage, -IntakeConstants.VOLTAGE_COMPENSATION, IntakeConstants.VOLTAGE_COMPENSATION) - .inVolts - ) - } - - /** - * Sets the roller motor brake mode - * - * @param brake if it brakes - */ - override fun setRollerBrakeMode(brake: Boolean) { - if (brake) { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - } else { - rollerSparkMax.idleMode = CANSparkMax.IdleMode.kCoast - } - } -} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt deleted file mode 100644 index 801b2e84..00000000 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/intake/IntakeIOSim.kt +++ /dev/null @@ -1,53 +0,0 @@ -package com.team4099.robot2023.subsystems.intake - -import com.team4099.lib.math.clamp -import com.team4099.robot2023.config.constants.Constants -import com.team4099.robot2023.config.constants.IntakeConstants -import edu.wpi.first.math.system.plant.DCMotor -import edu.wpi.first.wpilibj.simulation.FlywheelSim -import org.team4099.lib.units.base.amps -import org.team4099.lib.units.base.celsius -import org.team4099.lib.units.base.inSeconds -import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.derived.inVolts -import org.team4099.lib.units.derived.rotations -import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.perMinute - -object IntakeIOSim : IntakeIO { - private val rollerSim: FlywheelSim = FlywheelSim( - DCMotor.getNEO(1), - IntakeConstants.ROLLER_GEAR_RATIO, - IntakeConstants.ROLLER_INERTIA - ) - - private var appliedVoltage = 0.volts; - init{} - - override fun updateInputs(inputs: IntakeIO.IntakeIOInputs) { - - rollerSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - - inputs.rollerVelocity = rollerSim.getAngularVelocityRPM().rotations.perMinute - inputs.rollerAppliedVoltage = appliedVoltage - inputs.rollerSupplyCurrent = 0.amps - inputs.rollerStatorCurrent = rollerSim.currentDrawAmps.amps - inputs.rollerTemp = 0.0.celsius - - inputs.isSimulated = true - } - - override fun setRollerVoltage(voltage: ElectricalPotential) { - appliedVoltage = voltage - rollerSim.setInputVoltage( - clamp( - voltage, - -IntakeConstants.VOLTAGE_COMPENSATION, - IntakeConstants.VOLTAGE_COMPENSATION - ) - .inVolts - ) - } - - override fun setRollerBrakeMode(brake: Boolean) {} -} From c46c3bc92fe95901276b9e67e8f6fd3e3c3273cd Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Fri, 19 Jan 2024 22:51:21 -0500 Subject: [PATCH 31/38] idk --- .../robot2023/subsystems/elevator/ElevatorIOKraken.kt | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index e5f5851e..97b6ad8b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -20,7 +20,11 @@ import org.team4099.lib.units.derived.* object ElevatorIOKraken: ElevatorIO { private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) - private val leaderSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) + private val leaderSensor + + + + = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() From 10827578dba0ce9953b4224958d9d2002b49193d Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Sat, 20 Jan 2024 19:43:56 -0500 Subject: [PATCH 32/38] fixed imports --- .../commands/drivetrain/DrivePathCommand.kt | 34 +++++++++++++++++-- .../drivetrain/servemodule/SwerveModuleIO.kt | 30 +++++++++++++--- .../servemodule/SwerveModuleIOSim.kt | 33 ++++++++++++++++-- 3 files changed, 87 insertions(+), 10 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 9c6fea93..4284a536 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -22,11 +22,39 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* import java.util.function.Supplier import kotlin.math.PI +import org.team4099.lib.units.Velocity +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.cos +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMeter +import org.team4099.lib.units.derived.inMetersPerSecondPerMeterSeconds +import org.team4099.lib.units.derived.inMetersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.metersPerSecondPerMetersPerSecond +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.perMeter +import org.team4099.lib.units.derived.perMeterSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.sin +import org.team4099.lib.units.inDegreesPerSecond +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.inMetersPerSecondPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.inRadiansPerSecondPerSecond +import org.team4099.lib.units.perSecond class DrivePathCommand( val drivetrain: Drivetrain, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt index f40da16e..c6c01a06 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -2,10 +2,32 @@ package com.team4099.robot2023.subsystems.drivetrain.swervemodule import org.littletonrobotics.junction.LogTable import org.littletonrobotics.junction.inputs.LoggableInputs -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* - +import org.team4099.lib.units.AngularAcceleration +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.Meter +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.inMeters +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.IntegralGain +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.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.inMetersPerSecond +import org.team4099.lib.units.inRadiansPerSecond +import org.team4099.lib.units.perSecond interface SwerveModuleIO { class SwerveModuleIOInputs : LoggableInputs { var driveAppliedVoltage = 0.0.volts diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt index 49074abd..92ce36db 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt @@ -13,10 +13,37 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.controller.SimpleMotorFeedforward -import org.team4099.lib.units.* -import org.team4099.lib.units.base.* -import org.team4099.lib.units.derived.* import kotlin.random.Random +import org.team4099.lib.units.AngularAcceleration +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 +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +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.inKilogramsMeterSquared +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotations +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerDegree +import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond +import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { // Use inverses of gear ratios because our standard is <1 is reduction From 4d50becea4e0791140d878ab6908b7e2042a0b57 Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Sat, 20 Jan 2024 20:36:41 -0500 Subject: [PATCH 33/38] fixed some programming issues --- .../team4099/lib/phoenix6/PositionVoltage.kt | 101 +- .../team4099/lib/phoenix6/VelocityVoltage.kt | 98 +- .../com/team4099/robot2023/RobotContainer.kt | 5 +- .../drivetrain/DriveBrakeModeCommand.kt | 5 +- .../commands/drivetrain/DrivePathCommand.kt | 23 +- .../commands/drivetrain/GoToAngle.kt | 24 +- .../commands/drivetrain/GyroAutoLevel.kt | 25 +- .../drivetrain/OpenLoopReverseCommand.kt | 5 +- .../commands/drivetrain/TeleopDriveCommand.kt | 2 +- .../commands/drivetrain/ZeroSensorsCommand.kt | 2 +- .../team4099/robot2023/config/ControlBoard.kt | 8 +- .../config/constants/DrivetrainConstants.kt | 2 +- .../config/constants/ElevatorConstants.kt | 140 ++- .../subsystems/drivetrain/drive/Drivetrain.kt | 1085 +++++++++-------- .../drivetrain/drive/DrivetrainIO.kt | 42 +- .../drivetrain/drive/DrivetrainIOSim.kt | 18 +- .../subsystems/drivetrain/gyro/GyroIO.kt | 90 +- .../subsystems/drivetrain/gyro/GyroIONavx.kt | 126 +- .../drivetrain/servemodule/SwerveModule.kt | 495 ++++---- .../drivetrain/servemodule/SwerveModuleIO.kt | 211 ++-- .../servemodule/SwerveModuleIOSim.kt | 442 +++---- .../robot2023/subsystems/elevator/Elevator.kt | 647 +++++----- .../subsystems/elevator/ElevatorIO.kt | 152 ++- .../subsystems/elevator/ElevatorIOKraken.kt | 271 ++-- .../subsystems/elevator/ElevatorIONEO.kt | 318 ++--- .../subsystems/elevator/ElevatorIOSim.kt | 252 ++-- .../subsystems/superstructure/Request.kt | 11 +- 27 files changed, 2389 insertions(+), 2211 deletions(-) diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt index 5f7c1054..939bf44c 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/PositionVoltage.kt @@ -12,50 +12,61 @@ import org.team4099.lib.units.perSecond import com.ctre.phoenix6.controls.PositionVoltage as PositionVoltagePhoenix6 class PositionVoltage( - private var position: Angle, // Assuming an AngularPosition type exists similar to AngularVelocity - private var enableFOC: Boolean = true, - private var feedforward: ElectricalPotential = 0.0.volts, - private var slot: Int = 0, - private var overrideBrakeDurNeutral: Boolean = false, - private var limitForwardMotion: Boolean = false, - private var limitReverseMotion: Boolean = false, - var velocity: AngularVelocity = 0.0.degrees.perSecond, + private var position: + Angle, // Assuming an AngularPosition type exists similar to AngularVelocity + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false, + var velocity: AngularVelocity = 0.0.degrees.perSecond, ) { - val positionVoltagePhoenix6 = PositionVoltagePhoenix6(position.inRotations, velocity.inRotationsPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) - - fun setPosition(new_position: Angle) { - position = new_position - positionVoltagePhoenix6.Position = new_position.inRotations - } - - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - positionVoltagePhoenix6.EnableFOC = new_enableFOC - } - - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } - - fun setSlot(new_slot: Int) { - slot = new_slot - positionVoltagePhoenix6.Slot = new_slot - } - - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } - - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - positionVoltagePhoenix6.LimitForwardMotion = new_limitForward - } - - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + val positionVoltagePhoenix6 = + PositionVoltagePhoenix6( + position.inRotations, + velocity.inRotationsPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) + + fun setPosition(new_position: Angle) { + position = new_position + positionVoltagePhoenix6.Position = new_position.inRotations + } + + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + positionVoltagePhoenix6.EnableFOC = new_enableFOC + } + + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + positionVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } + + fun setSlot(new_slot: Int) { + slot = new_slot + positionVoltagePhoenix6.Slot = new_slot + } + + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + positionVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } + + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + positionVoltagePhoenix6.LimitForwardMotion = new_limitForward + } + + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + positionVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt index 91339c15..dde287dc 100644 --- a/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt +++ b/src/main/kotlin/com/team4099/lib/phoenix6/VelocityVoltage.kt @@ -1,6 +1,5 @@ package com.team4099.lib.phoenix6 -import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.derived.ElectricalPotential @@ -10,55 +9,68 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inRotationsPerSecond import org.team4099.lib.units.inRotationsPerSecondPerSecond import org.team4099.lib.units.perSecond +import com.ctre.phoenix6.controls.VelocityVoltage as VelocityVoltagePhoenix6 -class VelocityVoltage(private var velocity: AngularVelocity, - private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, - private var enableFOC:Boolean = true, - private var feedforward: ElectricalPotential = 0.0.volts, - private var slot:Int = 0, - private var overrideBrakeDurNeutral: Boolean = false, - private var limitForwardMotion: Boolean = false, - private var limitReverseMotion: Boolean = false){ +class VelocityVoltage( + private var velocity: AngularVelocity, + private var acceleration: AngularAcceleration = 0.0.degrees.perSecond.perSecond, + private var enableFOC: Boolean = true, + private var feedforward: ElectricalPotential = 0.0.volts, + private var slot: Int = 0, + private var overrideBrakeDurNeutral: Boolean = false, + private var limitForwardMotion: Boolean = false, + private var limitReverseMotion: Boolean = false +) { - val velocityVoltagePhoenix6 = VelocityVoltagePhoenix6(velocity.inRotationsPerSecond, acceleration.inRotationsPerSecondPerSecond, enableFOC, feedforward.inVolts, slot, overrideBrakeDurNeutral, limitForwardMotion, limitReverseMotion) + val velocityVoltagePhoenix6 = + VelocityVoltagePhoenix6( + velocity.inRotationsPerSecond, + acceleration.inRotationsPerSecondPerSecond, + enableFOC, + feedforward.inVolts, + slot, + overrideBrakeDurNeutral, + limitForwardMotion, + limitReverseMotion + ) - fun setVelocity(new_velocity: AngularVelocity) { - velocity = new_velocity - velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond - } + fun setVelocity(new_velocity: AngularVelocity) { + velocity = new_velocity + velocityVoltagePhoenix6.Velocity = velocity.inRotationsPerSecond + } - fun setAcceleration(new_accel: AngularAcceleration) { - acceleration = new_accel - velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond - } + fun setAcceleration(new_accel: AngularAcceleration) { + acceleration = new_accel + velocityVoltagePhoenix6.Acceleration = acceleration.inRotationsPerSecondPerSecond + } - fun setEnableFOC(new_enableFOC: Boolean) { - enableFOC = new_enableFOC - velocityVoltagePhoenix6.EnableFOC = new_enableFOC - } + fun setEnableFOC(new_enableFOC: Boolean) { + enableFOC = new_enableFOC + velocityVoltagePhoenix6.EnableFOC = new_enableFOC + } - fun setFeedforward(new_feedforward: ElectricalPotential) { - feedforward = new_feedforward - velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts - } + fun setFeedforward(new_feedforward: ElectricalPotential) { + feedforward = new_feedforward + velocityVoltagePhoenix6.FeedForward = new_feedforward.inVolts + } - fun setSlot(new_slot: Int) { - slot = new_slot - velocityVoltagePhoenix6.Slot = new_slot - } + fun setSlot(new_slot: Int) { + slot = new_slot + velocityVoltagePhoenix6.Slot = new_slot + } - fun setOverrideBrakeDurNeutral(new_override: Boolean) { - overrideBrakeDurNeutral = new_override - velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override - } + fun setOverrideBrakeDurNeutral(new_override: Boolean) { + overrideBrakeDurNeutral = new_override + velocityVoltagePhoenix6.OverrideBrakeDurNeutral = new_override + } - fun setLimitForwardMotion(new_limitForward: Boolean) { - limitForwardMotion = new_limitForward - velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward - } + fun setLimitForwardMotion(new_limitForward: Boolean) { + limitForwardMotion = new_limitForward + velocityVoltagePhoenix6.LimitForwardMotion = new_limitForward + } - fun setLimitReverseMotion(new_limitReverse: Boolean) { - limitReverseMotion = new_limitReverse - velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse - } -} \ No newline at end of file + fun setLimitReverseMotion(new_limitReverse: Boolean) { + limitReverseMotion = new_limitReverse + velocityVoltagePhoenix6.LimitReverseMotion = new_limitReverse + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 598a72ab..60ca3cb5 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -14,16 +14,14 @@ import com.team4099.robot2023.subsystems.elevator.ElevatorIONEO import com.team4099.robot2023.subsystems.elevator.ElevatorIOSim import com.team4099.robot2023.subsystems.limelight.LimelightVision import com.team4099.robot2023.subsystems.limelight.LimelightVisionIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.subsystems.vision.Vision import com.team4099.robot2023.subsystems.vision.camera.CameraIONorthstar import com.team4099.robot2023.util.driver.Ryan import edu.wpi.first.wpilibj.RobotBase -import org.team4099.lib.geometry.Pose2d import org.team4099.lib.smoothDeadband -import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.Angle import org.team4099.lib.units.derived.degrees +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest object RobotContainer { private val drivetrain: Drivetrain @@ -140,7 +138,6 @@ object RobotContainer { ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand()) ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) - } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt index 97472978..3eb85b2e 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DriveBrakeModeCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.radians import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class DriveBrakeModeCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) ) drivetrain.swerveModules.forEach() { it.setDriveBrakeMode(true) } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt index 4284a536..12a8841c 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/DrivePathCommand.kt @@ -6,7 +6,6 @@ import com.team4099.lib.trajectory.CustomTrajectoryGenerator import com.team4099.lib.trajectory.Waypoint import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.AllianceFlipUtil import com.team4099.robot2023.util.Velocity2d import edu.wpi.first.math.kinematics.ChassisSpeeds @@ -22,8 +21,6 @@ import org.team4099.lib.geometry.Pose2d import org.team4099.lib.geometry.Pose2dWPILIB import org.team4099.lib.hal.Clock import org.team4099.lib.kinematics.ChassisAccels -import java.util.function.Supplier -import kotlin.math.PI import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inMeters @@ -55,6 +52,9 @@ import org.team4099.lib.units.inMetersPerSecondPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.inRadiansPerSecondPerSecond import org.team4099.lib.units.perSecond +import java.util.function.Supplier +import kotlin.math.PI +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class DrivePathCommand( val drivetrain: Drivetrain, @@ -244,7 +244,8 @@ class DrivePathCommand( Pose2dWPILIB(desiredState.poseMeters.translation, desiredRotation.position) ) - drivetrain.currentRequest = DrivetrainRequest.ClosedLoop( + drivetrain.currentRequest = + DrivetrainRequest.ClosedLoop( nextDriveState, ChassisAccels(xAccel, yAccel, 0.0.radians.perSecond.perSecond).chassisAccelsWPILIB ) @@ -309,16 +310,18 @@ class DrivePathCommand( Logger.recordOutput("ActiveCommands/DrivePathCommand", false) if (interrupted) { // Stop where we are if interrupted - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } else { // Execute one last time to end up in the final state of the trajectory // Since we weren't interrupted, we know curTime > endTime execute() - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt index 30d1a3f2..f48ddc33 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GoToAngle.kt @@ -3,7 +3,6 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger @@ -11,10 +10,21 @@ import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegree +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreePerSecond +import org.team4099.lib.units.derived.inDegreesPerSecondPerDegreeSeconds +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond import kotlin.math.PI +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GoToAngle(val drivetrain: Drivetrain) : Command() { private val thetaPID: ProfiledPIDController> @@ -82,7 +92,8 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { val thetaFeedback = thetaPID.calculate(drivetrain.odometryPose.rotation, desiredAngle) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( thetaFeedback, Pair(0.0.meters.perSecond, 0.0.meters.perSecond), fieldOriented = true ) @@ -98,8 +109,9 @@ class GoToAngle(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index e49f994f..43260231 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -4,16 +4,23 @@ import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile -import org.team4099.lib.units.* +import org.team4099.lib.units.Value +import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.Second import org.team4099.lib.units.base.meters -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.Radian +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.inMetersPerSecond +import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { private val gyroPID: ProfiledPIDController> @@ -92,9 +99,8 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { override fun execute() { Logger.recordOutput("ActiveCommands/AutoLevelCommand", true) - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, gyroFeedback, fieldOriented = true - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop(0.0.radians.perSecond, gyroFeedback, fieldOriented = true) Logger.recordOutput( "AutoLevel/DesiredPitchDegrees", DrivetrainConstants.DOCKING_GYRO_SETPOINT.inDegrees @@ -120,8 +126,9 @@ class GyroAutoLevel(val drivetrain: Drivetrain) : Command() { } override fun end(interrupted: Boolean) { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( - 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - ) + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( + 0.0.radians.perSecond, Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + ) } } diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt index 6f4fa148..257be010 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/OpenLoopReverseCommand.kt @@ -1,11 +1,11 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.team4099.lib.units.base.feet import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { init { @@ -13,7 +13,8 @@ class OpenLoopReverseCommand(val drivetrain: Drivetrain) : Command() { } override fun execute() { - drivetrain.currentRequest = DrivetrainRequest.OpenLoop( + drivetrain.currentRequest = + DrivetrainRequest.OpenLoop( 0.degrees.perSecond, Pair(-5.0.feet.perSecond, 0.0.feet.perSecond), fieldOriented = false diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt index 18f554e4..6a09a062 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/TeleopDriveCommand.kt @@ -1,10 +1,10 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.driver.DriverProfile import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class TeleopDriveCommand( val driver: DriverProfile, diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt index d3b6d267..e8510fb3 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/ZeroSensorsCommand.kt @@ -1,9 +1,9 @@ package com.team4099.robot2023.commands.drivetrain import com.team4099.robot2023.subsystems.drivetrain.drive.Drivetrain -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class ZeroSensorsCommand(val drivetrain: Drivetrain) : Command() { diff --git a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt index 87aeed9c..7ace6cad 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt @@ -98,8 +98,8 @@ object ControlBoard { // for testing val setArmCommand = Trigger { technician.yButton } - val elevatorOpenLoopExtend = Trigger {driver.aButton} - val elevatorOpenLoopRetract = Trigger {driver.bButton} - val elevatorClosedLoopHigh = Trigger {driver.xButton} - val elevatorClosedLoopLow = Trigger { driver.yButton} + val elevatorOpenLoopExtend = Trigger { driver.aButton } + val elevatorOpenLoopRetract = Trigger { driver.bButton } + val elevatorClosedLoopHigh = Trigger { driver.xButton } + val elevatorClosedLoopLow = Trigger { driver.yButton } } 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 280c2a36..13aac5e5 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/DrivetrainConstants.kt @@ -170,4 +170,4 @@ object DrivetrainConstants { val SIM_STEERING_KI = 0.0.volts.perDegreeSeconds val SIM_STEERING_KD = 0.0.volts.perDegreePerSecond } -} \ No newline at end of file +} diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index ef75cbf8..5fba2d33 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -1,70 +1,84 @@ package com.team4099.robot2023.config.constants -import org.team4099.lib.units.base.* +import org.team4099.lib.units.base.Meter +import org.team4099.lib.units.base.amps +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters +import org.team4099.lib.units.base.percent +import org.team4099.lib.units.base.pounds +import org.team4099.lib.units.base.seconds +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.degrees -import org.team4099.lib.units.derived.perInch -import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts -import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond object ElevatorConstants { - // TODO: Change values later based on CAD - val REAL_KP = 0.0.volts / 1.inches - val REAL_KI = 0.0.volts / (1.inches * 1.seconds) - val REAL_KD = 0.0.volts / (1.inches.perSecond) - - val RAMP_RATE = 0.5.percent.perSecond - val CARRIAGE_MASS = 20.pounds - - val ELEVATOR_MAX_RETRACTION = 0.0.inches - val ELEVATOR_MAX_EXTENSION = 18.0.inches - - val LEADER_INVERTED = false - val FOLLOWER_INVERTED = true - - val SIM_KP = 0.0.volts / 1.inches - val SIM_KI = 0.0.volts / (1.inches * 1.seconds) - val SIM_KD = 0.0.volts / (1.inches.perSecond) - - val ELEVATOR_KS = 0.0.volts - val ELEVATOR_KG = 0.0.volts - val ELEVATOR_KV = 0.0.volts/0.0.inches.perSecond - val ELEVATOR_KA = 0.0.volts/0.0.inches.perSecond.perSecond - - val ENABLE_ELEVATOR = 1.0 - val ELEVATOR_IDLE_HEIGHT = 0.0.inches - val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches - val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches - val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches - val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches - - val ELEVATOR_TOLERANCE = 0.0.inches - - val MAX_VELOCITY = 0.0.meters.perSecond - val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond - - val SHOOT_SPEAKER_POSITION = 0.0.inches - val SHOOT_AMP_POSITION = 0.0.inches - val SOURCE_NOTE_OFFSET = 0.0.inches - val ELEVATOR_THETA_POS = 0.0.degrees - val HOMING_STATOR_CURRENT = 0.0.amps - val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds - val HOMING_APPLIED_VOLTAGE = 0.0.volts - val ELEVATOR_GROUND_OFFSET = 0.0.inches - - - val VOLTAGE_COMPENSATION = 12.0.volts - val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 - val SENSOR_CPR = 0 - val SPOOL_DIAMETER = 1.5.inches - - val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps - val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps - val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps - - val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds - val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps - -} \ No newline at end of file + // TODO: Change values later based on CAD + val REAL_KP = 0.0.volts / 1.inches + val REAL_KI = 0.0.volts / (1.inches * 1.seconds) + val REAL_KD = 0.0.volts / (1.inches.perSecond) + + val CARRIAGE_MASS = 20.pounds + + val ELEVATOR_MAX_RETRACTION = 0.0.inches + val ELEVATOR_MAX_EXTENSION = 18.0.inches + + val LEADER_INVERTED = false + val FOLLOWER_INVERTED = true + + val LEADER_KP: ProportionalGain = 0.0.volts / 1.inches + val LEADER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val LEADER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val FOLLOWER_KP: ProportionalGain = 0.0.volts / 1.inches + val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) + val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) + + val SIM_KP = 0.0.volts / 1.inches + val SIM_KI = 0.0.volts / (1.inches * 1.seconds) + val SIM_KD = 0.0.volts / (1.inches.perSecond) + + val ELEVATOR_KS = 0.0.volts + val ELEVATOR_KG = 0.0.volts + val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond + val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond + + val ENABLE_ELEVATOR = 1.0 + val ELEVATOR_IDLE_HEIGHT = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches + val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches + + val ELEVATOR_TOLERANCE = 0.0.inches + + val MAX_VELOCITY = 0.0.meters.perSecond + val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond + + val SHOOT_SPEAKER_POSITION = 0.0.inches + val SHOOT_AMP_POSITION = 0.0.inches + val SOURCE_NOTE_OFFSET = 0.0.inches + val ELEVATOR_THETA_POS = 0.0.degrees + val HOMING_STATOR_CURRENT = 0.0.amps + val HOMING_STALL_TIME_THRESHOLD = 0.0.seconds + val HOMING_APPLIED_VOLTAGE = 0.0.volts + val ELEVATOR_GROUND_OFFSET = 0.0.inches + + val VOLTAGE_COMPENSATION = 12.0.volts + val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 + val SENSOR_CPR = 0 + val SPOOL_DIAMETER = 1.5.inches + + val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + val LEADER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val LEADER_STATOR_CURRENT_LIMIT = 0.0.amps + + val FOLLOWER_SUPPLY_TIME_THRESHOLD = 0.0.seconds + val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps + val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps +} 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 870ddbdc..b94811e9 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 @@ -5,7 +5,6 @@ import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.DrivetrainConstants import com.team4099.robot2023.config.constants.VisionConstants import com.team4099.robot2023.subsystems.drivetrain.gyro.GyroIO -import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest import com.team4099.robot2023.util.Alert import com.team4099.robot2023.util.FMSData import com.team4099.robot2023.util.PoseEstimator @@ -46,610 +45,616 @@ import org.team4099.lib.units.derived.radians import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond import java.util.function.Supplier +import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest class Drivetrain(val gyroIO: GyroIO, swerveModuleIOs: DrivetrainIO) : SubsystemBase() { - val gyroNotConnectedAlert = - Alert( - "Gyro is not connected, field relative driving will be significantly worse.", - Alert.AlertType.ERROR - ) - - val gyroInputs = GyroIO.GyroIOInputs() - val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR - - var gyroYawOffset = 0.0.radians - - val closestAlignmentAngle: Angle - get() { - for (angle in -180..90 step 90) { - if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { - return angle.degrees - } - } - return 0.0.degrees + val gyroNotConnectedAlert = + Alert( + "Gyro is not connected, field relative driving will be significantly worse.", + Alert.AlertType.ERROR + ) + + val gyroInputs = GyroIO.GyroIOInputs() + val swerveModules = swerveModuleIOs.getSwerveModules() // FL, FR, BL, BR + + var gyroYawOffset = 0.0.radians + + val closestAlignmentAngle: Angle + get() { + for (angle in -180..90 step 90) { + if ((odometryPose.rotation - angle.degrees).absoluteValue <= 45.degrees) { + return angle.degrees } + } + return 0.0.degrees + } - var canMoveSafely = Supplier { false } - - var elevatorHeightSupplier = Supplier { 0.0.inches } - - init { - - // Wheel speeds - zeroSteering() + var canMoveSafely = Supplier { false } + + var elevatorHeightSupplier = Supplier { 0.0.inches } + + init { + + // Wheel speeds + zeroSteering() + } + + private val frontLeftWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val frontRightWheelLocation = + Translation2d( + DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backLeftWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + private val backRightWheelLocation = + Translation2d( + -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 + ) + + val moduleTranslations = + listOf( + frontLeftWheelLocation, + frontRightWheelLocation, + backLeftWheelLocation, + backRightWheelLocation + ) + + val swerveDriveKinematics = + SwerveDriveKinematics( + frontLeftWheelLocation.translation2d, + frontRightWheelLocation.translation2d, + backLeftWheelLocation.translation2d, + backRightWheelLocation.translation2d + ) + + var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + + var swerveDriveOdometry = + SwerveDriveOdometry( + swerveDriveKinematics, + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray() + ) + + var setPointStates = + mutableListOf( + SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() + ) + + var odometryPose: Pose2d + get() = swerveDrivePoseEstimator.getLatestPose() + set(value) { + swerveDrivePoseEstimator.resetPose(value) + + if (RobotBase.isReal()) {} else { + undriftedPose = odometryPose + swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 + } } - private val frontLeftWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val frontRightWheelLocation = - Translation2d( - DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backLeftWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) - private val backRightWheelLocation = - Translation2d( - -DrivetrainConstants.DRIVETRAIN_LENGTH / 2, -DrivetrainConstants.DRIVETRAIN_WIDTH / 2 - ) + var rawGyroAngle = odometryPose.rotation - val moduleTranslations = - listOf( - frontLeftWheelLocation, - frontRightWheelLocation, - backLeftWheelLocation, - backRightWheelLocation - ) + var undriftedPose: Pose2d + get() = Pose2d(swerveDriveOdometry.poseMeters) + set(value) { + swerveDriveOdometry.resetPosition( + gyroInputs.gyroYaw.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + value.pose2d + ) + } - val swerveDriveKinematics = - SwerveDriveKinematics( - frontLeftWheelLocation.translation2d, - frontRightWheelLocation.translation2d, - backLeftWheelLocation.translation2d, - backRightWheelLocation.translation2d - ) + var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) - var swerveDrivePoseEstimator = PoseEstimator(VecBuilder.fill(0.003, 0.003, 0.00001)) + var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) - var swerveDriveOdometry = - SwerveDriveOdometry( - swerveDriveKinematics, - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray() - ) + var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) - var setPointStates = - mutableListOf( - SwerveModuleState(), SwerveModuleState(), SwerveModuleState(), SwerveModuleState() - ) + var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) - var odometryPose: Pose2d - get() = swerveDrivePoseEstimator.getLatestPose() - set(value) { - swerveDrivePoseEstimator.resetPose(value) + var omegaVelocity = 0.0.radians.perSecond - if (RobotBase.isReal()) {} else { - undriftedPose = odometryPose - swerveModules.map { it.inputs.drift = 0.0.meters } // resetting drift to 0 - } - } + var lastModulePositions = + mutableListOf( + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition(), + SwerveModulePosition() + ) - var rawGyroAngle = odometryPose.rotation + var lastGyroYaw = 0.0.radians - var undriftedPose: Pose2d - get() = Pose2d(swerveDriveOdometry.poseMeters) - set(value) { - swerveDriveOdometry.resetPosition( - gyroInputs.gyroYaw.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - value.pose2d - ) - } + var velocityTarget = 0.degrees.perSecond - var targetPose: Pose2d = Pose2d(0.0.meters, 0.0.meters, 0.0.radians) + var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) - var drift: Transform2d = Transform2d(Translation2d(), 0.0.radians) + var fieldOrientation = true - var fieldVelocity = Velocity2d(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var robotVelocity = Pair(0.0.meters.perSecond, 0.0.meters.perSecond) + var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - var omegaVelocity = 0.0.radians.perSecond - - var lastModulePositions = - mutableListOf( - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition(), - SwerveModulePosition() - ) - - var lastGyroYaw = 0.0.radians - - var velocityTarget = 0.degrees.perSecond + var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED + var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() + set(value) { + when (value) { + is DrivetrainRequest.OpenLoop -> { + velocityTarget = value.angularVelocity + targetedDriveVector = value.driveVector + fieldOrientation = value.fieldOriented + } + is DrivetrainRequest.ClosedLoop -> { + targetedChassisSpeeds = value.chassisSpeeds + targetedChassisAccels = value.chassisAccels + } + else -> {} + } + field = value + } - var targetedDriveVector = Pair(0.meters.perSecond, 0.meters.perSecond) + init { + zeroSteering() + } - var fieldOrientation = true + override fun periodic() { + val startTime = Clock.realTimestamp + gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) + gyroIO.updateInputs(gyroInputs) - var targetedChassisSpeeds = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + swerveModules.forEach { it.periodic() } - var targetedChassisAccels = edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + // TODO: add logic to reduce setpoint based on elevator/arm position + DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond - var currentState: DrivetrainState = DrivetrainState.UNINITIALIZED - var currentRequest: DrivetrainRequest = DrivetrainRequest.ZeroSensors() - set(value) { - when (value) { - is DrivetrainRequest.OpenLoop -> { - velocityTarget = value.angularVelocity - targetedDriveVector = value.driveVector - fieldOrientation = value.fieldOriented - } - is DrivetrainRequest.ClosedLoop -> { - targetedChassisSpeeds = value.chassisSpeeds - targetedChassisAccels = value.chassisAccels - } - else -> {} - } - field = value - } + Logger.recordOutput( + "Drivetrain/maxSetpointMetersPerSecond", + DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - init { - zeroSteering() + // Update field velocity + val measuredStates = arrayOfNulls(4) + for (i in 0..3) { + measuredStates[i] = + SwerveModuleState( + swerveModules[i].inputs.driveVelocity.inMetersPerSecond, + swerveModules[i].inputs.steeringPosition.inRotation2ds + ) + } + val chassisState: ChassisSpeeds = + ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) + val fieldVelCalculated = + Translation2d( + chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters + ) + .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig + + robotVelocity = Pair(chassisState.vx, chassisState.vy) + fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) + + omegaVelocity = chassisState.omega + if (!gyroInputs.gyroConnected) { + gyroInputs.gyroYawRate = omegaVelocity + rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate + gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset } - override fun periodic() { - val startTime = Clock.realTimestamp - gyroNotConnectedAlert.set(!gyroInputs.gyroConnected) - gyroIO.updateInputs(gyroInputs) + // updating odometry every loop cycle + updateOdometry() + + Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) + Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) + + Logger.processInputs("Drivetrain/Gyro", gyroInputs) + Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) + Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) + + Logger.recordOutput( + VisionConstants.POSE_TOPIC_NAME, + doubleArrayOf( + odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians + ) + ) + Logger.recordOutput( + "Odometry/pose3d", + Pose3d( + odometryPose.x, + odometryPose.y, + 1.0.meters, + Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) + ) + .pose3d + ) + Logger.recordOutput( + "Odometry/targetPose", + doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + ) + + Logger.recordOutput( + "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", + (Clock.realTimestamp - startTime).inMilliseconds + ) + + var nextState = currentState + + when (currentState) { + DrivetrainState.UNINITIALIZED -> { + // Transitions + nextState = DrivetrainState.ZEROING_SENSORS + } + DrivetrainState.ZEROING_SENSORS -> { + zeroSensors() + + // Transitions + currentRequest = DrivetrainRequest.Idle() + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.OPEN_LOOP -> { + // Outputs + setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.CLOSED_LOOP -> { + // Outputs + setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) + + // Transitions + nextState = fromRequestToState(currentRequest) + } + DrivetrainState.IDLE -> { + nextState = fromRequestToState(currentRequest) + } + } - swerveModules.forEach { it.periodic() } + currentState = nextState + + // Log the current state + Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + } + + 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 + ) + } - // TODO: add logic to reduce setpoint based on elevator/arm position - DrivetrainConstants.DRIVE_SETPOINT_MAX = 15.feet.perSecond + var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - Logger.recordOutput( - "Drivetrain/maxSetpointMetersPerSecond", - DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + if (gyroInputs.gyroConnected) { + driveTwist = + edu.wpi.first.math.geometry.Twist2d( + driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians ) + lastGyroYaw = gyroInputs.rawGyroYaw + } - // Update field velocity - val measuredStates = arrayOfNulls(4) - for (i in 0..3) { - measuredStates[i] = - SwerveModuleState( - swerveModules[i].inputs.driveVelocity.inMetersPerSecond, - swerveModules[i].inputs.steeringPosition.inRotation2ds - ) - } - val chassisState: ChassisSpeeds = - ChassisSpeeds(swerveDriveKinematics.toChassisSpeeds(*measuredStates)) - val fieldVelCalculated = - Translation2d( - chassisState.vx.inMetersPerSecond.meters, chassisState.vy.inMetersPerSecond.meters - ) - .rotateBy(odometryPose.rotation) // we don't use this but it's there if you want it ig - - robotVelocity = Pair(chassisState.vx, chassisState.vy) - fieldVelocity = Velocity2d(fieldVelCalculated.x.perSecond, fieldVelCalculated.y.perSecond) - - omegaVelocity = chassisState.omega - if (!gyroInputs.gyroConnected) { - gyroInputs.gyroYawRate = omegaVelocity - rawGyroAngle += Constants.Universal.LOOP_PERIOD_TIME * gyroInputs.gyroYawRate - gyroInputs.gyroYaw = rawGyroAngle + gyroYawOffset - } + // reversing the drift to store the ground truth pose + if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { + val undriftedModules = arrayOfNulls(4) + for (i in 0 until 4) { + undriftedModules[i] = + SwerveModulePosition( + ( + swerveModules[i].modulePosition.distanceMeters.meters - + swerveModules[i].inputs.drift + ) + .inMeters, + swerveModules[i].modulePosition.angle + ) + } + swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) + + drift = undriftedPose.minus(odometryPose) + + Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) + } - // updating odometry every loop cycle - updateOdometry() - - Logger.recordOutput("Drivetrain/xVelocityMetersPerSecond", fieldVelocity.x.inMetersPerSecond) - Logger.recordOutput("Drivetrain/yVelocityMetersPerSecond", fieldVelocity.y.inMetersPerSecond) - - Logger.processInputs("Drivetrain/Gyro", gyroInputs) - Logger.recordOutput("Drivetrain/ModuleStates", *measuredStates) - Logger.recordOutput("Drivetrain/SetPointStates", *setPointStates.toTypedArray()) - - Logger.recordOutput(VisionConstants.POSE_TOPIC_NAME, doubleArrayOf(odometryPose.x.inMeters, odometryPose.y.inMeters, odometryPose.rotation.inRadians)) - Logger.recordOutput( - "Odometry/pose3d", - Pose3d( - odometryPose.x, - odometryPose.y, - 1.0.meters, - Rotation3d(gyroInputs.gyroRoll, gyroInputs.gyroPitch, gyroInputs.gyroYaw) - ) - .pose3d + swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + } + + /** + * @param angularVelocity Represents the angular velocity of the chassis + * @param driveVector Pair of linear velocities: First is X vel, second is Y vel + * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) + */ + fun setOpenLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + fieldOriented: Boolean = true + ) { + val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 + val allianceFlippedDriveVector = + Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) + + val swerveModuleStates: Array + var desiredChassisSpeeds: ChassisSpeeds + + if (fieldOriented) { + Logger.recordOutput("drivetrain/isFieldOriented", true) + desiredChassisSpeeds = + ChassisSpeeds.fromFieldRelativeSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, + odometryPose.rotation ) - Logger.recordOutput( - "Odometry/targetPose", - doubleArrayOf(targetPose.x.inMeters, targetPose.y.inMeters, targetPose.rotation.inRadians) + } else { + desiredChassisSpeeds = + ChassisSpeeds( + allianceFlippedDriveVector.first, + allianceFlippedDriveVector.second, + angularVelocity, ) + } - Logger.recordOutput( - "LoggedRobot/Subsystems/DrivetrainLoopTimeMS", - (Clock.realTimestamp - startTime).inMilliseconds + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy + ), + Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega ) - var nextState = currentState - - when (currentState) { - DrivetrainState.UNINITIALIZED -> { - // Transitions - nextState = DrivetrainState.ZEROING_SENSORS - } - DrivetrainState.ZEROING_SENSORS -> { - zeroSensors() - - // Transitions - currentRequest = DrivetrainRequest.Idle() - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.OPEN_LOOP -> { - // Outputs - setOpenLoop(velocityTarget, targetedDriveVector, fieldOrientation) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.CLOSED_LOOP -> { - // Outputs - setClosedLoop(targetedChassisSpeeds, targetedChassisAccels) - - // Transitions - nextState = fromRequestToState(currentRequest) - } - DrivetrainState.IDLE -> { - nextState = fromRequestToState(currentRequest) - } - } + val twistToNextPose: Twist2d = velocityTransform.log() - currentState = nextState - - // Log the current state - Logger.recordOutput("Drivetrain/requestedState", currentState.toString()) + desiredChassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) } - 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 - ) - } + swerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - var driveTwist = swerveDriveKinematics.toTwist2d(*wheelDeltas.toTypedArray()) - - if (gyroInputs.gyroConnected) { - driveTwist = - edu.wpi.first.math.geometry.Twist2d( - driveTwist.dx, driveTwist.dy, (gyroInputs.rawGyroYaw - lastGyroYaw).inRadians - ) - lastGyroYaw = gyroInputs.rawGyroYaw - } + SwerveDriveKinematics.desaturateWheelSpeeds( + swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + ) - // reversing the drift to store the ground truth pose - if (RobotBase.isSimulation() && Constants.Tuning.SIMULATE_DRIFT) { - val undriftedModules = arrayOfNulls(4) - for (i in 0 until 4) { - undriftedModules[i] = - SwerveModulePosition( - ( - swerveModules[i].modulePosition.distanceMeters.meters - - swerveModules[i].inputs.drift - ) - .inMeters, - swerveModules[i].modulePosition.angle - ) - } - swerveDriveOdometry.update(gyroInputs.gyroYaw.inRotation2ds, undriftedModules) - - drift = undriftedPose.minus(odometryPose) - - Logger.recordOutput(VisionConstants.SIM_POSE_TOPIC_NAME, undriftedPose.pose2d) - } + setPointStates = swerveModuleStates.toMutableList() - swerveDrivePoseEstimator.addDriveData(Clock.fpgaTime.inSeconds, Twist2d(driveTwist)) + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) } - - /** - * @param angularVelocity Represents the angular velocity of the chassis - * @param driveVector Pair of linear velocities: First is X vel, second is Y vel - * @param fieldOriented Are the chassis speeds driving relative to field (aka use gyro or not) - */ - fun setOpenLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - fieldOriented: Boolean = true - ) { - val flipDrive = if (FMSData.allianceColor == DriverStation.Alliance.Red) -1 else 1 - val allianceFlippedDriveVector = - Pair(driveVector.first * flipDrive, driveVector.second * flipDrive) - - val swerveModuleStates: Array - var desiredChassisSpeeds: ChassisSpeeds - - if (fieldOriented) { - Logger.recordOutput("drivetrain/isFieldOriented", true) - desiredChassisSpeeds = - ChassisSpeeds.fromFieldRelativeSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - odometryPose.rotation - ) - } else { - desiredChassisSpeeds = - ChassisSpeeds( - allianceFlippedDriveVector.first, - allianceFlippedDriveVector.second, - angularVelocity, - ) - } - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vx, - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.vy - ), - Constants.Universal.LOOP_PERIOD_TIME * desiredChassisSpeeds.omega - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - desiredChassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - } - - swerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates(desiredChassisSpeeds.chassisSpeedsWPILIB) - - SwerveDriveKinematics.desaturateWheelSpeeds( - swerveModuleStates, DrivetrainConstants.DRIVE_SETPOINT_MAX.inMetersPerSecond + } + + /** + * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular + * and linear acceleration. Calculates both angular and linear velocities and acceleration and + * calls setPositionClosedLoop for each SwerveModule object. + * + * @param angularVelocity The angular velocity of a specified drive + * @param driveVector.first The linear velocity on the X axis + * @param driveVector.second The linear velocity on the Y axis + * @param angularAcceleration The angular acceleration of a specified drive + * @param driveAcceleration.first The linear acceleration on the X axis + * @param driveAcceleration.second The linear acceleration on the Y axis + * @param fieldOriented Defines whether module states are calculated relative to field + */ + fun setClosedLoop( + angularVelocity: AngularVelocity, + driveVector: Pair, + angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, + driveAcceleration: Pair = + Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), + fieldOriented: Boolean = true, + ) { + val velSwerveModuleStates: Array? + val accelSwerveModuleStates: Array? + + if (fieldOriented) { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + // This is with respect to the field, meaning all velocity and acceleration vectors are + // adjusted to be relative to the field instead of relative to the robot. + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds.fromFieldRelativeSpeeds( + driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation + ) + .chassisSpeedsWPILIB ) - setPointStates = swerveModuleStates.toMutableList() - - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionOpenLoop(swerveModuleStates[moduleIndex]) - } - } - - /** - * Sets the drivetrain to the specified angular and X & Y velocities based on the current angular - * and linear acceleration. Calculates both angular and linear velocities and acceleration and - * calls setPositionClosedLoop for each SwerveModule object. - * - * @param angularVelocity The angular velocity of a specified drive - * @param driveVector.first The linear velocity on the X axis - * @param driveVector.second The linear velocity on the Y axis - * @param angularAcceleration The angular acceleration of a specified drive - * @param driveAcceleration.first The linear acceleration on the X axis - * @param driveAcceleration.second The linear acceleration on the Y axis - * @param fieldOriented Defines whether module states are calculated relative to field - */ - fun setClosedLoop( - angularVelocity: AngularVelocity, - driveVector: Pair, - angularAcceleration: AngularAcceleration = 0.0.radians.perSecond.perSecond, - driveAcceleration: Pair = - Pair(0.0.meters.perSecond.perSecond, 0.0.meters.perSecond.perSecond), - fieldOriented: Boolean = true, - ) { - val velSwerveModuleStates: Array? - val accelSwerveModuleStates: Array? - - if (fieldOriented) { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - // This is with respect to the field, meaning all velocity and acceleration vectors are - // adjusted to be relative to the field instead of relative to the robot. - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds.fromFieldRelativeSpeeds( - driveVector.first, driveVector.second, angularVelocity, odometryPose.rotation - ) - .chassisSpeedsWPILIB - ) - - // Although this isn't perfect, calculating acceleration states using the same math as - // velocity can get us "good enough" accel states to minimize skew - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels.fromFieldRelativeAccels( - driveAcceleration.first, - driveAcceleration.second, - angularAcceleration, - odometryPose.rotation - ) - .chassisAccelsWPILIB - ) - } else { - // Getting velocity and acceleration states from the drive & angular velocity vectors and - // drive & angular acceleration vectors (respectively) - velSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) - .chassisSpeedsWPILIB - ) - accelSwerveModuleStates = - swerveDriveKinematics.toSwerveModuleStates( - ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) - .chassisAccelsWPILIB - ) - } - - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + // Although this isn't perfect, calculating acceleration states using the same math as + // velocity can get us "good enough" accel states to minimize skew + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels.fromFieldRelativeAccels( + driveAcceleration.first, + driveAcceleration.second, + angularAcceleration, + odometryPose.rotation + ) + .chassisAccelsWPILIB + ) + } else { + // Getting velocity and acceleration states from the drive & angular velocity vectors and + // drive & angular acceleration vectors (respectively) + velSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisSpeeds(driveVector.first, driveVector.second, angularVelocity) + .chassisSpeedsWPILIB + ) + accelSwerveModuleStates = + swerveDriveKinematics.toSwerveModuleStates( + ChassisAccels(driveAcceleration.first, driveAcceleration.second, angularAcceleration) + .chassisAccelsWPILIB ) - - setPointStates = velSwerveModuleStates.toMutableList() - - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } } - fun setClosedLoop( - chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, - chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = - edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) - ) { - var chassisSpeeds = chassisSpeeds - - if (DrivetrainConstants.MINIMIZE_SKEW) { - val velocityTransform = - Transform2d( - Translation2d( - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vxMetersPerSecond.meters.perSecond, - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.vyMetersPerSecond.meters.perSecond - ), - Constants.Universal.LOOP_PERIOD_TIME * - chassisSpeeds.omegaRadiansPerSecond.radians.perSecond - ) - - val twistToNextPose: Twist2d = velocityTransform.log() - - chassisSpeeds = - ChassisSpeeds( - (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), - (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) - ) - .chassisSpeedsWPILIB - } - - val velSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) - val accelSwerveModuleStates: Array = - swerveDriveKinematics.toSwerveModuleStates(chassisAccels) + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - SwerveDriveKinematics.desaturateWheelSpeeds( - velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond - ) - - setPointStates = velSwerveModuleStates.toMutableList() + setPointStates = velSwerveModuleStates.toMutableList() - // Once we have all of our states obtained for both velocity and acceleration, apply these - // states to each swerve module - for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { - swerveModules[moduleIndex].setPositionClosedLoop( - velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] - ) - } + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } + } + + fun setClosedLoop( + chassisSpeeds: edu.wpi.first.math.kinematics.ChassisSpeeds, + chassisAccels: edu.wpi.first.math.kinematics.ChassisSpeeds = + edu.wpi.first.math.kinematics.ChassisSpeeds(0.0, 0.0, 0.0) + ) { + var chassisSpeeds = chassisSpeeds + + if (DrivetrainConstants.MINIMIZE_SKEW) { + val velocityTransform = + Transform2d( + Translation2d( + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vxMetersPerSecond.meters.perSecond, + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.vyMetersPerSecond.meters.perSecond + ), + Constants.Universal.LOOP_PERIOD_TIME * + chassisSpeeds.omegaRadiansPerSecond.radians.perSecond + ) - fun resetModuleZero() { - swerveModules.forEach { it.resetModuleZero() } - } + val twistToNextPose: Twist2d = velocityTransform.log() - /** Zeros all the sensors on the drivetrain. */ - fun zeroSensors() { - zeroGyroPitch(0.0.degrees) - zeroGyroRoll() - zeroSteering() - zeroDrive() + chassisSpeeds = + ChassisSpeeds( + (twistToNextPose.dx / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dy / Constants.Universal.LOOP_PERIOD_TIME), + (twistToNextPose.dtheta / Constants.Universal.LOOP_PERIOD_TIME) + ) + .chassisSpeedsWPILIB } - /** - * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. - * - * @param toAngle Zeros the gyro to the value - */ - fun zeroGyroYaw(toAngle: Angle = 0.degrees) { - gyroIO.zeroGyroYaw(toAngle) - - swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) - - if (RobotBase.isSimulation()) { - swerveDriveOdometry.resetPosition( - toAngle.inRotation2ds, - swerveModules.map { it.modulePosition }.toTypedArray(), - undriftedPose.pose2d - ) - } + val velSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisSpeeds) + val accelSwerveModuleStates: Array = + swerveDriveKinematics.toSwerveModuleStates(chassisAccels) - if (!(gyroInputs.gyroConnected)) { - gyroYawOffset = toAngle - rawGyroAngle - } - } + SwerveDriveKinematics.desaturateWheelSpeeds( + velSwerveModuleStates, DrivetrainConstants.MAX_AUTO_VEL.inMetersPerSecond + ) - fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroPitch(toAngle) - } + setPointStates = velSwerveModuleStates.toMutableList() - fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { - gyroIO.zeroGyroRoll(toAngle) + // Once we have all of our states obtained for both velocity and acceleration, apply these + // states to each swerve module + for (moduleIndex in 0 until DrivetrainConstants.WHEEL_COUNT) { + swerveModules[moduleIndex].setPositionClosedLoop( + velSwerveModuleStates[moduleIndex], accelSwerveModuleStates[moduleIndex] + ) } - - /** Zeros the steering motors for each swerve module. */ - fun zeroSteering() { - swerveModules.forEach { it.zeroSteering() } + } + + fun resetModuleZero() { + swerveModules.forEach { it.resetModuleZero() } + } + + /** Zeros all the sensors on the drivetrain. */ + fun zeroSensors() { + zeroGyroPitch(0.0.degrees) + zeroGyroRoll() + zeroSteering() + zeroDrive() + } + + /** + * Sets the gyroOffset in such a way that when added to the gyro angle it gives back toAngle. + * + * @param toAngle Zeros the gyro to the value + */ + fun zeroGyroYaw(toAngle: Angle = 0.degrees) { + gyroIO.zeroGyroYaw(toAngle) + + swerveDrivePoseEstimator.resetPose(Pose2d(odometryPose.x, odometryPose.y, gyroInputs.gyroYaw)) + + if (RobotBase.isSimulation()) { + swerveDriveOdometry.resetPosition( + toAngle.inRotation2ds, + swerveModules.map { it.modulePosition }.toTypedArray(), + undriftedPose.pose2d + ) } - /** Zeros the drive motors for each swerve module. */ - private fun zeroDrive() { - swerveModules.forEach { it.zeroDrive() } + if (!(gyroInputs.gyroConnected)) { + gyroYawOffset = toAngle - rawGyroAngle } - - fun addVisionData(visionData: List) { - swerveDrivePoseEstimator.addVisionData(visionData) + } + + fun zeroGyroPitch(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroPitch(toAngle) + } + + fun zeroGyroRoll(toAngle: Angle = 0.0.degrees) { + gyroIO.zeroGyroRoll(toAngle) + } + + /** Zeros the steering motors for each swerve module. */ + fun zeroSteering() { + swerveModules.forEach { it.zeroSteering() } + } + + /** Zeros the drive motors for each swerve module. */ + private fun zeroDrive() { + swerveModules.forEach { it.zeroDrive() } + } + + fun addVisionData(visionData: List) { + swerveDrivePoseEstimator.addVisionData(visionData) + } + + companion object { + enum class DrivetrainState { + UNINITIALIZED, + IDLE, + ZEROING_SENSORS, + OPEN_LOOP, + CLOSED_LOOP; + + inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { + return ( + (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || + (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || + (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || + (request is DrivetrainRequest.Idle && this == IDLE) + ) + } } - companion object { - enum class DrivetrainState { - UNINITIALIZED, - IDLE, - ZEROING_SENSORS, - OPEN_LOOP, - CLOSED_LOOP; - - inline fun equivalentToRequest(request: DrivetrainRequest): Boolean { - return ( - (request is DrivetrainRequest.ZeroSensors && this == ZEROING_SENSORS) || - (request is DrivetrainRequest.OpenLoop && this == OPEN_LOOP) || - (request is DrivetrainRequest.ClosedLoop && this == CLOSED_LOOP) || - (request is DrivetrainRequest.Idle && this == IDLE) - ) - } - } - - inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { - return when (request) { - is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP - is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP - is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS - is DrivetrainRequest.Idle -> DrivetrainState.IDLE - } - } + inline fun fromRequestToState(request: DrivetrainRequest): DrivetrainState { + return when (request) { + is DrivetrainRequest.OpenLoop -> DrivetrainState.OPEN_LOOP + is DrivetrainRequest.ClosedLoop -> DrivetrainState.CLOSED_LOOP + is DrivetrainRequest.ZeroSensors -> DrivetrainState.ZEROING_SENSORS + is DrivetrainRequest.Idle -> DrivetrainState.IDLE + } } -} \ No newline at end of file + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt index 8ffd9987..419b53e4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIO.kt @@ -4,24 +4,24 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIO interface DrivetrainIO { - fun getSwerveModules(): List { - return listOf( - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Front Right Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Left Wheel" - }), - SwerveModule( - object : SwerveModuleIO { - override val label = "Back Right Wheel" - }) - ) - } -} \ No newline at end of file + fun getSwerveModules(): List { + return listOf( + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Front Right Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Left Wheel" + }), + SwerveModule( + object : SwerveModuleIO { + override val label = "Back Right Wheel" + }) + ) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt index 25e40a71..b77281b3 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/drive/DrivetrainIOSim.kt @@ -4,12 +4,12 @@ import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModule import com.team4099.robot2023.subsystems.drivetrain.swervemodule.SwerveModuleIOSim object DrivetrainIOSim : DrivetrainIO { - override fun getSwerveModules(): List { - return listOf( - SwerveModule(SwerveModuleIOSim("Front Left Wheel")), - SwerveModule(SwerveModuleIOSim("Front Right Wheel")), - SwerveModule(SwerveModuleIOSim("Back Left Wheel")), - SwerveModule(SwerveModuleIOSim("Back Right Wheel")) - ) - } -} \ No newline at end of file + override fun getSwerveModules(): List { + return listOf( + SwerveModule(SwerveModuleIOSim("Front Left Wheel")), + SwerveModule(SwerveModuleIOSim("Front Right Wheel")), + SwerveModule(SwerveModuleIOSim("Back Left Wheel")), + SwerveModule(SwerveModuleIOSim("Back Right Wheel")) + ) + } +} 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 481e3875..89cbd4d9 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 @@ -10,52 +10,52 @@ import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond interface GyroIO { - class GyroIOInputs : LoggableInputs { - var rawGyroYaw = 0.0.radians - var gyroYaw = 0.0.radians - var gyroPitch = -3.degrees - var gyroRoll = 0.0.radians - var gyroYawRate = 0.0.radians.perSecond - var gyroPitchRate = 0.0.radians.perSecond - var gyroRollRate = 0.0.radians.perSecond - - var odometryYawPositions = arrayOf() - - var gyroConnected = false - - override fun toLog(table: LogTable?) { - table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) - table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) - table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) - table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) - table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) - table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) - table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) - table?.put("gyroConnected", gyroConnected) - } - - override fun fromLog(table: LogTable?) { - table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } - table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } - table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } - table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } - table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { - gyroYawRate = it.degrees.perSecond - } - table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { - gyroPitchRate = it.degrees.perSecond - } - table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { - gyroRollRate = it.degrees.perSecond - } - table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } - } + class GyroIOInputs : LoggableInputs { + var rawGyroYaw = 0.0.radians + var gyroYaw = 0.0.radians + var gyroPitch = -3.degrees + var gyroRoll = 0.0.radians + var gyroYawRate = 0.0.radians.perSecond + var gyroPitchRate = 0.0.radians.perSecond + var gyroRollRate = 0.0.radians.perSecond + + var odometryYawPositions = arrayOf() + + var gyroConnected = false + + override fun toLog(table: LogTable?) { + table?.put("rawGyroYawDegrees", rawGyroYaw.inDegrees) + table?.put("gyroYawAngleDegrees", gyroYaw.inDegrees) + table?.put("gyroPitchAngleDegrees", gyroPitch.inDegrees) + table?.put("gyroRollAngleDegrees", gyroRoll.inDegrees) + table?.put("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond) + table?.put("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond) + table?.put("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond) + table?.put("gyroConnected", gyroConnected) } - fun updateInputs(inputs: GyroIOInputs) {} - fun zeroGyroYaw(toAngle: Angle) {} + override fun fromLog(table: LogTable?) { + table?.get("rawGyroYawDegrees", rawGyroYaw.inDegrees)?.let { rawGyroYaw = it.degrees } + table?.get("gyroYawAngleDegrees", gyroYaw.inDegrees)?.let { gyroYaw = it.degrees } + table?.get("gyroPitchDegrees", gyroPitch.inDegrees)?.let { gyroPitch = it.degrees } + table?.get("gyroRollDegrees", gyroRoll.inDegrees)?.let { gyroRoll = it.degrees } + table?.get("gyroYawRateDegreesPerSecond", gyroYawRate.inDegreesPerSecond)?.let { + gyroYawRate = it.degrees.perSecond + } + table?.get("gyroPitchRateDegreesPerSecond", gyroPitchRate.inDegreesPerSecond)?.let { + gyroPitchRate = it.degrees.perSecond + } + table?.get("gyroRollRateDegreesPerSecond", gyroRollRate.inDegreesPerSecond)?.let { + gyroRollRate = it.degrees.perSecond + } + table?.getBoolean("gyroConnected", gyroConnected)?.let { gyroConnected = it } + } + } + fun updateInputs(inputs: GyroIOInputs) {} + + fun zeroGyroYaw(toAngle: Angle) {} - fun zeroGyroPitch(toAngle: Angle) {} + fun zeroGyroPitch(toAngle: Angle) {} - fun zeroGyroRoll(toAngle: Angle) {} -} \ No newline at end of file + fun zeroGyroRoll(toAngle: Angle) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt index c08c5395..06fd482b 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/gyro/GyroIONavx.kt @@ -11,77 +11,77 @@ import org.team4099.lib.units.perSecond import kotlin.math.IEEErem object GyroIONavx : GyroIO { - private val gyro = AHRS(SerialPort.Port.kMXP) + private val gyro = AHRS(SerialPort.Port.kMXP) - init { - gyro.zeroYaw() - } + init { + gyro.zeroYaw() + } - var gyroYawOffset: Angle = 0.0.degrees - var gyroPitchOffset: Angle = 0.0.degrees - var gyroRollOffset: Angle = 0.0.degrees + var gyroYawOffset: Angle = 0.0.degrees + var gyroPitchOffset: Angle = 0.0.degrees + var gyroRollOffset: Angle = 0.0.degrees - /** The current angle of the drivetrain. */ - val gyroYaw: Angle - get() { - if (gyro.isConnected) { - var rawYaw = gyro.angle + gyroYawOffset.inDegrees - rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate - return rawYaw.IEEErem(360.0).degrees - } else { - return (-1.337).degrees - } - } + /** The current angle of the drivetrain. */ + val gyroYaw: Angle + get() { + if (gyro.isConnected) { + var rawYaw = gyro.angle + gyroYawOffset.inDegrees + rawYaw += DrivetrainConstants.GYRO_RATE_COEFFICIENT * gyro.rate + return rawYaw.IEEErem(360.0).degrees + } else { + return (-1.337).degrees + } + } - val gyroPitch: Angle - get() { - if (gyro.isConnected) { - val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees - return rawPitch.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroPitch: Angle + get() { + if (gyro.isConnected) { + val rawPitch = gyro.pitch + gyroPitchOffset.inDegrees + return rawPitch.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroRoll: Angle - get() { - if (gyro.isConnected) { - val rawRoll = gyro.roll + gyroRollOffset.inDegrees - return rawRoll.IEEErem(360.0).degrees - } else { - return -1.337.degrees - } - } + val gyroRoll: Angle + get() { + if (gyro.isConnected) { + val rawRoll = gyro.roll + gyroRollOffset.inDegrees + return rawRoll.IEEErem(360.0).degrees + } else { + return -1.337.degrees + } + } - val gyroYawRate: AngularVelocity - get() { - if (gyro.isConnected) { - return gyro.rate.degrees.perSecond - } else { - return -1.337.degrees.perSecond - } - } + val gyroYawRate: AngularVelocity + get() { + if (gyro.isConnected) { + return gyro.rate.degrees.perSecond + } else { + return -1.337.degrees.perSecond + } + } - override fun updateInputs(inputs: GyroIO.GyroIOInputs) { - inputs.rawGyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyro.angle.degrees - inputs.gyroYaw = gyroYaw - inputs.gyroYawRate = gyroYawRate - inputs.gyroPitch = gyroPitch - inputs.gyroRoll = gyroRoll + override fun updateInputs(inputs: GyroIO.GyroIOInputs) { + inputs.rawGyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyro.angle.degrees + inputs.gyroYaw = gyroYaw + inputs.gyroYawRate = gyroYawRate + inputs.gyroPitch = gyroPitch + inputs.gyroRoll = gyroRoll - inputs.gyroConnected = gyro.isConnected - } + inputs.gyroConnected = gyro.isConnected + } - override fun zeroGyroYaw(toAngle: Angle) { - gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees - } + override fun zeroGyroYaw(toAngle: Angle) { + gyroYawOffset = (toAngle.inDegrees - gyro.angle).degrees + } - override fun zeroGyroPitch(toAngle: Angle) { - gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees - } + override fun zeroGyroPitch(toAngle: Angle) { + gyroPitchOffset = (toAngle.inDegrees - gyro.pitch).degrees + } - override fun zeroGyroRoll(toAngle: Angle) { - gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees - } -} \ No newline at end of file + override fun zeroGyroRoll(toAngle: Angle) { + gyroRollOffset = (toAngle.inDegrees - gyro.roll).degrees + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt index 77060f32..f43e426f 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModule.kt @@ -12,265 +12,282 @@ import org.team4099.lib.units.base.feet import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.meters import org.team4099.lib.units.base.seconds -import org.team4099.lib.units.derived.* +import org.team4099.lib.units.derived.Angle +import org.team4099.lib.units.derived.angle +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inRadians +import org.team4099.lib.units.derived.inRotation2ds +import org.team4099.lib.units.derived.inVoltsPerDegree +import org.team4099.lib.units.derived.inVoltsPerDegreePerSecond +import org.team4099.lib.units.derived.inVoltsPerDegreeSeconds +import org.team4099.lib.units.derived.inVoltsPerMeters +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecond +import org.team4099.lib.units.derived.inVoltsPerMetersPerSecondPerSecond +import org.team4099.lib.units.derived.perDegree +import org.team4099.lib.units.derived.perDegreePerSecond +import org.team4099.lib.units.derived.perDegreeSeconds +import org.team4099.lib.units.derived.perMeterPerSecond +import org.team4099.lib.units.derived.perMeterPerSecondPerSecond +import org.team4099.lib.units.derived.radians +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perSecond import kotlin.math.IEEErem import kotlin.math.withSign class SwerveModule(val io: SwerveModuleIO) { - val inputs = SwerveModuleIO.SwerveModuleIOInputs() - - var modulePosition = SwerveModulePosition() - - private var speedSetPoint: LinearVelocity = 0.feet.perSecond - private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond - - private var steeringSetPoint: Angle = 0.degrees - - private var shouldInvert = false - - private val steeringkP = - LoggedTunableValue( - "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) - ) - private val steeringkI = - LoggedTunableValue( - "Drivetrain/moduleSteeringkI", - Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) - ) - private val steeringkD = - LoggedTunableValue( - "Drivetrain/moduleSteeringkD", - Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) - ) - - private val steeringMaxVel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX - ) - private val steeringMaxAccel = - LoggedTunableValue( - "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX - ) - - private val drivekP = - LoggedTunableValue( - "Drivetrain/moduleDrivekP", - Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) - ) - - private val drivekI = - LoggedTunableValue( - "Drivetrain/moduleDrivekI", - Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) - ) - - private val drivekD = - LoggedTunableValue( - "Drivetrain/moduleDrivekD", - Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) - ) - - init { - if (isReal()) { - steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) - } else { - steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) - steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) - steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) - - drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) - drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) - drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) - } + val inputs = SwerveModuleIO.SwerveModuleIOInputs() + + var modulePosition = SwerveModulePosition() + + private var speedSetPoint: LinearVelocity = 0.feet.perSecond + private var accelerationSetPoint: LinearAcceleration = 0.feet.perSecond.perSecond + + private var steeringSetPoint: Angle = 0.degrees + + private var shouldInvert = false + + private val steeringkP = + LoggedTunableValue( + "Drivetrain/moduleSteeringkP", Pair({ it.inVoltsPerDegree }, { it.volts.perDegree }) + ) + private val steeringkI = + LoggedTunableValue( + "Drivetrain/moduleSteeringkI", + Pair({ it.inVoltsPerDegreeSeconds }, { it.volts.perDegreeSeconds }) + ) + private val steeringkD = + LoggedTunableValue( + "Drivetrain/moduleSteeringkD", + Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + ) + + private val steeringMaxVel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxVelRadPerSec", DrivetrainConstants.STEERING_VEL_MAX + ) + private val steeringMaxAccel = + LoggedTunableValue( + "Drivetrain/moduleSteeringMaxAccelRadPerSecSq", DrivetrainConstants.STEERING_ACCEL_MAX + ) + + private val drivekP = + LoggedTunableValue( + "Drivetrain/moduleDrivekP", + Pair({ it.inVoltsPerMetersPerSecond }, { it.volts.perMeterPerSecond }) + ) + + private val drivekI = + LoggedTunableValue( + "Drivetrain/moduleDrivekI", + Pair({ it.inVoltsPerMeters }, { it.volts / (1.meters.perSecond * 1.seconds) }) + ) + + private val drivekD = + LoggedTunableValue( + "Drivetrain/moduleDrivekD", + Pair({ it.inVoltsPerMetersPerSecondPerSecond }, { it.volts.perMeterPerSecondPerSecond }) + ) + + init { + if (isReal()) { + steeringkP.initDefault(DrivetrainConstants.PID.STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.DRIVE_KD) + } else { + steeringkP.initDefault(DrivetrainConstants.PID.SIM_STEERING_KP) + steeringkI.initDefault(DrivetrainConstants.PID.SIM_STEERING_KI) + steeringkD.initDefault(DrivetrainConstants.PID.SIM_STEERING_KD) + + drivekP.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KP) + drivekI.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KI) + drivekD.initDefault(DrivetrainConstants.PID.SIM_DRIVE_KD) } + } - fun periodic() { - io.updateInputs(inputs) - - // Updating SwerveModulePosition every loop cycle - modulePosition.distanceMeters = inputs.drivePosition.inMeters - modulePosition.angle = inputs.steeringPosition.inRotation2ds - - if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { - io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) - } - - if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { - io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) - } - - if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { - io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) - } - - Logger.processInputs(io.label, inputs) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveSpeedSetpointMetersPerSecond", - // if (!shouldInvert) speedSetPoint.inMetersPerSecond - // else -speedSetPoint.inMetersPerSecond - // ) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/driveAccelSetpointMetersPerSecondSq", - // accelerationSetPoint.inMetersPerSecondPerSecond - // ) - // Logger.getInstance() - // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) - // Logger.getInstance() - // .recordOutput( - // "${io.label}/steeringValueDegreesWithMod", - // inputs.steeringPosition.inDegrees.IEEErem(360.0) - // ) - } + fun periodic() { + io.updateInputs(inputs) - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param steering The angular position desired for the swerve module to be set to - * @param speed The speed desired for the swerve module to reach - * @param acceleration The linear acceleration used to calculate how to reach the desired speed - */ - fun set( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, - optimize: Boolean = true - ) { - if (speed == 0.feet.perSecond) { - io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) - return - } - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - speedSetPoint = - if (shouldInvert) { - speed * -1 - } else { - speed - } - accelerationSetPoint = - if (shouldInvert) { - acceleration * -1 - } else { - acceleration - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - - // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) - } + // Updating SwerveModulePosition every loop cycle + modulePosition.distanceMeters = inputs.drivePosition.inMeters + modulePosition.angle = inputs.steeringPosition.inRotation2ds - fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { - var steeringDifference = - (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - - shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize - if (shouldInvert) { - steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians - } - - val outputSpeed = - if (shouldInvert) { - speed * -1 - } else { - speed - } - steeringSetPoint = inputs.steeringPosition + steeringDifference - io.setOpenLoop(steeringSetPoint, outputSpeed) + if (steeringkP.hasChanged() || steeringkI.hasChanged() || steeringkD.hasChanged()) { + io.configureSteeringPID(steeringkP.get(), steeringkI.get(), steeringkD.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using open loop control. - * - * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - */ - fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { - if (optimize) { - val optimizedState = - SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) - io.setOpenLoop( - optimizedState.angle.angle, - optimizedState - .speedMetersPerSecond - .meters - .perSecond // consider desaturating wheel speeds here if it doesn't work - // from drivetrain - ) - Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) - } else { - io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) - Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) - } + if (steeringMaxVel.hasChanged() || steeringMaxAccel.hasChanged()) { + io.configureSteeringMotionMagic(steeringMaxVel.get(), steeringMaxAccel.get()) } - /** - * Sets the swerve module to the specified angular and X & Y velocities using feed forward. - * - * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y - * velocities - * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration - * vectors. - * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) - */ - fun setPositionClosedLoop( - desiredVelState: SwerveModuleState, - desiredAccelState: SwerveModuleState, - optimize: Boolean = true - ) { - if (optimize) { - val optimizedVelState = - SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) - val optimizedAccelState = - SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) - io.setClosedLoop( - optimizedVelState.angle.angle, - optimizedVelState.speedMetersPerSecond.meters.perSecond, - optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } else { - io.setClosedLoop( - desiredVelState.angle.angle, - desiredVelState.speedMetersPerSecond.meters.perSecond, - desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond - ) - } + if (drivekP.hasChanged() || drivekI.hasChanged() || drivekD.hasChanged()) { + io.configureDrivePID(drivekP.get(), drivekI.get(), drivekD.get()) } - /** Creates event of the current potentiometer value as needs to be manually readjusted. */ - fun resetModuleZero() { - io.resetModuleZero() + Logger.processInputs(io.label, inputs) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveSpeedSetpointMetersPerSecond", + // if (!shouldInvert) speedSetPoint.inMetersPerSecond + // else -speedSetPoint.inMetersPerSecond + // ) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/driveAccelSetpointMetersPerSecondSq", + // accelerationSetPoint.inMetersPerSecondPerSecond + // ) + // Logger.getInstance() + // .recordOutput("${io.label}/steeringSetpointDegrees", steeringSetPoint.inDegrees) + // Logger.getInstance() + // .recordOutput( + // "${io.label}/steeringValueDegreesWithMod", + // inputs.steeringPosition.inDegrees.IEEErem(360.0) + // ) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param steering The angular position desired for the swerve module to be set to + * @param speed The speed desired for the swerve module to reach + * @param acceleration The linear acceleration used to calculate how to reach the desired speed + */ + fun set( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration = 0.0.meters.perSecond.perSecond, + optimize: Boolean = true + ) { + if (speed == 0.feet.perSecond) { + io.setOpenLoop(steeringSetPoint, 0.0.meters.perSecond) + return } + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians - /** Zeros the steering motor */ - fun zeroSteering() { - io.zeroSteering() + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - /** Zeros the drive motor */ - fun zeroDrive() { - io.zeroDrive() + speedSetPoint = + if (shouldInvert) { + speed * -1 + } else { + speed + } + accelerationSetPoint = + if (shouldInvert) { + acceleration * -1 + } else { + acceleration + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + + // io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + io.setClosedLoop(steeringSetPoint, speedSetPoint, accelerationSetPoint) + } + + fun setOpenLoop(steering: Angle, speed: LinearVelocity, optimize: Boolean = true) { + var steeringDifference = + (steering - inputs.steeringPosition).inRadians.IEEErem(2 * Math.PI).radians + + shouldInvert = steeringDifference.absoluteValue > (Math.PI / 2).radians && optimize + if (shouldInvert) { + steeringDifference -= Math.PI.withSign(steeringDifference.inRadians).radians } - fun setDriveBrakeMode(brake: Boolean) { - io.setDriveBrakeMode(brake) + val outputSpeed = + if (shouldInvert) { + speed * -1 + } else { + speed + } + steeringSetPoint = inputs.steeringPosition + steeringDifference + io.setOpenLoop(steeringSetPoint, outputSpeed) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using open loop control. + * + * @param desiredState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + */ + fun setPositionOpenLoop(desiredState: SwerveModuleState, optimize: Boolean = true) { + if (optimize) { + val optimizedState = + SwerveModuleState.optimize(desiredState, inputs.steeringPosition.inRotation2ds) + io.setOpenLoop( + optimizedState.angle.angle, + optimizedState + .speedMetersPerSecond + .meters + .perSecond // consider desaturating wheel speeds here if it doesn't work + // from drivetrain + ) + Logger.recordOutput("${io.label}/steeringSetpointOptimized", optimizedState.angle.degrees) + } else { + io.setOpenLoop(desiredState.angle.angle, desiredState.speedMetersPerSecond.meters.perSecond) + Logger.recordOutput("${io.label}/steeringSetpointNonOptimized", desiredState.angle.degrees) } - - fun setSteeringBrakeMode(brake: Boolean) { - io.setSteeringBrakeMode(brake) + } + + /** + * Sets the swerve module to the specified angular and X & Y velocities using feed forward. + * + * @param desiredVelState The desired SwerveModuleState. Contains desired angle as well as X and Y + * velocities + * @param desiredAccelState The desired SwerveModuleState that contains desired acceleration + * vectors. + * @param optimize Whether velocity and acceleration vectors should be optimized (if possible) + */ + fun setPositionClosedLoop( + desiredVelState: SwerveModuleState, + desiredAccelState: SwerveModuleState, + optimize: Boolean = true + ) { + if (optimize) { + val optimizedVelState = + SwerveModuleState.optimize(desiredVelState, inputs.steeringPosition.inRotation2ds) + val optimizedAccelState = + SwerveModuleState.optimize(desiredAccelState, inputs.steeringPosition.inRotation2ds) + io.setClosedLoop( + optimizedVelState.angle.angle, + optimizedVelState.speedMetersPerSecond.meters.perSecond, + optimizedAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) + } else { + io.setClosedLoop( + desiredVelState.angle.angle, + desiredVelState.speedMetersPerSecond.meters.perSecond, + desiredAccelState.speedMetersPerSecond.meters.perSecond.perSecond + ) } -} \ No newline at end of file + } + + /** Creates event of the current potentiometer value as needs to be manually readjusted. */ + fun resetModuleZero() { + io.resetModuleZero() + } + + /** Zeros the steering motor */ + fun zeroSteering() { + io.zeroSteering() + } + + /** Zeros the drive motor */ + fun zeroDrive() { + io.zeroDrive() + } + + fun setDriveBrakeMode(brake: Boolean) { + io.setDriveBrakeMode(brake) + } + + fun setSteeringBrakeMode(brake: Boolean) { + io.setSteeringBrakeMode(brake) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt index c6c01a06..bbc82e41 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIO.kt @@ -28,117 +28,118 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.inRadiansPerSecond import org.team4099.lib.units.perSecond + interface SwerveModuleIO { - class SwerveModuleIOInputs : LoggableInputs { - var driveAppliedVoltage = 0.0.volts - var steeringAppliedVoltage = 0.0.volts - - var driveStatorCurrent = 0.0.amps - var driveSupplyCurrent = 0.0.amps - var steeringStatorCurrent = 0.0.amps - var steeringSupplyCurrent = 0.0.amps - - var drivePosition = 0.0.meters - var steeringPosition = 0.0.degrees - - var driveVelocity = 0.0.meters.perSecond - var steeringVelocity = 0.0.degrees.perSecond - - var driveTemp = 0.0.celsius - var steeringTemp = 0.0.celsius - - var potentiometerOutputRaw = 0.0 - var potentiometerOutputRadians = 0.0.radians - - var drift = 0.0.meters - - override fun toLog(table: LogTable?) { - table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) - table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) - table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) - table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) - table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) - table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) - table?.put("drivePositionMeters", drivePosition.inMeters) - table?.put("steeringPositionRadians", steeringPosition.inRadians) - table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) - table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - table?.put("driveTempCelcius", driveTemp.inCelsius) - table?.put("steeringTempCelcius", steeringTemp.inCelsius) - table?.put("potentiometerOutputRaw", potentiometerOutputRaw) - table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) - table?.put("driftPositionMeters", drift.inMeters) - } - - override fun fromLog(table: LogTable?) { - table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { - driveAppliedVoltage = it.volts - } - table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { - steeringAppliedVoltage = it.volts - } - table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { - driveStatorCurrent = it.amps - } - table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { - driveSupplyCurrent = it.amps - } - table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { - steeringStatorCurrent = it.amps - } - table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { - steeringSupplyCurrent = it.amps - } - table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { - drivePosition = it.meters - } - table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { - steeringPosition = it.radians - } - table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { - driveVelocity = it.meters.perSecond - } - table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) - ?.let { steeringVelocity = it.radians.perSecond } - table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } - table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { - steeringTemp = it.celsius - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { - potentiometerOutputRaw = it - } - table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { - potentiometerOutputRadians = it.radians - } - table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } - } + class SwerveModuleIOInputs : LoggableInputs { + var driveAppliedVoltage = 0.0.volts + var steeringAppliedVoltage = 0.0.volts + + var driveStatorCurrent = 0.0.amps + var driveSupplyCurrent = 0.0.amps + var steeringStatorCurrent = 0.0.amps + var steeringSupplyCurrent = 0.0.amps + + var drivePosition = 0.0.meters + var steeringPosition = 0.0.degrees + + var driveVelocity = 0.0.meters.perSecond + var steeringVelocity = 0.0.degrees.perSecond + + var driveTemp = 0.0.celsius + var steeringTemp = 0.0.celsius + + var potentiometerOutputRaw = 0.0 + var potentiometerOutputRadians = 0.0.radians + + var drift = 0.0.meters + + override fun toLog(table: LogTable?) { + table?.put("driveAppliedVoltage", driveAppliedVoltage.inVolts) + table?.put("steeringAppliedVoltage", steeringAppliedVoltage.inVolts) + table?.put("driveStatorCurrentAmps", driveStatorCurrent.inAmperes) + table?.put("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes) + table?.put("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes) + table?.put("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes) + table?.put("drivePositionMeters", drivePosition.inMeters) + table?.put("steeringPositionRadians", steeringPosition.inRadians) + table?.put("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond) + table?.put("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + table?.put("driveTempCelcius", driveTemp.inCelsius) + table?.put("steeringTempCelcius", steeringTemp.inCelsius) + table?.put("potentiometerOutputRaw", potentiometerOutputRaw) + table?.put("potentiometerOutputRadians", potentiometerOutputRadians.inRadians) + table?.put("driftPositionMeters", drift.inMeters) + } + + override fun fromLog(table: LogTable?) { + table?.getDouble("driveAppliedVoltage", driveAppliedVoltage.inVolts)?.let { + driveAppliedVoltage = it.volts + } + table?.getDouble("steeringAppliedVoltage", steeringAppliedVoltage.inVolts)?.let { + steeringAppliedVoltage = it.volts + } + table?.getDouble("driveStatorCurrentAmps", driveStatorCurrent.inAmperes)?.let { + driveStatorCurrent = it.amps + } + table?.getDouble("driveSupplyCurrentAmps", driveSupplyCurrent.inAmperes)?.let { + driveSupplyCurrent = it.amps + } + table?.getDouble("steeringStatorCurrentAmps", steeringStatorCurrent.inAmperes)?.let { + steeringStatorCurrent = it.amps + } + table?.getDouble("steeringSupplyCurrentAmps", steeringSupplyCurrent.inAmperes)?.let { + steeringSupplyCurrent = it.amps + } + table?.getDouble("drivePositionMeters", drivePosition.inMeters)?.let { + drivePosition = it.meters + } + table?.getDouble("steeringPositionRadians", steeringPosition.inRadians)?.let { + steeringPosition = it.radians + } + table?.getDouble("driveVelocityMetersPerSecond", driveVelocity.inMetersPerSecond)?.let { + driveVelocity = it.meters.perSecond + } + table?.getDouble("steeringVelocityRadiansPerSecond", steeringVelocity.inRadiansPerSecond) + ?.let { steeringVelocity = it.radians.perSecond } + table?.getDouble("driveTempCelcius", driveTemp.inCelsius)?.let { driveTemp = it.celsius } + table?.getDouble("steeringTempCelcius", steeringTemp.inCelsius)?.let { + steeringTemp = it.celsius + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRaw)?.let { + potentiometerOutputRaw = it + } + table?.getDouble("potentiometerOutputRaw", potentiometerOutputRadians.inRadians)?.let { + potentiometerOutputRadians = it.radians + } + table?.getDouble("driftPositionMeters", drift.inMeters)?.let { drift = it.meters } } + } - val label: String + val label: String - fun updateInputs(inputs: SwerveModuleIOInputs) {} + fun updateInputs(inputs: SwerveModuleIOInputs) {} - fun setSteeringSetpoint(angle: Angle) {} - fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} - fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} + fun setSteeringSetpoint(angle: Angle) {} + fun setClosedLoop(steering: Angle, speed: LinearVelocity, acceleration: LinearAcceleration) {} + fun setOpenLoop(steering: Angle, speed: LinearVelocity) {} - fun resetModuleZero() {} - fun zeroSteering() {} - fun zeroDrive() {} + fun resetModuleZero() {} + fun zeroSteering() {} + fun zeroDrive() {} - fun setDriveBrakeMode(brake: Boolean) {} + fun setDriveBrakeMode(brake: Boolean) {} - fun setSteeringBrakeMode(brake: Boolean) {} + fun setSteeringBrakeMode(brake: Boolean) {} - fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) {} - fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} - fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} -} \ No newline at end of file + fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) {} + fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} + fun configureSteeringMotionMagic(maxVel: AngularVelocity, maxAccel: AngularAcceleration) {} +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt index 92ce36db..5a219453 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/drivetrain/servemodule/SwerveModuleIOSim.kt @@ -13,7 +13,6 @@ import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.controller.SimpleMotorFeedforward -import kotlin.random.Random import org.team4099.lib.units.AngularAcceleration import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearAcceleration @@ -44,231 +43,232 @@ import org.team4099.lib.units.derived.radians import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inMetersPerSecond import org.team4099.lib.units.perSecond +import kotlin.random.Random class SwerveModuleIOSim(override val label: String) : SwerveModuleIO { - // Use inverses of gear ratios because our standard is <1 is reduction - val driveMotorSim: FlywheelSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, - DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - val steerMotorSim = - FlywheelSim( - DCMotor.getNEO(1), - 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, - DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared - ) - - init { - MotorChecker.add( - "Drivetrain", - "Drive", - MotorCollection( - mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) - - MotorChecker.add( - "Drivetrain", - "Steering", - MotorCollection( - mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), - 65.amps, - 90.celsius, - 45.amps, - 100.celsius - ) - ) + // Use inverses of gear ratios because our standard is <1 is reduction + val driveMotorSim: FlywheelSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.DRIVE_SENSOR_GEAR_RATIO, + DrivetrainConstants.DRIVE_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + val steerMotorSim = + FlywheelSim( + DCMotor.getNEO(1), + 1 / DrivetrainConstants.STEERING_SENSOR_GEAR_RATIO, + DrivetrainConstants.STEERING_WHEEL_INERTIA.inKilogramsMeterSquared + ) + + init { + MotorChecker.add( + "Drivetrain", + "Drive", + MotorCollection( + mutableListOf(SimulatedMotor(driveMotorSim, "$label Drive Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + + MotorChecker.add( + "Drivetrain", + "Steering", + MotorCollection( + mutableListOf(SimulatedMotor(steerMotorSim, "$label Steering Motor")), + 65.amps, + 90.celsius, + 45.amps, + 100.celsius + ) + ) + } + + var turnRelativePosition = 0.0.radians + var turnAbsolutePosition = + (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to + var driveVelocity = 0.0.meters.perSecond + var drift: Length = 0.0.meters + + private val driveFeedback = + PIDController( + DrivetrainConstants.PID.SIM_DRIVE_KP, + DrivetrainConstants.PID.SIM_DRIVE_KI, + DrivetrainConstants.PID.SIM_DRIVE_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + private val driveFeedForward = + SimpleMotorFeedforward( + DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV + ) + + private val steeringFeedback = + PIDController( + DrivetrainConstants.PID.SIM_STEERING_KP, + DrivetrainConstants.PID.SIM_STEERING_KI, + DrivetrainConstants.PID.SIM_STEERING_KD, + Constants.Universal.LOOP_PERIOD_TIME + ) + + init { + steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) + steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR + } + + override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { + driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + val angleDifference: Angle = + (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + .radians + turnAbsolutePosition += angleDifference + turnRelativePosition += angleDifference + + // constrains it to 2pi radians + while (turnAbsolutePosition < 0.radians) { + turnAbsolutePosition += (2.0 * Math.PI).radians } - - var turnRelativePosition = 0.0.radians - var turnAbsolutePosition = - (Math.random() * 2.0 * Math.PI).radians // getting a random value that we zero to - var driveVelocity = 0.0.meters.perSecond - var drift: Length = 0.0.meters - - private val driveFeedback = - PIDController( - DrivetrainConstants.PID.SIM_DRIVE_KP, - DrivetrainConstants.PID.SIM_DRIVE_KI, - DrivetrainConstants.PID.SIM_DRIVE_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - private val driveFeedForward = - SimpleMotorFeedforward( - DrivetrainConstants.PID.SIM_DRIVE_KS, DrivetrainConstants.PID.SIM_DRIVE_KV - ) - - private val steeringFeedback = - PIDController( - DrivetrainConstants.PID.SIM_STEERING_KP, - DrivetrainConstants.PID.SIM_STEERING_KI, - DrivetrainConstants.PID.SIM_STEERING_KD, - Constants.Universal.LOOP_PERIOD_TIME - ) - - init { - steeringFeedback.enableContinuousInput(-Math.PI.radians, Math.PI.radians) - steeringFeedback.errorTolerance = DrivetrainConstants.ALLOWED_STEERING_ANGLE_ERROR - } - - override fun updateInputs(inputs: SwerveModuleIO.SwerveModuleIOInputs) { - driveMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - steerMotorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - val angleDifference: Angle = - (steerMotorSim.angularVelocityRadPerSec * Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - .radians - turnAbsolutePosition += angleDifference - turnRelativePosition += angleDifference - - // constrains it to 2pi radians - while (turnAbsolutePosition < 0.radians) { - turnAbsolutePosition += (2.0 * Math.PI).radians - } - while (turnAbsolutePosition > (2.0 * Math.PI).radians) { - turnAbsolutePosition -= (2.0 * Math.PI).radians - } - - // s = r * theta -> d/2 * rad/s = m/s - driveVelocity = - (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - - // simming drift - var loopCycleDrift = 0.0.meters - if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { - loopCycleDrift = - (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) - .meters // 0.0005 is just a nice number that ended up working out for drift - } - drift += loopCycleDrift - - // pi * d * rotations = distance travelled - inputs.drivePosition = - inputs.drivePosition + - DrivetrainConstants.WHEEL_DIAMETER * - Math.PI * - ( - driveMotorSim.angularVelocityRadPerSec * - Constants.Universal.LOOP_PERIOD_TIME.inSeconds - ) - .radians - .inRotations + - loopCycleDrift // adding a random amount of drift - inputs.steeringPosition = turnAbsolutePosition - inputs.drift = drift - - inputs.driveVelocity = driveVelocity - inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond - - inputs.driveAppliedVoltage = (-1337).volts - inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps - inputs.driveStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.driveTemp = (-1337).celsius - inputs.steeringTemp = (-1337).celsius - - inputs.steeringAppliedVoltage = (-1337).volts - inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps - inputs.steeringStatorCurrent = - (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator - // current - - inputs.potentiometerOutputRadians = turnAbsolutePosition - inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians - - // Setting a more accurate simulated voltage under load - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage( - inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes - ) - ) + while (turnAbsolutePosition > (2.0 * Math.PI).radians) { + turnAbsolutePosition -= (2.0 * Math.PI).radians } - // 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) - driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) - } + // s = r * theta -> d/2 * rad/s = m/s + driveVelocity = + (DrivetrainConstants.WHEEL_DIAMETER / 2 * driveMotorSim.angularVelocityRadPerSec).perSecond - private fun setSteeringVoltage(volts: ElectricalPotential) { - val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) - steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + // simming drift + var loopCycleDrift = 0.0.meters + if (Constants.Tuning.SIMULATE_DRIFT && driveVelocity > 2.0.meters.perSecond) { + loopCycleDrift = + (Random.nextDouble() * Constants.Tuning.DRIFT_CONSTANT) + .meters // 0.0005 is just a nice number that ended up working out for drift } - - override fun setSteeringSetpoint(angle: Angle) { - val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) - Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) - Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) - Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) - Logger.recordOutput( - "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + drift += loopCycleDrift + + // pi * d * rotations = distance travelled + inputs.drivePosition = + inputs.drivePosition + + DrivetrainConstants.WHEEL_DIAMETER * + Math.PI * + ( + driveMotorSim.angularVelocityRadPerSec * + Constants.Universal.LOOP_PERIOD_TIME.inSeconds ) - setSteeringVoltage(feedback) - } - - override fun setClosedLoop( - steering: Angle, - speed: LinearVelocity, - acceleration: LinearAcceleration - ) { - Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) - val feedforward = driveFeedForward.calculate(speed, acceleration) - setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) - - setSteeringSetpoint(steering) - } - - override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { - setDriveVoltage( - RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) - ) - setSteeringSetpoint(steering) - } - - override fun resetModuleZero() { - println("Resetting your module's 0 doesn't do anything meaningful in sim :(") - } - - override fun zeroDrive() { - println("Zero drive do anything meaningful in sim") - } - - override fun zeroSteering() { - turnAbsolutePosition = 0.0.radians - } - - override fun configureDrivePID( - kP: ProportionalGain, Volt>, - kI: IntegralGain, Volt>, - kD: DerivativeGain, Volt> - ) { - driveFeedback.setPID(kP, kI, kD) - } - - override fun configureSteeringPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - steeringFeedback.setPID(kP, kI, kD) - } - - override fun setDriveBrakeMode(brake: Boolean) { - println("Can't set brake mode in simulation") - } - - override fun configureSteeringMotionMagic( - maxVel: AngularVelocity, - maxAccel: AngularAcceleration - ) { - println("Can't configure motion magic in simulation") - } -} \ No newline at end of file + .radians + .inRotations + + loopCycleDrift // adding a random amount of drift + inputs.steeringPosition = turnAbsolutePosition + inputs.drift = drift + + inputs.driveVelocity = driveVelocity + inputs.steeringVelocity = steerMotorSim.angularVelocityRadPerSec.radians.perSecond + + inputs.driveAppliedVoltage = (-1337).volts + inputs.driveSupplyCurrent = driveMotorSim.currentDrawAmps.amps + inputs.driveStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.driveTemp = (-1337).celsius + inputs.steeringTemp = (-1337).celsius + + inputs.steeringAppliedVoltage = (-1337).volts + inputs.steeringSupplyCurrent = steerMotorSim.currentDrawAmps.amps + inputs.steeringStatorCurrent = + (-1337).amps // no way to get applied voltage to motor so can't actually calculate stator + // current + + inputs.potentiometerOutputRadians = turnAbsolutePosition + inputs.potentiometerOutputRaw = turnAbsolutePosition.inRadians + + // Setting a more accurate simulated voltage under load + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage( + inputs.driveSupplyCurrent.inAmperes + inputs.steeringSupplyCurrent.inAmperes + ) + ) + } + + // 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) + driveMotorSim.setInputVoltage(driveAppliedVolts.inVolts) + } + + private fun setSteeringVoltage(volts: ElectricalPotential) { + val turnAppliedVolts = clamp(volts, -12.0.volts, 12.0.volts) + steerMotorSim.setInputVoltage(turnAppliedVolts.inVolts) + } + + override fun setSteeringSetpoint(angle: Angle) { + val feedback = steeringFeedback.calculate(turnAbsolutePosition, angle) + Logger.recordOutput("Drivetrain/PID/steeringFeedback", feedback.inVolts) + Logger.recordOutput("Drivetrain/PID/kP", steeringFeedback.proportionalGain.inVoltsPerDegree) + Logger.recordOutput("Drivetrain/PID/kI", steeringFeedback.integralGain.inVoltsPerDegreeSeconds) + Logger.recordOutput( + "Drivetrain/PID/kD", steeringFeedback.derivativeGain.inVoltsPerDegreePerSecond + ) + setSteeringVoltage(feedback) + } + + override fun setClosedLoop( + steering: Angle, + speed: LinearVelocity, + acceleration: LinearAcceleration + ) { + Logger.recordOutput("$label/desiredDriveSpeedMPS", speed.inMetersPerSecond) + val feedforward = driveFeedForward.calculate(speed, acceleration) + setDriveVoltage(feedforward + driveFeedback.calculate(driveVelocity, speed)) + + setSteeringSetpoint(steering) + } + + override fun setOpenLoop(steering: Angle, speed: LinearVelocity) { + setDriveVoltage( + RoboRioSim.getVInVoltage().volts * (speed / DrivetrainConstants.DRIVE_SETPOINT_MAX) + ) + setSteeringSetpoint(steering) + } + + override fun resetModuleZero() { + println("Resetting your module's 0 doesn't do anything meaningful in sim :(") + } + + override fun zeroDrive() { + println("Zero drive do anything meaningful in sim") + } + + override fun zeroSteering() { + turnAbsolutePosition = 0.0.radians + } + + override fun configureDrivePID( + kP: ProportionalGain, Volt>, + kI: IntegralGain, Volt>, + kD: DerivativeGain, Volt> + ) { + driveFeedback.setPID(kP, kI, kD) + } + + override fun configureSteeringPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + steeringFeedback.setPID(kP, kI, kD) + } + + override fun setDriveBrakeMode(brake: Boolean) { + println("Can't set brake mode in simulation") + } + + override fun configureSteeringMotionMagic( + maxVel: AngularVelocity, + maxAccel: AngularAcceleration + ) { + println("Can't configure motion magic in simulation") + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index b53531ee..e3e10729 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -5,352 +5,379 @@ import com.team4099.lib.logging.LoggedTunableNumber import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants -import edu.wpi.first.units.Voltage import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command -import edu.wpi.first.wpilibj2.command.Commands -import edu.wpi.first.wpilibj2.command.Commands.run import edu.wpi.first.wpilibj2.command.Commands.runOnce -import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ElevatorFeedforward import org.team4099.lib.controller.TrapezoidProfile import org.team4099.lib.units.base.Meter -import org.team4099.lib.units.base.inches -import org.team4099.lib.units.derived.* -import org.team4099.lib.units.perSecond import org.team4099.lib.units.base.inInches -import org.littletonrobotics.junction.Logger import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.degrees +import org.team4099.lib.units.derived.inDegrees +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.inVoltsPerInch +import org.team4099.lib.units.derived.inVoltsPerInchPerSecond +import org.team4099.lib.units.derived.inVoltsPerInchSeconds +import org.team4099.lib.units.derived.perInch +import org.team4099.lib.units.derived.perInchSeconds +import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inInchesPerSecond -import kotlin.time.Duration.Companion.seconds +import org.team4099.lib.units.perSecond +import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest class Elevator(val io: ElevatorIO) { - val inputs = ElevatorIO.ElevatorInputs() - private var elevatorFeedforward : ElevatorFeedforward = - ElevatorFeedforward( - ElevatorConstants.ELEVATOR_KS, - ElevatorConstants.ELEVATOR_KG, - ElevatorConstants.ELEVATOR_KV, - ElevatorConstants.ELEVATOR_KA - ) - - private val kP = LoggedTunableValue( - "Elevator/kP", - Pair({ it.inVoltsPerInch }, { it.volts.perInch }) + val inputs = ElevatorIO.ElevatorInputs() + private var elevatorFeedforward: ElevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA ) - private val kI = LoggedTunableValue( - "Elevator/kI", - Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) + + private val kP = + LoggedTunableValue("Elevator/kP", Pair({ it.inVoltsPerInch }, { it.volts.perInch })) + private val kI = + LoggedTunableValue( + "Elevator/kI", Pair({ it.inVoltsPerInchSeconds }, { it.volts.perInchSeconds }) ) - private val kD = LoggedTunableValue( - "Elevator/kD", - Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) + private val kD = + LoggedTunableValue( + "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) - object TunableElevatorHeights { - val enableElevator = - LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) - - val minPosition = - LoggedTunableValue( - "Elevator/minPosition", - ElevatorConstants.ELEVATOR_IDLE_HEIGHT, - Pair({ it.inInches }, { it.inches }) - ) - val maxPosition = - LoggedTunableValue( - "Elevator/maxPosition", - ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, - Pair({ it.inInches }, { it.inches }) - ) - - //TODO: change voltages - val openLoopExtendVoltage = - LoggedTunableValue( - "Elevator/openLoopExtendVoltage", - 8.volts, - Pair({ it.inVolts }, { it.volts }) - ) - val openLoopRetractVoltage = - LoggedTunableValue( - "Elevator/openLoopRetractVoltage", - -12.0.volts, - Pair({ it.inVolts }, { it.volts }) - ) - - val shootSpeakerPosition = LoggedTunableValue( - "Elevator/shootSpeakerPosition", - ElevatorConstants.SHOOT_SPEAKER_POSITION - ) - val shootAmpPosition = LoggedTunableValue( - "Elevator/shootAmpPosition", - ElevatorConstants.SHOOT_AMP_POSITION - ) - val sourceNoteOffset = LoggedTunableValue( - "Elevator/sourceNoteOffset", - ElevatorConstants.SOURCE_NOTE_OFFSET - ) - - val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) - val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) - val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) - val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) - val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) - val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) - val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) - val thetaPos1 = LoggedTunableValue("Elevator/thetaPos1", - ElevatorConstants.ELEVATOR_THETA_POS, - Pair({ it.inDegrees }, { it.degrees }) - ) - } - - val forwardLimitReached: Boolean - get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION - val reverseLimitReached: Boolean - get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION - - val forwardOpenLoopLimitReached: Boolean - get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION - val reverseOpenLoopLimitReached: Boolean - get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION - - var isHomed = false - - var currentState: ElevatorState = ElevatorState.UNINITIALIZED - var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) - set(value) { - when(value) { - is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage - is ElevatorRequest.TargetingPosition -> { - elevatorPositionTarget = value.position - } - else -> {} - } - field = value + object TunableElevatorHeights { + val enableElevator = + LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) + + val minPosition = + LoggedTunableValue( + "Elevator/minPosition", + ElevatorConstants.ELEVATOR_IDLE_HEIGHT, + Pair({ it.inInches }, { it.inches }) + ) + val maxPosition = + LoggedTunableValue( + "Elevator/maxPosition", + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION, + Pair({ it.inInches }, { it.inches }) + ) + + // TODO: change voltages + val openLoopExtendVoltage = + LoggedTunableValue( + "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts }) + ) + val openLoopRetractVoltage = + LoggedTunableValue( + "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts }) + ) + + val shootSpeakerPosition = + LoggedTunableValue( + "Elevator/shootSpeakerPosition", ElevatorConstants.SHOOT_SPEAKER_POSITION + ) + val shootAmpPosition = + LoggedTunableValue("Elevator/shootAmpPosition", ElevatorConstants.SHOOT_AMP_POSITION) + val sourceNoteOffset = + LoggedTunableValue("Elevator/sourceNoteOffset", ElevatorConstants.SOURCE_NOTE_OFFSET) + + val xPos = LoggedTunableValue("Elevator/xPos", 0.0.inches) + val yPos = LoggedTunableValue("Elevator/yPos", 0.0.inches) + val zPos = LoggedTunableValue("Elevator/zPos", 0.0.inches) + val thetaPos = LoggedTunableValue("Elevator/thetaPos", 0.0.degrees) + val xPos1 = LoggedTunableValue("Elevator/xPos1", 0.0.inches) + val yPos1 = LoggedTunableValue("Elevator/yPos1", 0.0.inches) + val zPos1 = LoggedTunableValue("Elevator/zPos1", 0.0.inches) + val thetaPos1 = + LoggedTunableValue( + "Elevator/thetaPos1", + ElevatorConstants.ELEVATOR_THETA_POS, + Pair({ it.inDegrees }, { it.degrees }) + ) + } + + val forwardLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION + val reverseLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION + + val forwardOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition >= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION + val reverseOpenLoopLimitReached: Boolean + get() = inputs.elevatorPosition <= ElevatorConstants.ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION + + var isHomed = false + + var currentState: ElevatorState = ElevatorState.UNINITIALIZED + var currentRequest: ElevatorRequest = ElevatorRequest.OpenLoop(0.0.volts) + set(value) { + when (value) { + is ElevatorRequest.OpenLoop -> elevatorVoltageTarget = value.voltage + is ElevatorRequest.TargetingPosition -> { + elevatorPositionTarget = value.position } + else -> {} + } + field = value + } - var elevatorPositionTarget = 0.0.inches - private set - var elevatorVelocityTarget = 0.0.inches.perSecond - private set - var elevatorVoltageTarget = 0.0.volts - private set - - private var lastRequestedPosition = -9999.inches - private var lastRequestedVelocity = -9999.inches.perSecond - private var lastRequestedVoltage = -9999.volts - - private var timeProfileGeneratedAt = Clock.fpgaTime - - private var lastHomingStatorCurrentTripTime = Clock.fpgaTime - - // Trapezoidal Profile Constraints - private var elevatorConstraints: TrapezoidProfile.Constraints = - TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION) - private var elevatorSetpoint: TrapezoidProfile.State = - TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) - private var elevatorProfile = - TrapezoidProfile( - elevatorConstraints, - TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), - TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) - ) - - val isAtTargetedPosition: Boolean - get() = - (currentRequest is ElevatorRequest.TargetingPosition && - elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && - (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= - ElevatorConstants.ELEVATOR_TOLERANCE) || - (TunableElevatorHeights.enableElevator.get() != 1.0) - - val canContinueSafely: Boolean - get() = - currentRequest is ElevatorRequest.TargetingPosition && ( - ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || - elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) - ) && lastRequestedPosition == elevatorPositionTarget - - - init { - TunableElevatorHeights + var elevatorPositionTarget = 0.0.inches + private set + var elevatorVelocityTarget = 0.0.inches.perSecond + private set + var elevatorVoltageTarget = 0.0.volts + private set - // Initializes PID constants and changes FF depending on if sim or real - if(RobotBase.isReal()) { - isHomed = false + private var lastRequestedPosition = -9999.inches + private var lastRequestedVelocity = -9999.inches.perSecond + private var lastRequestedVoltage = -9999.volts - kP.initDefault(ElevatorConstants.REAL_KP) - kI.initDefault(ElevatorConstants.REAL_KI) - kD.initDefault(ElevatorConstants.REAL_KD) - } else { - isHomed = true + private var timeProfileGeneratedAt = Clock.fpgaTime - kP.initDefault(ElevatorConstants.SIM_KP) - kI.initDefault(ElevatorConstants.SIM_KI) - kD.initDefault(ElevatorConstants.SIM_KD) - } + private var lastHomingStatorCurrentTripTime = Clock.fpgaTime - elevatorFeedforward = ElevatorFeedforward( - ElevatorConstants.ELEVATOR_KS, - ElevatorConstants.ELEVATOR_KG, - ElevatorConstants.ELEVATOR_KV, - ElevatorConstants.ELEVATOR_KA - ) + // Trapezoidal Profile Constraints + private var elevatorConstraints: TrapezoidProfile.Constraints = + TrapezoidProfile.Constraints( + ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION + ) + private var elevatorSetpoint: TrapezoidProfile.State = + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + private var elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond), + TrapezoidProfile.State(-9999.inches, -9999.inches.perSecond) + ) - io.configPID(kP.get(), kI.get(), kD.get()) + val isAtTargetedPosition: Boolean + get() = + ( + currentRequest is ElevatorRequest.TargetingPosition && + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) && + (inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= + ElevatorConstants.ELEVATOR_TOLERANCE + ) || + (TunableElevatorHeights.enableElevator.get() != 1.0) + + val canContinueSafely: Boolean + get() = + currentRequest is ElevatorRequest.TargetingPosition && + ( + ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || + elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) + ) && + lastRequestedPosition == elevatorPositionTarget + + init { + TunableElevatorHeights + + // Initializes PID constants and changes FF depending on if sim or real + if (RobotBase.isReal()) { + isHomed = false + + kP.initDefault(ElevatorConstants.REAL_KP) + kI.initDefault(ElevatorConstants.REAL_KI) + kD.initDefault(ElevatorConstants.REAL_KD) + } else { + isHomed = true + + kP.initDefault(ElevatorConstants.SIM_KP) + kI.initDefault(ElevatorConstants.SIM_KI) + kD.initDefault(ElevatorConstants.SIM_KD) } - fun periodic() { - io.updateInputs(inputs) - if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { - io.configPID(kP.get(), kI.get(), kD.get()) + elevatorFeedforward = + ElevatorFeedforward( + ElevatorConstants.ELEVATOR_KS, + ElevatorConstants.ELEVATOR_KG, + ElevatorConstants.ELEVATOR_KV, + ElevatorConstants.ELEVATOR_KA + ) + + io.configPID(kP.get(), kI.get(), kD.get()) + } + + fun periodic() { + io.updateInputs(inputs) + if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { + io.configPID(kP.get(), kI.get(), kD.get()) + } + Logger.processInputs("Elevator", inputs) + Logger.recordOutput("Elevator/currentState", currentState.name) + Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) + Logger.recordOutput( + "Elevator/elevatorHeight", + inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET + ) + if (Constants.Tuning.DEBUGING_MODE) { + Logger.recordOutput("Elevator/isHomed", isHomed) + Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) + + Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) + Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) + + Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) + Logger.recordOutput( + "Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond + ) + Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) + + Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) + Logger.recordOutput( + "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) + + Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) + Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + } + var nextState = currentState + when (currentState) { + ElevatorState.UNINITIALIZED -> { + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.OPEN_LOOP -> { + setOutputVoltage(elevatorVoltageTarget) + nextState = fromElevatorRequestToState(currentRequest) + } + ElevatorState.TARGETING_POSITION -> { + if ((elevatorPositionTarget != lastRequestedPosition) || + (elevatorVelocityTarget != lastRequestedVelocity) + ) { + val preProfileGenerate = Clock.realTimestamp + elevatorProfile = + TrapezoidProfile( + elevatorConstraints, + TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), + TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity) + ) + val postProfileGenerate = Clock.realTimestamp + Logger.recordOutput( + "/Elevator/profileGenerationMS", + postProfileGenerate.inSeconds - preProfileGenerate.inSeconds + ) + Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) + Logger.recordOutput( + "Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond + ) + timeProfileGeneratedAt = Clock.fpgaTime + lastRequestedPosition = elevatorPositionTarget + lastRequestedVelocity = elevatorVelocityTarget } - Logger.processInputs("Elevator", inputs) - Logger.recordOutput("Elevator/currentState", currentState.name) - Logger.recordOutput("Elevator/currentRequest", currentRequest.javaClass.simpleName) - Logger.recordOutput("Elevator/elevatorHeight", inputs.elevatorPosition - ElevatorConstants.ELEVATOR_GROUND_OFFSET) - if (Constants.Tuning.DEBUGING_MODE) { - Logger.recordOutput("Elevator/isHomed", isHomed) - Logger.recordOutput("Elevator/canContinueSafely", canContinueSafely) - - Logger.recordOutput("Elevator/isAtTargetPosition", isAtTargetedPosition) - Logger.recordOutput("Elevator/lastGeneratedAt", timeProfileGeneratedAt.inSeconds) - - Logger.recordOutput("Elevator/elevatorPositionTarget", elevatorPositionTarget.inInches) - Logger.recordOutput("Elevator/elevatorVelocityTarget", elevatorVelocityTarget.inInchesPerSecond) - Logger.recordOutput("Elevator/elevatorVoltageTarget", elevatorVoltageTarget.inVolts) - - Logger.recordOutput("Elevator/lastElevatorPositionTarget", lastRequestedPosition.inInches) - Logger.recordOutput( - "Elevator/lastElevatorVelocityTarget", lastRequestedVelocity.inInchesPerSecond - ) - Logger.recordOutput("Elevator/lastElevatorVoltageTarget", lastRequestedVoltage.inVolts) - - Logger.recordOutput("Elevator/forwardLimitReached", forwardLimitReached) - Logger.recordOutput("Elevator/reverseLimitReached", reverseLimitReached) + val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt + val profilePosition = elevatorProfile.calculate(timeElapsed) + setPosition(profilePosition) + Logger.recordOutput( + "Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed) + ) + Logger.recordOutput( + "Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond + ) + Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) + nextState = fromElevatorRequestToState(currentRequest) + if (!(currentState.equivalentToRequest(currentRequest))) { + lastRequestedVelocity = -999.inches.perSecond + lastRequestedPosition = -999.inches } - var nextState = currentState - when (currentState) { - ElevatorState.UNINITIALIZED -> { - nextState = fromElevatorRequestToState(currentRequest) - } - ElevatorState.OPEN_LOOP -> { - setOutputVoltage(elevatorVoltageTarget) - nextState = fromElevatorRequestToState(currentRequest) - } - ElevatorState.TARGETING_POSITION -> { - if ((elevatorPositionTarget != lastRequestedPosition) || (elevatorVelocityTarget != lastRequestedVelocity)) { - val preProfileGenerate = Clock.realTimestamp - elevatorProfile = TrapezoidProfile(elevatorConstraints, TrapezoidProfile.State(elevatorPositionTarget, elevatorVelocityTarget), TrapezoidProfile.State(inputs.elevatorPosition, inputs.elevatorVelocity)) - val postProfileGenerate = Clock.realTimestamp - Logger.recordOutput("/Elevator/profileGenerationMS", postProfileGenerate.inSeconds - preProfileGenerate.inSeconds) - Logger.recordOutput("Elevator/initialPosition", elevatorProfile.initial.position.inInches) - Logger.recordOutput("Elevator/initialVelocity", elevatorProfile.initial.velocity.inInchesPerSecond) - timeProfileGeneratedAt = Clock.fpgaTime - lastRequestedPosition = elevatorPositionTarget - lastRequestedVelocity = elevatorVelocityTarget - } - val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt - val profilePosition = elevatorProfile.calculate(timeElapsed) - setPosition(profilePosition) - Logger.recordOutput("Elevator/completedMotionProfile", elevatorProfile.isFinished(timeElapsed)) - Logger.recordOutput("Elevator/profileTargetVelocity", profilePosition.velocity.inInchesPerSecond) - Logger.recordOutput("Elevator/profileTargetPosition", profilePosition.position.inInches) - nextState = fromElevatorRequestToState(currentRequest) - if (! (currentState.equivalentToRequest(currentRequest))) { - lastRequestedVelocity = -999.inches.perSecond - lastRequestedPosition = -999.inches - } - } - ElevatorState.HOME -> { - if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { - lastHomingStatorCurrentTripTime = Clock.fpgaTime - } - if (! inputs.isSimulating && (! isHomed && inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && Clock.fpgaTime - lastHomingStatorCurrentTripTime < ElevatorConstants.HOMING_STALL_TIME_THRESHOLD)) { - setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) - } - else { - zeroEncoder() - isHomed = true - } - if (isHomed) { - nextState = fromElevatorRequestToState(currentRequest) - } - } + } + ElevatorState.HOME -> { + if (inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT) { + lastHomingStatorCurrentTripTime = Clock.fpgaTime } - currentState = nextState - } - - fun setOutputVoltage(voltage: ElectricalPotential) { - if ((forwardLimitReached) && (voltage > 0.volts) || (reverseLimitReached) && (voltage < 0.volts)) { - io.setOutputVoltage(0.volts) + if (!inputs.isSimulating && + ( + !isHomed && + inputs.leaderStatorCurrent < ElevatorConstants.HOMING_STATOR_CURRENT && + Clock.fpgaTime - lastHomingStatorCurrentTripTime < + ElevatorConstants.HOMING_STALL_TIME_THRESHOLD + ) + ) { + setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + } else { + zeroEncoder() + isHomed = true } - else { - io.setOutputVoltage(voltage) + if (isHomed) { + nextState = fromElevatorRequestToState(currentRequest) } + } } - - fun setHomeVoltage(voltage: ElectricalPotential) { - io.setOutputVoltage(voltage) + currentState = nextState + } + + fun setOutputVoltage(voltage: ElectricalPotential) { + if ((forwardLimitReached) && (voltage > 0.volts) || + (reverseLimitReached) && (voltage < 0.volts) + ) { + io.setOutputVoltage(0.volts) + } else { + io.setOutputVoltage(voltage) } - - fun zeroEncoder() { - io.zeroEncoder() + } + + fun setHomeVoltage(voltage: ElectricalPotential) { + io.setOutputVoltage(voltage) + } + + fun zeroEncoder() { + io.zeroEncoder() + } + + fun setPosition(setpoint: TrapezoidProfile.State) { + val elevatorAcceleration = + (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME + elevatorSetpoint = setpoint + val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) + if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || + (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition) + ) { + io.setOutputVoltage(0.volts) + } else { + io.setPosition(setpoint.position, feedForward) } - - fun setPosition(setpoint : TrapezoidProfile.State < Meter >) { - val elevatorAcceleration = (setpoint.velocity - elevatorSetpoint.velocity) / Constants.Universal.LOOP_PERIOD_TIME - elevatorSetpoint = setpoint - val feedForward = elevatorFeedforward.calculate(setpoint.velocity, elevatorAcceleration) - if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition)) { - io.setOutputVoltage(0.volts) - } - else { - io.setPosition(setpoint.position, feedForward) - } + } + + companion object { + enum class ElevatorState { + UNINITIALIZED, + TARGETING_POSITION, + OPEN_LOOP, + HOME; + inline fun equivalentToRequest(request: ElevatorRequest): Boolean { + return (request is ElevatorRequest.Home && this == HOME) || + (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || + (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) + } } - companion object { - enum class ElevatorState { - UNINITIALIZED, - TARGETING_POSITION, - OPEN_LOOP, - HOME; - inline fun equivalentToRequest(request : ElevatorRequest) : Boolean { - return (request is ElevatorRequest.Home && this == HOME) || (request is ElevatorRequest.OpenLoop && this == OPEN_LOOP) || (request is ElevatorRequest.TargetingPosition && this == TARGETING_POSITION) - } - } - - inline fun fromElevatorRequestToState(request : ElevatorRequest) : ElevatorState { - return when(request) { - is ElevatorRequest.Home -> ElevatorState.HOME - is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP - is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION - } - } + inline fun fromElevatorRequestToState(request: ElevatorRequest): ElevatorState { + return when (request) { + is ElevatorRequest.Home -> ElevatorState.HOME + is ElevatorRequest.OpenLoop -> ElevatorState.OPEN_LOOP + is ElevatorRequest.TargetingPosition -> ElevatorState.TARGETING_POSITION + } } + } - fun testElevatorOpenLoopRetractCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.OpenLoop(-10.volts) - }) - } + fun testElevatorOpenLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(-10.volts) }) + } - fun testElevatorOpenLoopExtendCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.OpenLoop(10.volts) - }) - } + fun testElevatorOpenLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.OpenLoop(10.volts) }) + } - fun elevatorClosedLoopRetractCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.TargetingPosition(12.inches) - }) - } + fun elevatorClosedLoopRetractCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(12.inches) }) + } - fun testElevatorClosedLoopExtendCommand(): Command { - return runOnce({ - currentRequest = ElevatorRequest.TargetingPosition(4.inches) - }) - } -} \ No newline at end of file + fun testElevatorClosedLoopExtendCommand(): Command { + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) }) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index d5c27eb3..e01bd0a4 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -21,93 +21,89 @@ import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.perSecond interface ElevatorIO { - class ElevatorInputs : LoggableInputs { - var elevatorPosition = 0.0.inches - var elevatorVelocity = 0.0.inches.perSecond + class ElevatorInputs : LoggableInputs { + var elevatorPosition = 0.0.inches + var elevatorVelocity = 0.0.inches.perSecond - var leaderAppliedVoltage = 0.0.volts - var followerAppliedVoltage = 0.0.volts + var leaderAppliedVoltage = 0.0.volts + var followerAppliedVoltage = 0.0.volts - var leaderSupplyCurrent = 0.0.amps - var leaderStatorCurrent = 0.0.amps + var leaderSupplyCurrent = 0.0.amps + var leaderStatorCurrent = 0.0.amps - var followerSupplyCurrent = 0.0.amps - var followerStatorCurrent = 0.0.amps + var followerSupplyCurrent = 0.0.amps + var followerStatorCurrent = 0.0.amps - var leaderTempCelcius = 0.0.celsius - var followerTempCelcius = 0.0.celsius + var leaderTempCelcius = 0.0.celsius + var followerTempCelcius = 0.0.celsius - var leaderRawPosition = 0.0 - var followerRawPosition = 0.0 + var leaderRawPosition = 0.0 + var followerRawPosition = 0.0 - var isSimulating = false + var isSimulating = false - override fun toLog(table: LogTable) { - table?.put("elevatorPositionInches", elevatorPosition.inInches) - table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) - table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) - table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) - table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) - table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) - table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) - table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) - table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) - table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) - table?.put("elevatorLeaderRawPosition", leaderRawPosition) - table?.put("elevatorFollowRawPosition", followerRawPosition) - } + override fun toLog(table: LogTable) { + table?.put("elevatorPositionInches", elevatorPosition.inInches) + table?.put("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond) + table?.put("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts) + table?.put("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts) + table?.put("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes) + table?.put("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes) + table?.put("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes) + table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) + table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) + table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) + table?.put("elevatorLeaderRawPosition", leaderRawPosition) + table?.put("elevatorFollowRawPosition", followerRawPosition) + } - override fun fromLog(table: LogTable?) { - table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { - elevatorPosition = it.inches - } - table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { - elevatorVelocity = it.inches.perSecond - } - table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { - leaderAppliedVoltage = it.volts - } - table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { - followerAppliedVoltage = it.volts - } - table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { - leaderStatorCurrent = it.amps - } - table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { - leaderSupplyCurrent = it.amps - } - table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { - leaderTempCelcius = it.celsius - } + override fun fromLog(table: LogTable?) { + table?.get("elevatorPositionInches", elevatorPosition.inInches)?.let { + elevatorPosition = it.inches + } + table?.get("elevatorVelocityInchesPerSec", elevatorVelocity.inInchesPerSecond)?.let { + elevatorVelocity = it.inches.perSecond + } + table?.get("elevatorLeaderAppliedVolts", leaderAppliedVoltage.inVolts)?.let { + leaderAppliedVoltage = it.volts + } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorLeaderStatorCurrentAmps", leaderStatorCurrent.inAmperes)?.let { + leaderStatorCurrent = it.amps + } + table?.get("elevatorLeaderSupplyCurrentAmps", leaderSupplyCurrent.inAmperes)?.let { + leaderSupplyCurrent = it.amps + } + table?.get("elevatorLeaderTempCelcius", leaderTempCelcius.inCelsius)?.let { + leaderTempCelcius = it.celsius + } - table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { - followerAppliedVoltage = it.volts - } - table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { - followerStatorCurrent = it.amps - } - table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { - followerSupplyCurrent = it.amps - } - table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { - followerTempCelcius = it.celsius - } - table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { - leaderRawPosition = it - } - table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { - followerRawPosition = it - } - } + table?.get("elevatorFollowerAppliedVolts", followerAppliedVoltage.inVolts)?.let { + followerAppliedVoltage = it.volts + } + table?.get("elevatorFollowerStatorCurrentAmps", followerStatorCurrent.inAmperes)?.let { + followerStatorCurrent = it.amps + } + table?.get("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes)?.let { + followerSupplyCurrent = it.amps + } + table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { + followerTempCelcius = it.celsius + } + table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { leaderRawPosition = it } + table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { followerRawPosition = it } } + } - fun updateInputs(inputs: ElevatorInputs) {} - fun setOutputVoltage(voltage: ElectricalPotential) {} - fun setPosition(position: Length, feedForward: ElectricalPotential) {} - fun zeroEncoder() {} - fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) {} + fun updateInputs(inputs: ElevatorInputs) {} + fun setOutputVoltage(voltage: ElectricalPotential) {} + fun setPosition(position: Length, feedForward: ElectricalPotential) {} + fun zeroEncoder() {} + fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) {} } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 97b6ad8b..fd3907d1 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -6,113 +6,176 @@ import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.PositionDutyCycle import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue +import com.team4099.lib.phoenix6.PositionVoltage import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants 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.controller.PIDController -import org.team4099.lib.units.Velocity -import org.team4099.lib.units.base.* +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 +import org.team4099.lib.units.base.inAmperes +import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.ctreLinearMechanismSensor -import org.team4099.lib.units.derived.* - -object ElevatorIOKraken: ElevatorIO { - private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) - private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) - private val leaderSensor - - - - = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) - private val followerSensor = ctreLinearMechanismSensor(elevatorLeaderKraken, ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION) - private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() - private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() - - var elevatorLeaderStatorCurrentSignal: StatusSignal - var elevatorLeaderSupplyCurrentSignal: StatusSignal - var elevatorLeadertempSignal: StatusSignal - var elevatorLeaderDutyCycle: StatusSignal - var elevatorFollowerStatorCurrentSignal: StatusSignal - var elevatorFollowerSupplyCurrentSignal: StatusSignal - var elevatorFollowertempSignal: StatusSignal - var elevatorFollowerDutyCycle: StatusSignal - - init { - elevatorLeaderKraken.clearStickyFaults() - elevatorFollowerKraken.clearStickyFaults() - elevatorLeaderConfiguration.Slot0.kP = leaderSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.LEADER_KP) - elevatorLeaderConfiguration.Slot0.kI = leaderSensor.integralVelocityGainToRawUnits(ElevatorConstants.LEADER_KI) - elevatorLeaderConfiguration.Slot0.kD = leaderSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.LEADER_KD) - - elevatorFollowerConfiguration.Slot0.kP = followerSensor.proportionalVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KP) - elevatorFollowerConfiguration.Slot0.kI = followerSensor.integralVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KI) - elevatorFollowerConfiguration.Slot0.kD = followerSensor.derivativeVelocityGainToRawUnits(ElevatorConstants.FOLLOWER_KD) - - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds - elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes - elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds - elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true - elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes - elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false - - elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake - elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) - elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) - elevatorLeaderKraken.inverted = true - elevatorFollowerKraken.inverted = false - MotorChecker.add("Elevator", "Extension", MotorCollection(mutableListOf( - Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), (Falcon500(elevatorFollowerKraken, "Follower Extension Motor"))), ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, 30.celsius, ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT-30.amps, 110.celsius)) - elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent - elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent - elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp - elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle - elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent - elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent - elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp - elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle - } - - override fun configPID(kP: ProportionalGain, kI: IntegralGain, kD: DerivativeGain) { - val pidController = Slot0Configs() - pidController.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) - pidController.kI = leaderSensor.integralPositionGainToRawUnits(kI) - pidController.kD = leaderSensor.derivativePositionGainToRawUnits(kD) - } - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - inputs.elevatorPosition = leaderSensor.position - inputs.elevatorVelocity = leaderSensor.velocity - inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts - inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts - inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps - inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps - inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps - inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps - inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius - inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius - inputs.leaderRawPosition = leaderSensor.getRawPosition() - inputs.followerRawPosition = followerSensor.getRawPosition() - } - - override fun setOutputVoltage(voltage: ElectricalPotential) { - elevatorLeaderKraken.setVoltage(voltage.inVolts) - } - - override fun setPosition(position: Length, feedForward: ElectricalPotential) { - elevatorLeaderKraken.setControl(PositionDutyCycle(leaderSensor.positionToRawUnits(position), 0.0, true, feedForward.inVolts, 0, false, false, false)) - } - - override fun zeroEncoder() { - elevatorLeaderKraken.setPosition(0.0) - elevatorFollowerKraken.setPosition(0.0) - } -} \ No newline at end of file +import org.team4099.lib.units.derived.DerivativeGain +import org.team4099.lib.units.derived.ElectricalPotential +import org.team4099.lib.units.derived.IntegralGain +import org.team4099.lib.units.derived.ProportionalGain +import org.team4099.lib.units.derived.Volt +import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.volts + +object ElevatorIOKraken : ElevatorIO { + private val elevatorLeaderKraken: TalonFX = TalonFX(Constants.Elevator.LEADER_MOTOR_ID) + private val elevatorFollowerKraken: TalonFX = TalonFX(Constants.Elevator.FOLLOWER_MOTOR_ID) + private val leaderSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.SENSOR_CPR, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val followerSensor = + ctreLinearMechanismSensor( + elevatorLeaderKraken, + ElevatorConstants.SENSOR_CPR, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + private val elevatorLeaderConfiguration: TalonFXConfiguration = TalonFXConfiguration() + private val elevatorFollowerConfiguration: TalonFXConfiguration = TalonFXConfiguration() + + var elevatorLeaderStatorCurrentSignal: StatusSignal + var elevatorLeaderSupplyCurrentSignal: StatusSignal + var elevatorLeadertempSignal: StatusSignal + var elevatorLeaderDutyCycle: StatusSignal + var elevatorFollowerStatorCurrentSignal: StatusSignal + var elevatorFollowerSupplyCurrentSignal: StatusSignal + var elevatorFollowertempSignal: StatusSignal + var elevatorFollowerDutyCycle: StatusSignal + + init { + elevatorLeaderKraken.clearStickyFaults() + elevatorFollowerKraken.clearStickyFaults() + elevatorLeaderConfiguration.Slot0.kP = + leaderSensor.proportionalPositionGainToRawUnits(ElevatorConstants.LEADER_KP) + elevatorLeaderConfiguration.Slot0.kI = + leaderSensor.integralPositionGainToRawUnits(ElevatorConstants.LEADER_KI) + elevatorLeaderConfiguration.Slot0.kD = + leaderSensor.derivativePositionGainToRawUnits(ElevatorConstants.LEADER_KD) + + elevatorFollowerConfiguration.Slot0.kP = + followerSensor.proportionalPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KP) + elevatorFollowerConfiguration.Slot0.kI = + followerSensor.integralPositionGainToRawUnits(ElevatorConstants.FOLLOWER_KI) + elevatorFollowerConfiguration.Slot0.kD = + followerSensor.derivativePositionGainToRawUnits(ElevatorConstants.FOLLOWER_KD) + + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.LEADER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.LEADER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.LEADER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorLeaderConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes + elevatorLeaderConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimit = + ElevatorConstants.FOLLOWER_SUPPLY_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentThreshold = + ElevatorConstants.FOLLOWER_THRESHOLD_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.SupplyTimeThreshold = + ElevatorConstants.FOLLOWER_SUPPLY_TIME_THRESHOLD.inSeconds + elevatorFollowerConfiguration.CurrentLimits.SupplyCurrentLimitEnable = true + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimit = + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes + elevatorFollowerConfiguration.CurrentLimits.StatorCurrentLimitEnable = false + + elevatorLeaderConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorFollowerConfiguration.MotorOutput.NeutralMode = NeutralModeValue.Brake + elevatorLeaderKraken.configurator.apply(elevatorLeaderConfiguration) + elevatorFollowerKraken.configurator.apply(elevatorFollowerConfiguration) + elevatorLeaderKraken.inverted = true + elevatorFollowerKraken.inverted = false + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Falcon500(elevatorLeaderKraken, "Leader Extension Motor"), + (Falcon500(elevatorFollowerKraken, "Follower Extension Motor")) + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 110.celsius + ) + ) + elevatorLeaderStatorCurrentSignal = elevatorLeaderKraken.statorCurrent + elevatorLeaderSupplyCurrentSignal = elevatorLeaderKraken.supplyCurrent + elevatorLeadertempSignal = elevatorLeaderKraken.deviceTemp + elevatorLeaderDutyCycle = elevatorLeaderKraken.dutyCycle + elevatorFollowerStatorCurrentSignal = elevatorFollowerKraken.statorCurrent + elevatorFollowerSupplyCurrentSignal = elevatorFollowerKraken.supplyCurrent + elevatorFollowertempSignal = elevatorFollowerKraken.deviceTemp + elevatorFollowerDutyCycle = elevatorFollowerKraken.dutyCycle + } + + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + val pidConfiguration = Slot0Configs() + pidConfiguration.kP = leaderSensor.proportionalPositionGainToRawUnits(kP) + pidConfiguration.kI = leaderSensor.integralPositionGainToRawUnits(kI) + pidConfiguration.kD = leaderSensor.derivativePositionGainToRawUnits(kD) + elevatorLeaderKraken.configurator.apply(pidConfiguration) + elevatorFollowerKraken.configurator.apply(pidConfiguration) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + inputs.elevatorVelocity = leaderSensor.velocity + inputs.leaderAppliedVoltage = elevatorLeaderDutyCycle.value.volts + inputs.followerAppliedVoltage = elevatorFollowerDutyCycle.value.volts + inputs.leaderSupplyCurrent = elevatorLeaderSupplyCurrentSignal.value.amps + inputs.leaderStatorCurrent = elevatorLeaderStatorCurrentSignal.value.amps + inputs.followerSupplyCurrent = elevatorFollowerSupplyCurrentSignal.value.amps + inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps + inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius + inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius + inputs.leaderRawPosition = leaderSensor.getRawPosition() + inputs.followerRawPosition = followerSensor.getRawPosition() + } + + override fun setOutputVoltage(voltage: ElectricalPotential) { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } + + override fun setPosition(position: Length, feedForward: ElectricalPotential) { + elevatorLeaderKraken.setControl( + PositionDutyCycle( + leaderSensor.positionToRawUnits(position), + 0.0, + true, + feedForward.inVolts, + 0, + false, + false, + false + ) + ) + } + + override fun zeroEncoder() { + elevatorLeaderKraken.setPosition(0.0) + elevatorFollowerKraken.setPosition(0.0) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 72cded85..7aa8c0c5 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -28,161 +28,173 @@ import kotlin.math.absoluteValue object ElevatorIONEO : ElevatorIO { - private val leaderSparkMax = - CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val leaderSensor = - sparkMaxLinearMechanismSensor( - leaderSparkMax, - ElevatorConstants.GEAR_RATIO, - ElevatorConstants.SPOOL_DIAMETER, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - private val followerSparkMax = - CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - - private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController - - init { - - // reseting motor - leaderSparkMax.restoreFactoryDefaults() - followerSparkMax.restoreFactoryDefaults() - - leaderSparkMax.clearFaults() - followerSparkMax.clearFaults() + private val leaderSparkMax = + CANSparkMax(Constants.Elevator.LEADER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - // basic settings - leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) - followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + private val leaderSensor = + sparkMaxLinearMechanismSensor( + leaderSparkMax, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) - leaderSparkMax.inverted = true + private val followerSparkMax = + CANSparkMax(Constants.Elevator.FOLLOWER_MOTOR_ID, CANSparkMaxLowLevel.MotorType.kBrushless) - leaderSparkMax.setSmartCurrentLimit(ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) - followerSparkMax.setSmartCurrentLimit(ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt()) + private val followerSensor = + sparkMaxLinearMechanismSensor( + followerSparkMax, + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.SPOOL_DIAMETER, + ElevatorConstants.VOLTAGE_COMPENSATION + ) - leaderSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond - followerSparkMax.openLoopRampRate = ElevatorConstants.RAMP_RATE.inPercentOutputPerSecond - - leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake - - // makes follower motor output exact same power as leader - followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) - - leaderPIDController.ff = 0.0 - - leaderSparkMax.burnFlash() - followerSparkMax.burnFlash() - - MotorChecker.add( - "Elevator", - "Extension", - MotorCollection( - mutableListOf( - Neo(leaderSparkMax, "Leader Extension Motor"), - Neo(followerSparkMax, "Follower Extension Motor") - ), - ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, - 30.celsius, - ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, - 90.celsius - ), - ) - } - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - inputs.elevatorPosition = leaderSensor.position - - inputs.elevatorVelocity = leaderSensor.velocity - - // voltage in * percent out - inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput - - inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps - - // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent - // AppliedVoltage = percentOutput * BatteryVoltage - // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = - // percentOutput * statorCurrent - - inputs.leaderSupplyCurrent = - inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue - - inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius - - // voltage in * percent out - inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput - - inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps - - inputs.followerSupplyCurrent = - inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue - - inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius - - inputs.leaderRawPosition = leaderSparkMax.encoder.position - - inputs.followerRawPosition = followerSparkMax.encoder.position - - Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param voltage the voltage to set the motor to - */ - override fun setOutputVoltage(voltage: ElectricalPotential) { - // divide by 2 cause full power elevator is scary - leaderSparkMax.setVoltage(voltage.inVolts) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param position the target position the PID controller will use - * @param feedforward change in voltage to account for external forces on the system - */ - override fun setPosition(position: Length, feedforward: ElectricalPotential) { - leaderPIDController.setReference( - leaderSensor.positionToRawUnits( - clamp( - position, - ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, - ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION - ) - ), - CANSparkMax.ControlType.kPosition, - 0, - feedforward.inVolts, + private val leaderPIDController: SparkMaxPIDController = leaderSparkMax.pidController + private val followerPIDController: SparkMaxPIDController = followerSparkMax.pidController + init { + + // reseting motor + leaderSparkMax.restoreFactoryDefaults() + followerSparkMax.restoreFactoryDefaults() + + leaderSparkMax.clearFaults() + followerSparkMax.clearFaults() + + // basic settings + leaderSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + followerSparkMax.enableVoltageCompensation(ElevatorConstants.VOLTAGE_COMPENSATION.inVolts) + + leaderSparkMax.inverted = true + + leaderSparkMax.setSmartCurrentLimit( + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + followerSparkMax.setSmartCurrentLimit( + ElevatorConstants.FOLLOWER_STATOR_CURRENT_LIMIT.inAmperes.toInt() + ) + + leaderSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + followerSparkMax.idleMode = CANSparkMax.IdleMode.kBrake + + // makes follower motor output exact same power as leader + followerSparkMax.follow(leaderSparkMax, ElevatorConstants.FOLLOWER_INVERTED) + + leaderPIDController.ff = 0.0 + + leaderSparkMax.burnFlash() + followerSparkMax.burnFlash() + + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + Neo(leaderSparkMax, "Leader Extension Motor"), + Neo(followerSparkMax, "Follower Extension Motor") + ), + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT, + 30.celsius, + ElevatorConstants.LEADER_STATOR_CURRENT_LIMIT - 30.amps, + 90.celsius + ), + ) + } + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + inputs.elevatorPosition = leaderSensor.position + + inputs.elevatorVelocity = leaderSensor.velocity + + // voltage in * percent out + inputs.leaderAppliedVoltage = leaderSparkMax.busVoltage.volts * leaderSparkMax.appliedOutput + + inputs.leaderStatorCurrent = leaderSparkMax.outputCurrent.amps + + // BatteryVoltage * SupplyCurrent = percentOutput * BatteryVoltage * StatorCurrent + // AppliedVoltage = percentOutput * BatteryVoltage + // SupplyCurrent = (percentOutput * BatteryVoltage / BatteryVoltage) * StatorCurrent = + // percentOutput * statorCurrent + + inputs.leaderSupplyCurrent = + inputs.leaderStatorCurrent * leaderSparkMax.appliedOutput.absoluteValue + + inputs.leaderTempCelcius = leaderSparkMax.motorTemperature.celsius + + // voltage in * percent out + inputs.followerAppliedVoltage = leaderSparkMax.busVoltage.volts * followerSparkMax.appliedOutput + + inputs.followerStatorCurrent = followerSparkMax.outputCurrent.amps + + inputs.followerSupplyCurrent = + inputs.followerStatorCurrent * followerSparkMax.appliedOutput.absoluteValue + + inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius + + inputs.leaderRawPosition = leaderSparkMax.encoder.position + + inputs.followerRawPosition = followerSparkMax.encoder.position + + Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + // divide by 2 cause full power elevator is scary + leaderSparkMax.setVoltage(voltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + leaderPIDController.setReference( + leaderSensor.positionToRawUnits( + clamp( + position, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_RETRACTION, + ElevatorConstants.ELEVATOR_SOFT_LIMIT_EXTENSION ) - } - - /** set the current encoder position to be the encoders zero value */ - override fun zeroEncoder() { - leaderSparkMax.encoder.position = 0.0 - followerSparkMax.encoder.position = 0.0 - } - - /** - * updates the PID controller values using the sensor measurement for proportional intregral and - * derivative gain multiplied by the 3 PID constants - * - * @param kP a constant which will be used to scale the proportion gain - * @param kI a constant which will be used to scale the integral gain - * @param kD a constant which will be used to scale the derivative gain - */ - override fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) - leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) - leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) - } -} \ No newline at end of file + ), + CANSparkMax.ControlType.kPosition, + 0, + feedforward.inVolts, + ) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + leaderSparkMax.encoder.position = 0.0 + followerSparkMax.encoder.position = 0.0 + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + leaderPIDController.p = leaderSensor.proportionalPositionGainToRawUnits(kP) + leaderPIDController.i = leaderSensor.integralPositionGainToRawUnits(kI) + leaderPIDController.d = leaderSensor.derivativePositionGainToRawUnits(kD) + followerPIDController.p = followerSensor.proportionalPositionGainToRawUnits(kP) + followerPIDController.i = followerSensor.integralPositionGainToRawUnits(kI) + followerPIDController.d = followerSensor.derivativePositionGainToRawUnits(kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index 0acf1792..915c87ac 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -11,7 +11,14 @@ import edu.wpi.first.wpilibj.simulation.BatterySim import edu.wpi.first.wpilibj.simulation.ElevatorSim import edu.wpi.first.wpilibj.simulation.RoboRioSim import org.team4099.lib.controller.PIDController -import org.team4099.lib.units.base.* +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 +import org.team4099.lib.units.base.inKilograms +import org.team4099.lib.units.base.inMeters +import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -23,124 +30,125 @@ import org.team4099.lib.units.perSecond object ElevatorIOSim : ElevatorIO { - val elevatorSim: ElevatorSim = ElevatorSim( - DCMotor.getNEO(2), - ElevatorConstants.GEAR_RATIO, - ElevatorConstants.CARRIAGE_MASS.inKilograms, - ElevatorConstants.SPOOL_DIAMETER.inMeters/2, - ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, - ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, - true, - 0.0 - ) - - init { - MotorChecker.add( - "Elevator", - "Extension", - MotorCollection( - mutableListOf( - SimulatedMotor( - elevatorSim, - "Extension Motor", - ) - ), - baseCurrentLimit = 60.amps, - firstStageTemperatureLimit = 10.celsius, - firstStageCurrentLimit = 45.amps, - motorShutDownThreshold = 20.celsius - ) - ) - } - - private val elevatorController = - PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) - - private var lastAppliedVoltage = 0.0.volts - - override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { - elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) - - inputs.elevatorPosition = elevatorSim.positionMeters.meters - inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond - - inputs.leaderTempCelcius = 0.0.celsius - inputs.leaderStatorCurrent = 0.0.amps - inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 - inputs.leaderAppliedVoltage = lastAppliedVoltage - - inputs.followerTempCelcius = 0.0.celsius - inputs.followerStatorCurrent = 0.0.amps - inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 - inputs.followerAppliedVoltage = lastAppliedVoltage - - inputs.leaderRawPosition = 0.0 - inputs.followerRawPosition = 0.0 - - inputs.isSimulating = true - - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) - ) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param voltage the voltage to set the motor to - */ - override fun setOutputVoltage(voltage: ElectricalPotential) { - val clampedVoltage = - clamp( - voltage, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - lastAppliedVoltage = clampedVoltage - - elevatorSim.setInputVoltage(clampedVoltage.inVolts) - } - - /** - * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed - * limit - * - * @param position the target position the PID controller will use - * @param feedforward change in voltage to account for external forces on the system - */ - override fun setPosition(position: Length, feedforward: ElectricalPotential) { - val ff = - clamp( - feedforward, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) - - lastAppliedVoltage = ff + feedback - elevatorSim.setInputVoltage((ff + feedback).inVolts) - } - - /** set the current encoder position to be the encoders zero value */ - override fun zeroEncoder() { - println("don't work right now") - } - - /** - * updates the PID controller values using the sensor measurement for proportional intregral and - * derivative gain multiplied by the 3 PID constants - * - * @param kP a constant which will be used to scale the proportion gain - * @param kI a constant which will be used to scale the integral gain - * @param kD a constant which will be used to scale the derivative gain - */ - override fun configPID( - kP: ProportionalGain, - kI: IntegralGain, - kD: DerivativeGain - ) { - elevatorController.setPID(kP, kI, kD) - } -} \ No newline at end of file + val elevatorSim: ElevatorSim = + ElevatorSim( + DCMotor.getNEO(2), + ElevatorConstants.GEAR_RATIO, + ElevatorConstants.CARRIAGE_MASS.inKilograms, + ElevatorConstants.SPOOL_DIAMETER.inMeters / 2, + ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, + ElevatorConstants.ELEVATOR_MAX_EXTENSION.inMeters, + true, + 0.0 + ) + + init { + MotorChecker.add( + "Elevator", + "Extension", + MotorCollection( + mutableListOf( + SimulatedMotor( + elevatorSim, + "Extension Motor", + ) + ), + baseCurrentLimit = 60.amps, + firstStageTemperatureLimit = 10.celsius, + firstStageCurrentLimit = 45.amps, + motorShutDownThreshold = 20.celsius + ) + ) + } + + private val elevatorController = + PIDController(ElevatorConstants.SIM_KP, ElevatorConstants.SIM_KI, ElevatorConstants.SIM_KD) + + private var lastAppliedVoltage = 0.0.volts + + override fun updateInputs(inputs: ElevatorIO.ElevatorInputs) { + elevatorSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds) + + inputs.elevatorPosition = elevatorSim.positionMeters.meters + inputs.elevatorVelocity = elevatorSim.velocityMetersPerSecond.meters.perSecond + + inputs.leaderTempCelcius = 0.0.celsius + inputs.leaderStatorCurrent = 0.0.amps + inputs.leaderSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.leaderAppliedVoltage = lastAppliedVoltage + + inputs.followerTempCelcius = 0.0.celsius + inputs.followerStatorCurrent = 0.0.amps + inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 + inputs.followerAppliedVoltage = lastAppliedVoltage + + inputs.leaderRawPosition = 0.0 + inputs.followerRawPosition = 0.0 + + inputs.isSimulating = true + + RoboRioSim.setVInVoltage( + BatterySim.calculateDefaultBatteryLoadedVoltage(elevatorSim.currentDrawAmps) + ) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param voltage the voltage to set the motor to + */ + override fun setOutputVoltage(voltage: ElectricalPotential) { + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } + + /** + * Sets the voltage of the elevator motors but also checks to make sure elevator doesn't exceed + * limit + * + * @param position the target position the PID controller will use + * @param feedforward change in voltage to account for external forces on the system + */ + override fun setPosition(position: Length, feedforward: ElectricalPotential) { + val ff = + clamp( + feedforward, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) + + lastAppliedVoltage = ff + feedback + elevatorSim.setInputVoltage((ff + feedback).inVolts) + } + + /** set the current encoder position to be the encoders zero value */ + override fun zeroEncoder() { + println("don't work right now") + } + + /** + * updates the PID controller values using the sensor measurement for proportional intregral and + * derivative gain multiplied by the 3 PID constants + * + * @param kP a constant which will be used to scale the proportion gain + * @param kI a constant which will be used to scale the integral gain + * @param kD a constant which will be used to scale the derivative gain + */ + override fun configPID( + kP: ProportionalGain, + kI: IntegralGain, + kD: DerivativeGain + ) { + elevatorController.setPID(kP, kI, kD) + } +} diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt index a1e0e8c6..6ae241f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/superstructure/Request.kt @@ -1,24 +1,15 @@ package com.team4099.robot2023.subsystems.superstructure -import com.team4099.robot2023.config.constants.GamePiece -import com.team4099.robot2023.config.constants.NodeTier import edu.wpi.first.math.kinematics.ChassisSpeeds import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.LinearVelocity import org.team4099.lib.units.base.Length -import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.ElectricalPotential -import org.team4099.lib.units.perSecond - sealed interface Request { - - sealed interface ElevatorRequest : Request { - class TargetingPosition( - val position: Length - ) : ElevatorRequest + class TargetingPosition(val position: Length) : ElevatorRequest class OpenLoop(val voltage: ElectricalPotential) : ElevatorRequest class Home() : ElevatorRequest } From cf89177c0eb39be6ae86ae0b229693e83e318cea Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Mon, 22 Jan 2024 19:07:24 -0500 Subject: [PATCH 34/38] fixed more programming issues --- .../robot2023/subsystems/elevator/Elevator.kt | 18 +++++++++++++ .../subsystems/elevator/ElevatorIOKraken.kt | 14 +++++++--- .../subsystems/elevator/ElevatorIONEO.kt | 9 ++++++- .../subsystems/elevator/ElevatorIOSim.kt | 27 ++++++++++--------- 4 files changed, 51 insertions(+), 17 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index e3e10729..19680ea9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -15,6 +15,7 @@ import org.team4099.lib.units.base.Meter import org.team4099.lib.units.base.inInches import org.team4099.lib.units.base.inSeconds import org.team4099.lib.units.base.inches +import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.degrees import org.team4099.lib.units.derived.inDegrees @@ -27,6 +28,7 @@ import org.team4099.lib.units.derived.perInchSeconds import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inInchesPerSecond import org.team4099.lib.units.perSecond +import kotlin.time.Duration.Companion.seconds import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest class Elevator(val io: ElevatorIO) { @@ -49,6 +51,22 @@ class Elevator(val io: ElevatorIO) { LoggedTunableValue( "Elevator/kD", Pair({ it.inVoltsPerInchPerSecond }, { it.volts / 1.0.inches.perSecond }) ) + private val kS = + LoggedTunableValue( + "Elevator/kS", Pair({ it.inVolts}, {it.volts}) + ) + private val kG = + LoggedTunableValue( + "Elevator/kG", Pair({ it.inVolts}, {it.volts}) + ) + private val kV = + LoggedTunableValue( + "Elevator/kG", Pair({it.inVoltsPerInchPerSecond}, {it.volts / 1.0.inches.perSecond}) + ) + private val kA = + LoggedTunableValue( + "Elevator/kA", Pair({it.inVolts.perMetersPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond}) + ) object TunableElevatorHeights { val enableElevator = diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index fd3907d1..c5c97865 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -18,6 +18,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.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.ctreLinearMechanismSensor import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential @@ -156,16 +157,21 @@ object ElevatorIOKraken : ElevatorIO { } override fun setOutputVoltage(voltage: ElectricalPotential) { - elevatorLeaderKraken.setVoltage(voltage.inVolts) + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts))) { + elevatorLeaderKraken.setVoltage(0.0) + } + else { + elevatorLeaderKraken.setVoltage(voltage.inVolts) + } } override fun setPosition(position: Length, feedForward: ElectricalPotential) { elevatorLeaderKraken.setControl( - PositionDutyCycle( + PositionVoltage( leaderSensor.positionToRawUnits(position), - 0.0, true, - feedForward.inVolts, + feedForward, 0, false, false, diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index 7aa8c0c5..d0c9eb43 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -16,6 +16,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.inPercentOutputPerSecond +import org.team4099.lib.units.base.inches import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential import org.team4099.lib.units.derived.IntegralGain @@ -146,7 +147,13 @@ object ElevatorIONEO : ElevatorIO { */ override fun setOutputVoltage(voltage: ElectricalPotential) { // divide by 2 cause full power elevator is scary - leaderSparkMax.setVoltage(voltage.inVolts) + if (((leaderSensor.position < 0.5.inches) && (voltage < 0.volts)) || + (leaderSensor.position > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) + ) { + leaderSparkMax.setVoltage(0.0) + } else { + leaderSparkMax.setVoltage(voltage.inVolts) + } } /** diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index 915c87ac..db706340 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -18,6 +18,7 @@ import org.team4099.lib.units.base.celsius import org.team4099.lib.units.base.inKilograms import org.team4099.lib.units.base.inMeters import org.team4099.lib.units.base.inSeconds +import org.team4099.lib.units.base.inches import org.team4099.lib.units.base.meters import org.team4099.lib.units.derived.DerivativeGain import org.team4099.lib.units.derived.ElectricalPotential @@ -99,16 +100,19 @@ object ElevatorIOSim : ElevatorIO { * @param voltage the voltage to set the motor to */ override fun setOutputVoltage(voltage: ElectricalPotential) { - val clampedVoltage = - clamp( - voltage, - -ElevatorConstants.VOLTAGE_COMPENSATION, - ElevatorConstants.VOLTAGE_COMPENSATION - ) - - lastAppliedVoltage = clampedVoltage - - elevatorSim.setInputVoltage(clampedVoltage.inVolts) + if (!((elevatorSim.positionMeters.meters < 0.5.inches) && (voltage < 0.volts)) && + !(elevatorSim.positionMeters.meters > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) + ) { + val clampedVoltage = + clamp( + voltage, + -ElevatorConstants.VOLTAGE_COMPENSATION, + ElevatorConstants.VOLTAGE_COMPENSATION + ) + lastAppliedVoltage = clampedVoltage + + elevatorSim.setInputVoltage(clampedVoltage.inVolts) + } } /** @@ -127,13 +131,12 @@ object ElevatorIOSim : ElevatorIO { ) val feedback = elevatorController.calculate(elevatorSim.positionMeters.meters, position) - lastAppliedVoltage = ff + feedback + setOutputVoltage(ff + feedback) elevatorSim.setInputVoltage((ff + feedback).inVolts) } /** set the current encoder position to be the encoders zero value */ override fun zeroEncoder() { - println("don't work right now") } /** From 686b95b9f64a8521fd24c4ecbd3f408b2dd87454 Mon Sep 17 00:00:00 2001 From: Aagames1 Date: Mon, 22 Jan 2024 20:27:51 -0500 Subject: [PATCH 35/38] fixed some programming issues --- .../config/constants/ElevatorConstants.kt | 10 +++++++--- .../robot2023/subsystems/elevator/Elevator.kt | 14 ++++++++------ .../robot2023/subsystems/elevator/ElevatorIO.kt | 4 ++-- .../subsystems/elevator/ElevatorIOKraken.kt | 4 +--- 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 5fba2d33..407c8ad1 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -45,13 +45,17 @@ object ElevatorConstants { val ELEVATOR_KG = 0.0.volts val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond + val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts + val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts - val ENABLE_ELEVATOR = 1.0 + val ENABLE_ELEVATOR = false val ELEVATOR_IDLE_HEIGHT = 0.0.inches val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_SAFE_THRESHOLD = 5.0.inches + val ELEVATOR_TOLERANCE = 0.0.inches @@ -68,8 +72,7 @@ object ElevatorConstants { val ELEVATOR_GROUND_OFFSET = 0.0.inches val VOLTAGE_COMPENSATION = 12.0.volts - val GEAR_RATIO = 4.0 / 1 * 4.0 / 1 - val SENSOR_CPR = 0 + val ELEVATOR_PULLEY_TO_MOTOR = 4.0 / 1 * 4.0 / 1 val SPOOL_DIAMETER = 1.5.inches val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps @@ -81,4 +84,5 @@ object ElevatorConstants { val FOLLOWER_STATOR_CURRENT_LIMIT = 0.0.amps val FOLLOWER_SUPPLY_CURRENT_LIMIT = 0.0.amps val FOLLOWER_THRESHOLD_CURRENT_LIMIT = 0.0.amps + } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 19680ea9..1a527112 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -88,11 +88,11 @@ class Elevator(val io: ElevatorIO) { // TODO: change voltages val openLoopExtendVoltage = LoggedTunableValue( - "Elevator/openLoopExtendVoltage", 8.volts, Pair({ it.inVolts }, { it.volts }) + "Elevator/openLoopExtendVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE, Pair({ it.inVolts }, { it.volts }) ) val openLoopRetractVoltage = LoggedTunableValue( - "Elevator/openLoopRetractVoltage", -12.0.volts, Pair({ it.inVolts }, { it.volts }) + "Elevator/openLoopRetractVoltage", ElevatorConstants.ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE, Pair({ it.inVolts }, { it.volts }) ) val shootSpeakerPosition = @@ -187,7 +187,7 @@ class Elevator(val io: ElevatorIO) { get() = currentRequest is ElevatorRequest.TargetingPosition && ( - ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= 5.inches) || + ((inputs.elevatorPosition - elevatorPositionTarget).absoluteValue <= ElevatorConstants.ELEVATOR_SAFE_THRESHOLD) || elevatorProfile.isFinished(Clock.fpgaTime - timeProfileGeneratedAt) ) && lastRequestedPosition == elevatorPositionTarget @@ -208,6 +208,8 @@ class Elevator(val io: ElevatorIO) { kP.initDefault(ElevatorConstants.SIM_KP) kI.initDefault(ElevatorConstants.SIM_KI) kD.initDefault(ElevatorConstants.SIM_KD) + + io.configPID(kP.get(), kI.get(), kD.get()) } elevatorFeedforward = @@ -218,7 +220,7 @@ class Elevator(val io: ElevatorIO) { ElevatorConstants.ELEVATOR_KA ) - io.configPID(kP.get(), kI.get(), kD.get()) + } fun periodic() { @@ -316,7 +318,7 @@ class Elevator(val io: ElevatorIO) { ElevatorConstants.HOMING_STALL_TIME_THRESHOLD ) ) { - setHomeVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + io.setVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) } else { zeroEncoder() isHomed = true @@ -355,7 +357,7 @@ class Elevator(val io: ElevatorIO) { if ((forwardLimitReached) && (setpoint.position > inputs.elevatorPosition) || (reverseLimitReached) && (setpoint.position < inputs.elevatorPosition) ) { - io.setOutputVoltage(0.volts) + io.setPosition(inputs.elevatorPosition, feedForward) } else { io.setPosition(setpoint.position, feedForward) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index e01bd0a4..c832874c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -37,8 +37,8 @@ interface ElevatorIO { var leaderTempCelcius = 0.0.celsius var followerTempCelcius = 0.0.celsius - var leaderRawPosition = 0.0 - var followerRawPosition = 0.0 + var leaderRawPosition = 0.0.rotations + var followerRawPosition = 0.0.rotations var isSimulating = false diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index c5c97865..2af4a0e0 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -34,7 +34,6 @@ object ElevatorIOKraken : ElevatorIO { private val leaderSensor = ctreLinearMechanismSensor( elevatorLeaderKraken, - ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION @@ -42,7 +41,6 @@ object ElevatorIOKraken : ElevatorIO { private val followerSensor = ctreLinearMechanismSensor( elevatorLeaderKraken, - ElevatorConstants.SENSOR_CPR, ElevatorConstants.GEAR_RATIO, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION @@ -169,7 +167,7 @@ object ElevatorIOKraken : ElevatorIO { override fun setPosition(position: Length, feedForward: ElectricalPotential) { elevatorLeaderKraken.setControl( PositionVoltage( - leaderSensor.positionToRawUnits(position), + leaderSensor.getRawPosition(), true, feedForward, 0, From 7c071041850824a24f844161f49b8016c9b1b145 Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Tue, 23 Jan 2024 18:32:44 -0500 Subject: [PATCH 36/38] fixed more problems --- .../robot2023/subsystems/elevator/ElevatorIOKraken.kt | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index 2af4a0e0..bee4ac6d 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -4,9 +4,9 @@ import com.ctre.phoenix6.StatusSignal import com.ctre.phoenix6.configs.Slot0Configs import com.ctre.phoenix6.configs.TalonFXConfiguration import com.ctre.phoenix6.controls.PositionDutyCycle +import com.ctre.phoenix6.controls.PositionVoltage import com.ctre.phoenix6.hardware.TalonFX import com.ctre.phoenix6.signals.NeutralModeValue -import com.team4099.lib.phoenix6.PositionVoltage import com.team4099.robot2023.config.constants.Constants import com.team4099.robot2023.config.constants.ElevatorConstants import com.team4099.robot2023.subsystems.falconspin.Falcon500 @@ -26,6 +26,7 @@ import org.team4099.lib.units.derived.IntegralGain import org.team4099.lib.units.derived.ProportionalGain import org.team4099.lib.units.derived.Volt import org.team4099.lib.units.derived.inVolts +import org.team4099.lib.units.derived.rotations import org.team4099.lib.units.derived.volts object ElevatorIOKraken : ElevatorIO { @@ -34,14 +35,14 @@ object ElevatorIOKraken : ElevatorIO { private val leaderSensor = ctreLinearMechanismSensor( elevatorLeaderKraken, - ElevatorConstants.GEAR_RATIO, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION ) private val followerSensor = ctreLinearMechanismSensor( elevatorLeaderKraken, - ElevatorConstants.GEAR_RATIO, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION ) @@ -168,8 +169,9 @@ object ElevatorIOKraken : ElevatorIO { elevatorLeaderKraken.setControl( PositionVoltage( leaderSensor.getRawPosition(), + 0.0, true, - feedForward, + feedForward.inVolts, 0, false, false, From d075a80c020aa3cd40bc5368148d5a6c2c73458a Mon Sep 17 00:00:00 2001 From: nbhog <146785661+nbhog@users.noreply.github.com> Date: Tue, 23 Jan 2024 20:30:14 -0500 Subject: [PATCH 37/38] tested simulation --- simgui-ds.json | 5 +++++ .../com/team4099/robot2023/BuildConstants.kt | 10 ++++----- .../com/team4099/robot2023/RobotContainer.kt | 2 +- .../commands/drivetrain/GyroAutoLevel.kt | 1 + .../config/constants/ElevatorConstants.kt | 22 +++++++++---------- .../robot2023/subsystems/elevator/Elevator.kt | 17 ++++++++------ .../subsystems/elevator/ElevatorIO.kt | 7 ------ .../subsystems/elevator/ElevatorIOKraken.kt | 2 -- .../subsystems/elevator/ElevatorIONEO.kt | 8 ++----- .../subsystems/elevator/ElevatorIOSim.kt | 11 +++------- vendordeps/PathplannerLib.json | 6 ++--- 11 files changed, 41 insertions(+), 50 deletions(-) diff --git a/simgui-ds.json b/simgui-ds.json index 73cc713c..69b1a3cb 100644 --- a/simgui-ds.json +++ b/simgui-ds.json @@ -88,5 +88,10 @@ "buttonCount": 0, "povCount": 0 } + ], + "robotJoysticks": [ + { + "guid": "Keyboard0" + } ] } diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 4bf29e8a..240d0719 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 = 45 -const val GIT_SHA = "5e1ed64a8159953426ea83a1f210857ac81b7ef8" -const val GIT_DATE = "2024-01-19T22:05:22Z" +const val GIT_REVISION = 52 +const val GIT_SHA = "7c071041850824a24f844161f49b8016c9b1b145" +const val GIT_DATE = "2024-01-23T18:32:44Z" const val GIT_BRANCH = "elevator" -const val BUILD_DATE = "2024-01-19T22:38:32Z" -const val BUILD_UNIX_TIME = 1705721912200L +const val BUILD_DATE = "2024-01-23T20:24:59Z" +const val BUILD_UNIX_TIME = 1706059499671L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt index 60ca3cb5..5d96cd2b 100644 --- a/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt +++ b/src/main/kotlin/com/team4099/robot2023/RobotContainer.kt @@ -137,7 +137,7 @@ object RobotContainer { ControlBoard.elevatorOpenLoopExtend.whileTrue(elevator.testElevatorOpenLoopExtendCommand()) ControlBoard.elevatorOpenLoopRetract.whileTrue(elevator.testElevatorOpenLoopRetractCommand()) ControlBoard.elevatorClosedLoopHigh.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) - ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.testElevatorClosedLoopExtendCommand()) + ControlBoard.elevatorClosedLoopLow.whileTrue(elevator.elevatorClosedLoopRetractCommand()) } fun mapTestControls() {} diff --git a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt index 43260231..bbfff2f6 100644 --- a/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt +++ b/src/main/kotlin/com/team4099/robot2023/commands/drivetrain/GyroAutoLevel.kt @@ -8,6 +8,7 @@ import edu.wpi.first.wpilibj2.command.Command import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ProfiledPIDController import org.team4099.lib.controller.TrapezoidProfile +import org.team4099.lib.units.Fraction import org.team4099.lib.units.Value import org.team4099.lib.units.Velocity import org.team4099.lib.units.base.Meter diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 407c8ad1..60a66d69 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -21,7 +21,7 @@ object ElevatorConstants { val REAL_KI = 0.0.volts / (1.inches * 1.seconds) val REAL_KD = 0.0.volts / (1.inches.perSecond) - val CARRIAGE_MASS = 20.pounds + val CARRIAGE_MASS = 30.892.pounds val ELEVATOR_MAX_RETRACTION = 0.0.inches val ELEVATOR_MAX_EXTENSION = 18.0.inches @@ -37,30 +37,30 @@ object ElevatorConstants { val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) - val SIM_KP = 0.0.volts / 1.inches + val SIM_KP = 2.0.volts / 1.inches val SIM_KI = 0.0.volts / (1.inches * 1.seconds) val SIM_KD = 0.0.volts / (1.inches.perSecond) val ELEVATOR_KS = 0.0.volts - val ELEVATOR_KG = 0.0.volts - val ELEVATOR_KV = 0.0.volts / 0.0.inches.perSecond - val ELEVATOR_KA = 0.0.volts / 0.0.inches.perSecond.perSecond + val ELEVATOR_KG = 0.32.volts + val ELEVATOR_KV = 0.39.volts / 1.inches.perSecond + val ELEVATOR_KA = 0.00083.volts / 1.inches.perSecond.perSecond val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts val ENABLE_ELEVATOR = false val ELEVATOR_IDLE_HEIGHT = 0.0.inches - val ELEVATOR_SOFT_LIMIT_EXTENSION = 0.0.inches - val ELEVATOR_SOFT_LIMIT_RETRACTION = 0.0.inches + val ELEVATOR_SOFT_LIMIT_EXTENSION = 17.5.inches + val ELEVATOR_SOFT_LIMIT_RETRACTION = -1.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_EXTENSION = 0.0.inches val ELEVATOR_OPEN_LOOP_SOFT_LIMIT_RETRACTION = 0.0.inches val ELEVATOR_SAFE_THRESHOLD = 5.0.inches - val ELEVATOR_TOLERANCE = 0.0.inches + val ELEVATOR_TOLERANCE = 0.2.inches - val MAX_VELOCITY = 0.0.meters.perSecond - val MAX_ACCELERATION = 0.0.meters.perSecond.perSecond + val MAX_VELOCITY = 0.82.meters.perSecond + val MAX_ACCELERATION = 0.5.meters.perSecond.perSecond val SHOOT_SPEAKER_POSITION = 0.0.inches val SHOOT_AMP_POSITION = 0.0.inches @@ -73,7 +73,7 @@ object ElevatorConstants { val VOLTAGE_COMPENSATION = 12.0.volts val ELEVATOR_PULLEY_TO_MOTOR = 4.0 / 1 * 4.0 / 1 - val SPOOL_DIAMETER = 1.5.inches + val SPOOL_DIAMETER = 1.591.inches val LEADER_SUPPLY_CURRENT_LIMIT = 0.0.amps val LEADER_THRESHOLD_CURRENT_LIMIT = 0.0.amps diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt index 1a527112..26b64436 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/Elevator.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.config.constants.ElevatorConstants import edu.wpi.first.wpilibj.RobotBase import edu.wpi.first.wpilibj2.command.Command import edu.wpi.first.wpilibj2.command.Commands.runOnce +import edu.wpi.first.wpilibj2.command.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ElevatorFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -23,6 +24,7 @@ import org.team4099.lib.units.derived.inVolts import org.team4099.lib.units.derived.inVoltsPerInch import org.team4099.lib.units.derived.inVoltsPerInchPerSecond import org.team4099.lib.units.derived.inVoltsPerInchSeconds +import org.team4099.lib.units.derived.inVoltsPerMeterPerSecondPerSecond import org.team4099.lib.units.derived.perInch import org.team4099.lib.units.derived.perInchSeconds import org.team4099.lib.units.derived.volts @@ -31,7 +33,7 @@ import org.team4099.lib.units.perSecond import kotlin.time.Duration.Companion.seconds import com.team4099.robot2023.subsystems.superstructure.Request.ElevatorRequest as ElevatorRequest -class Elevator(val io: ElevatorIO) { +class Elevator(val io: ElevatorIO) : SubsystemBase() { val inputs = ElevatorIO.ElevatorInputs() private var elevatorFeedforward: ElevatorFeedforward = ElevatorFeedforward( @@ -65,12 +67,13 @@ class Elevator(val io: ElevatorIO) { ) private val kA = LoggedTunableValue( - "Elevator/kA", Pair({it.inVolts.perMetersPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond}) + "Elevator/kA", Pair({it.inVoltsPerMeterPerSecondPerSecond}, {it.volts / 1.0.meters.perSecond.perSecond}) ) object TunableElevatorHeights { val enableElevator = - LoggedTunableNumber("Elevator/enableMovementElevator", ElevatorConstants.ENABLE_ELEVATOR) + LoggedTunableNumber("Elevator/enableMovementElevator", + if (ElevatorConstants.ENABLE_ELEVATOR) 1.0 else 0.0) val minPosition = LoggedTunableValue( @@ -223,7 +226,7 @@ class Elevator(val io: ElevatorIO) { } - fun periodic() { + override fun periodic() { io.updateInputs(inputs) if ((kP.hasChanged()) || (kI.hasChanged()) || (kD.hasChanged())) { io.configPID(kP.get(), kI.get(), kD.get()) @@ -318,7 +321,7 @@ class Elevator(val io: ElevatorIO) { ElevatorConstants.HOMING_STALL_TIME_THRESHOLD ) ) { - io.setVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) + io.setOutputVoltage(ElevatorConstants.HOMING_APPLIED_VOLTAGE) } else { zeroEncoder() isHomed = true @@ -394,10 +397,10 @@ class Elevator(val io: ElevatorIO) { } fun elevatorClosedLoopRetractCommand(): Command { - return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(12.inches) }) + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) }) } fun testElevatorClosedLoopExtendCommand(): Command { - return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(4.inches) }) + return runOnce({ currentRequest = ElevatorRequest.TargetingPosition(16.inches) }) } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt index c832874c..a5e20665 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIO.kt @@ -37,9 +37,6 @@ interface ElevatorIO { var leaderTempCelcius = 0.0.celsius var followerTempCelcius = 0.0.celsius - var leaderRawPosition = 0.0.rotations - var followerRawPosition = 0.0.rotations - var isSimulating = false override fun toLog(table: LogTable) { @@ -53,8 +50,6 @@ interface ElevatorIO { table?.put("elevatorFollowerSupplyCurrentAmps", followerSupplyCurrent.inAmperes) table?.put("elevatorLeaderTempCelsius", leaderTempCelcius.inCelsius) table?.put("elevatorFollowerTempCelsius", followerTempCelcius.inCelsius) - table?.put("elevatorLeaderRawPosition", leaderRawPosition) - table?.put("elevatorFollowRawPosition", followerRawPosition) } override fun fromLog(table: LogTable?) { @@ -92,8 +87,6 @@ interface ElevatorIO { table?.get("elevatorFollowerTempCelcius", followerTempCelcius.inCelsius)?.let { followerTempCelcius = it.celsius } - table?.get("elevatorLeaderRawPosition", leaderRawPosition)?.let { leaderRawPosition = it } - table?.get("elevatorFollowerRawPosition", leaderRawPosition)?.let { followerRawPosition = it } } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt index bee4ac6d..16b8bd07 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOKraken.kt @@ -151,8 +151,6 @@ object ElevatorIOKraken : ElevatorIO { inputs.followerStatorCurrent = elevatorFollowerStatorCurrentSignal.value.amps inputs.leaderTempCelcius = elevatorLeadertempSignal.value.celsius inputs.followerTempCelcius = elevatorFollowertempSignal.value.celsius - inputs.leaderRawPosition = leaderSensor.getRawPosition() - inputs.followerRawPosition = followerSensor.getRawPosition() } override fun setOutputVoltage(voltage: ElectricalPotential) { diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt index d0c9eb43..57e88ff9 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIONEO.kt @@ -35,7 +35,7 @@ object ElevatorIONEO : ElevatorIO { private val leaderSensor = sparkMaxLinearMechanismSensor( leaderSparkMax, - ElevatorConstants.GEAR_RATIO, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION ) @@ -46,7 +46,7 @@ object ElevatorIONEO : ElevatorIO { private val followerSensor = sparkMaxLinearMechanismSensor( followerSparkMax, - ElevatorConstants.GEAR_RATIO, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, ElevatorConstants.SPOOL_DIAMETER, ElevatorConstants.VOLTAGE_COMPENSATION ) @@ -132,10 +132,6 @@ object ElevatorIONEO : ElevatorIO { inputs.followerTempCelcius = followerSparkMax.motorTemperature.celsius - inputs.leaderRawPosition = leaderSparkMax.encoder.position - - inputs.followerRawPosition = followerSparkMax.encoder.position - Logger.recordOutput("Elevator/leaderRawRotations", leaderSparkMax.encoder.position) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt index db706340..d81e1d26 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/elevator/ElevatorIOSim.kt @@ -10,6 +10,7 @@ import edu.wpi.first.math.system.plant.DCMotor import edu.wpi.first.wpilibj.simulation.BatterySim import edu.wpi.first.wpilibj.simulation.ElevatorSim import edu.wpi.first.wpilibj.simulation.RoboRioSim +import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.PIDController import org.team4099.lib.units.base.Length import org.team4099.lib.units.base.Meter @@ -34,7 +35,7 @@ object ElevatorIOSim : ElevatorIO { val elevatorSim: ElevatorSim = ElevatorSim( DCMotor.getNEO(2), - ElevatorConstants.GEAR_RATIO, + ElevatorConstants.ELEVATOR_PULLEY_TO_MOTOR, ElevatorConstants.CARRIAGE_MASS.inKilograms, ElevatorConstants.SPOOL_DIAMETER.inMeters / 2, ElevatorConstants.ELEVATOR_MAX_RETRACTION.inMeters, @@ -83,9 +84,6 @@ object ElevatorIOSim : ElevatorIO { inputs.followerSupplyCurrent = elevatorSim.currentDrawAmps.amps / 2 inputs.followerAppliedVoltage = lastAppliedVoltage - inputs.leaderRawPosition = 0.0 - inputs.followerRawPosition = 0.0 - inputs.isSimulating = true RoboRioSim.setVInVoltage( @@ -100,9 +98,7 @@ object ElevatorIOSim : ElevatorIO { * @param voltage the voltage to set the motor to */ override fun setOutputVoltage(voltage: ElectricalPotential) { - if (!((elevatorSim.positionMeters.meters < 0.5.inches) && (voltage < 0.volts)) && - !(elevatorSim.positionMeters.meters > ElevatorConstants.ELEVATOR_MAX_EXTENSION - 0.5.inches && (voltage > 0.volts)) - ) { + Logger.recordOutput("Elevator/OutputTest", voltage) val clampedVoltage = clamp( voltage, @@ -112,7 +108,6 @@ object ElevatorIOSim : ElevatorIO { lastAppliedVoltage = clampedVoltage elevatorSim.setInputVoltage(clampedVoltage.inVolts) - } } /** diff --git a/vendordeps/PathplannerLib.json b/vendordeps/PathplannerLib.json index eb271997..0bf11fbf 100644 --- a/vendordeps/PathplannerLib.json +++ b/vendordeps/PathplannerLib.json @@ -1,7 +1,7 @@ { "fileName": "PathplannerLib.json", "name": "PathplannerLib", - "version": "2024.0.0-beta-6", + "version": "2024.1.4", "uuid": "1b42324f-17c6-4875-8e77-1c312bc8c786", "frcYear": "2024", "mavenUrls": [ @@ -12,7 +12,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-java", - "version": "2024.0.0-beta-6" + "version": "2024.1.4" } ], "jniDependencies": [], @@ -20,7 +20,7 @@ { "groupId": "com.pathplanner.lib", "artifactId": "PathplannerLib-cpp", - "version": "2024.0.0-beta-6", + "version": "2024.1.4", "libName": "PathplannerLib", "headerClassifier": "headers", "sharedLibrary": false, From 308fc8dd72bd4cbd7ab749defdf361a469bdc43b Mon Sep 17 00:00:00 2001 From: Matthew Choulas Date: Tue, 23 Jan 2024 20:55:04 -0500 Subject: [PATCH 38/38] bit more tuning --- .../kotlin/com/team4099/robot2023/BuildConstants.kt | 10 +++++----- .../robot2023/config/constants/ElevatorConstants.kt | 8 ++++---- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 240d0719..fab5921e 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 = 52 -const val GIT_SHA = "7c071041850824a24f844161f49b8016c9b1b145" -const val GIT_DATE = "2024-01-23T18:32:44Z" +const val GIT_REVISION = 53 +const val GIT_SHA = "d075a80c020aa3cd40bc5368148d5a6c2c73458a" +const val GIT_DATE = "2024-01-23T20:30:14Z" const val GIT_BRANCH = "elevator" -const val BUILD_DATE = "2024-01-23T20:24:59Z" -const val BUILD_UNIX_TIME = 1706059499671L +const val BUILD_DATE = "2024-01-23T20:50:22Z" +const val BUILD_UNIX_TIME = 1706061022253L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt index 60a66d69..071adc8d 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/ElevatorConstants.kt @@ -37,18 +37,18 @@ object ElevatorConstants { val FOLLOWER_KI: IntegralGain = 0.0.volts / (1.inches * 1.seconds) val FOLLOWER_KD: DerivativeGain = 0.0.volts / (1.inches.perSecond) - val SIM_KP = 2.0.volts / 1.inches + val SIM_KP = 3.0.volts / 1.inches val SIM_KI = 0.0.volts / (1.inches * 1.seconds) val SIM_KD = 0.0.volts / (1.inches.perSecond) val ELEVATOR_KS = 0.0.volts - val ELEVATOR_KG = 0.32.volts + val ELEVATOR_KG = 0.35.volts val ELEVATOR_KV = 0.39.volts / 1.inches.perSecond val ELEVATOR_KA = 0.00083.volts / 1.inches.perSecond.perSecond val ELEVATOR_OPEN_LOOP_EXTEND_VOLTAGE = 8.0.volts val ELEVATOR_OPEN_LOOP_RETRACT_VOLTAGE = -12.0.volts - val ENABLE_ELEVATOR = false + val ENABLE_ELEVATOR = true val ELEVATOR_IDLE_HEIGHT = 0.0.inches val ELEVATOR_SOFT_LIMIT_EXTENSION = 17.5.inches val ELEVATOR_SOFT_LIMIT_RETRACTION = -1.0.inches @@ -60,7 +60,7 @@ object ElevatorConstants { val ELEVATOR_TOLERANCE = 0.2.inches val MAX_VELOCITY = 0.82.meters.perSecond - val MAX_ACCELERATION = 0.5.meters.perSecond.perSecond + val MAX_ACCELERATION = 2.meters.perSecond.perSecond val SHOOT_SPEAKER_POSITION = 0.0.inches val SHOOT_AMP_POSITION = 0.0.inches