From ce61989bf976b7883412353f9962442edc38162f Mon Sep 17 00:00:00 2001 From: SirBeans <146898838+SirBeans@users.noreply.github.com> Date: Tue, 23 Jan 2024 19:18:33 -0500 Subject: [PATCH] Makes flywheel and wrist extend subsystemBase and puts logger values in flywheel periodic --- .../com/team4099/robot2023/BuildConstants.kt | 12 ++++----- .../config/constants/FlywheelConstants.kt | 2 ++ .../robot2023/subsystems/flywheel/Flywheel.kt | 25 ++++++++++++++++--- .../robot2023/subsystems/wrist/Wrist.kt | 7 +++--- 4 files changed, 34 insertions(+), 12 deletions(-) diff --git a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt index 964ffad7..c5b9408c 100644 --- a/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/BuildConstants.kt @@ -4,12 +4,12 @@ package com.team4099.robot2023 * Automatically generated file containing build version information. */ const val MAVEN_GROUP = "" -const val MAVEN_NAME = "Crescendo-2024" +const val MAVEN_NAME = "Crescendo-2024v2" const val VERSION = "unspecified" -const val GIT_REVISION = 86 -const val GIT_SHA = "c4213047cf865f51a94aec532ae4b5e5ab6465da" -const val GIT_DATE = "2024-01-21T04:31:29Z" +const val GIT_REVISION = 88 +const val GIT_SHA = "b165996f5456e4e0ce9ce385825d05becb33dc7b" +const val GIT_DATE = "2024-01-21T04:36:39Z" const val GIT_BRANCH = "shooter" -const val BUILD_DATE = "2024-01-21T04:32:08Z" -const val BUILD_UNIX_TIME = 1705829528858L +const val BUILD_DATE = "2024-01-23T19:04:55Z" +const val BUILD_UNIX_TIME = 1706054695960L const val DIRTY = 1 diff --git a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt index 678ae65e..de408e90 100644 --- a/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt +++ b/src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt @@ -37,6 +37,7 @@ object FlywheelConstants { val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps + val FLYWHEEL_TOLERANCE = 50.0.rotations.perMinute object PID { val REAL_KP: ProportionalGain, Volt> = 0.001.volts / 1.0.rotations.perMinute val REAL_KI: IntegralGain, Volt> = @@ -55,5 +56,6 @@ object FlywheelConstants { val FLYWHEEL_KA = 0.01.volts / 1.rotations.perMinute.perSecond val FLYWHEEL_INIT_VOLTAGE = 0.0.volts + } } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt index ab8fe106..06bafede 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/Flywheel.kt @@ -3,11 +3,15 @@ package com.team4099.robot2023.subsystems.flywheel import com.team4099.lib.hal.Clock import com.team4099.lib.logging.LoggedTunableValue import com.team4099.robot2023.config.constants.FlywheelConstants +import com.team4099.robot2023.config.constants.WristConstants import com.team4099.robot2023.subsystems.superstructure.Request +import com.team4099.robot2023.subsystems.wrist.Wrist import com.team4099.robot2023.subsystems.wrist.Wrist.Companion.fromRequestToState 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.SimpleMotorFeedforward import org.team4099.lib.units.AngularVelocity import org.team4099.lib.units.base.seconds @@ -23,7 +27,7 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.perMinute import org.team4099.lib.units.perSecond -class Flywheel(val io: FlywheelIO) { +class Flywheel(val io: FlywheelIO): SubsystemBase() { private val kP = LoggedTunableValue( "Flywheel/kP", @@ -105,7 +109,16 @@ class Flywheel(val io: FlywheelIO) { FlywheelConstants.PID.FLYWHEEL_KA ) } - fun periodic() { + override fun periodic() { + + Logger.processInputs("Flywheel", inputs) + + Logger.recordOutput("Flywheel/currentState", currentState.name) + + Logger.recordOutput("Flywheel/requestedState", currentRequest.javaClass.simpleName) + + Logger.recordOutput("Flywheel/isAtTargetedPosition", isAtTargetedVelocity) + io.updateInputs(inputs) if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) { io.configPID(kP.get(), kI.get(), kD.get()) @@ -134,7 +147,13 @@ class Flywheel(val io: FlywheelIO) { } } } - + val isAtTargetedVelocity: Boolean + get() = + ( + currentState == FlywheelStates.TARGETING_VELOCITY && + (inputs.rightFlywheelVelocity - flywheelTargetVelocity).absoluteValue <= + FlywheelConstants.FLYWHEEL_TOLERANCE + ) fun setFlywheelVoltage(appliedVoltage: ElectricalPotential) { io.setFlywheelVoltage(appliedVoltage) } diff --git a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt index 2c0e7a7b..dbb61b9c 100644 --- a/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt +++ b/src/main/kotlin/com/team4099/robot2023/subsystems/wrist/Wrist.kt @@ -8,6 +8,7 @@ import com.team4099.robot2023.subsystems.superstructure.Request 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.SubsystemBase import org.littletonrobotics.junction.Logger import org.team4099.lib.controller.ArmFeedforward import org.team4099.lib.controller.TrapezoidProfile @@ -32,7 +33,7 @@ import org.team4099.lib.units.derived.volts import org.team4099.lib.units.inDegreesPerSecond import org.team4099.lib.units.perSecond -class Wrist(val io: WristIO) { +class Wrist(val io: WristIO):SubsystemBase() { val inputs = WristIO.WristIOInputs() private val wristkS = @@ -66,7 +67,7 @@ class Wrist(val io: WristIO) { ) private val wristkD = LoggedTunableValue( - "wrist/kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) + "Wrist/kD", Pair({ it.inVoltsPerDegreePerSecond }, { it.volts.perDegreePerSecond }) ) var currentRequest: Request.WristRequest = Request.WristRequest.Zero() @@ -136,7 +137,7 @@ class Wrist(val io: WristIO) { ) } - fun periodic() { + override fun periodic() { io.updateInputs(inputs) if (wristkP.hasChanged() || wristkI.hasChanged() || wristkD.hasChanged()) { io.configPID(wristkP.get(), wristkI.get(), wristkD.get())