configuring wrist sim pid (what a w)

simgui-ds.json

@@ -90,6 +90,7 @@
     }
   ],
   "robotJoysticks": [
+    {},
     {
       "guid": "Keyboard0"
     }

simgui.json

@@ -5,5 +5,18 @@
       "/Shuffleboard/Pre-match/Mode": "String Chooser",
       "/SmartDashboard/AutonomousMode": "String Chooser"
     }
+  },
+  "NetworkTables": {
+    "transitory": {
+      "SmartDashboard": {
+        "TunableNumbers": {
+          "Wrist": {
+            "open": true
+          },
+          "open": true
+        },
+        "open": true
+      }
+    }
   }
 }

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 = 89
-const val GIT_SHA = "52fe581ede50f7c3ba6b2715326845f25f545315"
-const val GIT_DATE = "2024-01-23T19:27:19Z"
+const val GIT_REVISION = 90
+const val GIT_SHA = "33de50baa842e9ae87bdd2787c2090daf7b92813"
+const val GIT_DATE = "2024-01-24T20:31:21Z"
 const val GIT_BRANCH = "shooter"
-const val BUILD_DATE = "2024-01-24T20:28:32Z"
-const val BUILD_UNIX_TIME = 1706146112215L
+const val BUILD_DATE = "2024-01-25T20:03:18Z"
+const val BUILD_UNIX_TIME = 1706230998972L
 const val DIRTY = 1

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

@@ -25,6 +25,7 @@ import org.team4099.lib.smoothDeadband
 import org.team4099.lib.units.derived.Angle
 import org.team4099.lib.units.derived.degrees
 import com.team4099.robot2023.subsystems.superstructure.Request.DrivetrainRequest as DrivetrainRequest
+import com.team4099.robot2023.commands.wrist.WristPositioningCommand
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -145,9 +146,10 @@ object RobotContainer {
     ControlBoard.spinUpFlywheel.whileTrue(flywheel.flywheelSpinUpCommand())
     ControlBoard.openLoopFlywheel.whileTrue(flywheel.flywheelOpenLoopCommand())
 
-    ControlBoard.resetWrist.whileTrue(wrist.wristResetCommand())
+    //ControlBoard.resetWrist.whileTrue(wrist.wristResetCommand())
     ControlBoard.spinUpWrist.whileTrue(wrist.wristPositionCommand())
     ControlBoard.openLoopWrist.whileTrue(wrist.wristOpenLoopCommand())
+    ControlBoard.wristPID.whileTrue(WristPositioningCommand(wrist))
   }
 
   fun mapTestControls() {}

src/main/kotlin/com/team4099/robot2023/commands/wrist/WristPositioningCommand.kt

@@ -0,0 +1,27 @@
+package com.team4099.robot2023.commands.wrist
+
+import edu.wpi.first.wpilibj2.command.Command
+import com.team4099.robot2023.subsystems.wrist.Wrist
+import com.team4099.robot2023.subsystems.wrist.WristIOSim.setWristPosition
+import org.littletonrobotics.junction.Logger
+import org.team4099.lib.units.derived.degrees
+import org.team4099.lib.units.derived.volts
+
+class WristPositioningCommand(wrist: Wrist) : Command()  {
+    init {
+        addRequirements(wrist)
+    }
+
+    override fun initialize() {
+        setWristPosition(45.degrees, 5.volts)
+    }
+
+    override fun execute() {
+        Logger.recordOutput("ActiveCommands/WristPositioningCommand", true)
+    }
+
+    override fun isFinished(): Boolean {
+        setWristPosition(0.degrees, 5.volts)
+        return false
+    }
+}

src/main/kotlin/com/team4099/robot2023/config/ControlBoard.kt

@@ -104,9 +104,11 @@ object ControlBoard {
 
   val spinUpFlywheel = Trigger { driver.xButton }
 
-  val resetWrist = Trigger { operator.aButton }
+  //val resetWrist = Trigger { operator.aButton }
 
   val openLoopWrist = Trigger { operator.bButton }
 
   val spinUpWrist = Trigger { operator.xButton }
+
+  val wristPID = Trigger { operator.aButton }
 }

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

@@ -43,9 +43,9 @@ object WristConstants {
     val REAL_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
     val REAL_KD: DerivativeGain<Radian, Volt> = 0.0.volts / (1.0.rotations / 1.0.seconds)
 
-    val SIM_KP: ProportionalGain<Radian, Volt> = 0.001.volts / 1.0.degrees
+    val SIM_KP: ProportionalGain<Radian, Volt> = 0.091250.volts / 1.0.degrees
     val SIM_KI: IntegralGain<Radian, Volt> = 0.0.volts / (1.0.degrees * 1.0.seconds)
-    val SIM_KD: DerivativeGain<Radian, Volt> = 0.0.volts / (1.0.rotations / 1.0.seconds)
+    val SIM_KD: DerivativeGain<Radian, Volt> = 0.05.volts / (1.0.rotations / 1.0.seconds)
 
     val WRIST_KG = 0.65.volts
     val WRIST_KV = 1.61.volts / 1.0.degrees.perSecond

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