Remove left flywheel motor and added logger values to periodic in fly…

…wheel

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 = 86
-const val GIT_SHA = "c4213047cf865f51a94aec532ae4b5e5ab6465da"
-const val GIT_DATE = "2024-01-21T04:31:29Z"
-const val GIT_BRANCH = "shooter"
-const val BUILD_DATE = "2024-01-21T04:32:08Z"
-const val BUILD_UNIX_TIME = 1705829528858L
+const val GIT_REVISION = 88
+const val GIT_SHA = "b165996f5456e4e0ce9ce385825d05becb33dc7b"
+const val GIT_DATE = "2024-01-21T04:36:39Z"
+const val GIT_BRANCH = "HEAD"
+const val BUILD_DATE = "2024-01-22T19:59:20Z"
+const val BUILD_UNIX_TIME = 1705971560513L
 const val DIRTY = 1

src/main/kotlin/com/team4099/robot2023/RobotContainer.kt

@@ -68,6 +68,7 @@ object RobotContainer {
 
     vision.setDataInterfaces({ drivetrain.odometryPose }, { drivetrain.addVisionData(it) })
     limelight.poseSupplier = { drivetrain.odometryPose }
+
   }
 
   fun mapDefaultCommands() {

src/main/kotlin/com/team4099/robot2023/config/constants/FlywheelConstants.kt

@@ -37,6 +37,8 @@ object FlywheelConstants {
   val LEFT_flywheel_TRIGGER_THRESHOLD_TIME = 10.0.seconds
   val LEFT_FLYWHEEL_STATOR_CURRENT_LIMIT = 50.0.amps
 
+  val FLYWHEEL_VELOCITY_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> =

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,8 @@ 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() {
+  //right flywheel is the only motor
   private val kP =
     LoggedTunableValue(
       "Flywheel/kP",
@@ -71,6 +76,14 @@ class Flywheel(val io: FlywheelIO) {
 
   var flywheelTargetVelocity: AngularVelocity = 0.rotations.perMinute
 
+  val isAtTargetedPosition: Boolean
+    get() =
+      (
+              currentState == Flywheel.Companion.FlywheelStates.TARGETING_VELOCITY &&
+                      (inputs.rightFlywheelVelocity - flywheelTargetVelocity).absoluteValue <=
+                      FlywheelConstants.FLYWHEEL_VELOCITY_TOLERANCE
+              )
+
   var currentState = Companion.FlywheelStates.UNINITIALIZED
 
   var currentRequest: Request.FlywheelRequest = Request.FlywheelRequest.OpenLoop(0.0.volts)
@@ -105,7 +118,7 @@ class Flywheel(val io: FlywheelIO) {
         FlywheelConstants.PID.FLYWHEEL_KA
       )
   }
-  fun periodic() {
+  override fun periodic() {
     io.updateInputs(inputs)
     if (kP.hasChanged() || kI.hasChanged() || kD.hasChanged()) {
       io.configPID(kP.get(), kI.get(), kD.get())
@@ -115,6 +128,13 @@ class Flywheel(val io: FlywheelIO) {
       flywheelFeedForward =
         SimpleMotorFeedforward(flywheelkS.get(), flywheelkV.get(), flywheelkA.get())
     }
+    Logger.processInputs("Flywheel", inputs)
+
+    Logger.recordOutput("Flywheel/currentState", currentState.name)
+
+    Logger.recordOutput("Flywheel/requestedState", currentRequest.javaClass.simpleName)
+
+    Logger.recordOutput("Flywheel/isAtTargetedVelocity", isAtTargetedPosition)
 
     var nextState = currentState
     when (currentState) {

src/main/kotlin/com/team4099/robot2023/subsystems/flywheel/FlywheelIO.kt

@@ -45,11 +45,7 @@ interface FlywheelIO {
       table.put("flywheelRightSupplyCurrent", rightFlywheelSupplyCurrent.inAmperes)
       table.put("flywheelRightTemperature", rightFlywheelTemperature.inCelsius)
 
-      table.put("flywheelLeftVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond)
-      table.put("flywheelLeftAppliedVoltage", leftFlywheelAppliedVoltage.inVolts)
-      table.put("flywheelLeftStatorCurrent", leftFlywheelStatorCurrent.inAmperes)
-      table.put("flywheelLeftSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes)
-      table.put("flywheelLeftTemperature", leftFlywheelTemperature.inCelsius)
+
     }
 
     override fun fromLog(table: LogTable) {
@@ -69,22 +65,6 @@ interface FlywheelIO {
       table.get("rightFlywheelTempreature", rightFlywheelTemperature.inCelsius).let {
         rightFlywheelTemperature = it.celsius
       }
-
-      // LeFt motor
-      table.get("leftFlywheelVelocityRPM", leftFlywheelVelocity.inRadiansPerSecond).let {
-        leftFlywheelVelocity = it.radians.perSecond
-      }
-      table.get("leftFlywheelAppliedVoltage", leftFlywheelAppliedVoltage.inVolts).let {
-        leftFlywheelAppliedVoltage = it.volts
-      }
-      table.get("leftFlywheelStatorCurrent", leftFlywheelStatorCurrent.inAmperes).let {
-        leftFlywheelStatorCurrent = it.amps
-      }
-      table.get("leftFlywheelSupplyCurrent", leftFlywheelSupplyCurrent.inAmperes).let {
-        leftFlywheelSupplyCurrent = it.amps
-      }
-      table.get("leftFlywheelTempreature", leftFlywheelTemperature.inCelsius).let {
-        leftFlywheelTemperature = it.celsius
       }
     }
   }

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 =
@@ -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())