Makes flywheel and wrist extend subsystemBase and puts logger values …

…in flywheel periodic
team4099 · Jan 24, 2024 · ce61989 · ce61989
1 parent b165996
commit ce61989
Show file tree

Hide file tree

Showing 4 changed files with 34 additions and 12 deletions.
diff --git 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
@@ -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<Velocity<Radian>, Volt> = 0.001.volts / 1.0.rotations.perMinute
     val REAL_KI: IntegralGain<Velocity<Radian>, 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
@@ -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
@@ -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())