trying to fix applied voltage

also fixed some constants

simgui.json

@@ -1,4 +1,11 @@
 {
+  "HALProvider": {
+    "Other Devices": {
+      "window": {
+        "visible": false
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",

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 = 90
-const val GIT_SHA = "33de50baa842e9ae87bdd2787c2090daf7b92813"
-const val GIT_DATE = "2024-01-24T20:31:21Z"
+const val GIT_REVISION = 91
+const val GIT_SHA = "c0c4d2a27924aee8076363b091858c762200b7ca"
+const val GIT_DATE = "2024-01-25T20:05:57Z"
 const val GIT_BRANCH = "shooter"
-const val BUILD_DATE = "2024-01-25T20:03:18Z"
-const val BUILD_UNIX_TIME = 1706230998972L
+const val BUILD_DATE = "2024-01-26T19:58:12Z"
+const val BUILD_UNIX_TIME = 1706317092819L
 const val DIRTY = 1

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

@@ -26,6 +26,7 @@ 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
+import com.team4099.robot2023.commands.wrist.WristResetCommand
 
 object RobotContainer {
   private val drivetrain: Drivetrain
@@ -147,9 +148,10 @@ object RobotContainer {
     ControlBoard.openLoopFlywheel.whileTrue(flywheel.flywheelOpenLoopCommand())
 
     //ControlBoard.resetWrist.whileTrue(wrist.wristResetCommand())
-    ControlBoard.spinUpWrist.whileTrue(wrist.wristPositionCommand())
-    ControlBoard.openLoopWrist.whileTrue(wrist.wristOpenLoopCommand())
-    ControlBoard.wristPID.whileTrue(WristPositioningCommand(wrist))
+    ControlBoard.spinUpWrist.whileTrue(wrist.wristPositionCommand()) //matthew's pos cmd
+    //ControlBoard.openLoopWrist.whileTrue(wrist.wristOpenLoopCommand())
+    ControlBoard.wristReset.whileTrue(WristResetCommand(wrist)) //matthew's reset cmd
+    ControlBoard.wristPID.whileTrue(WristPositioningCommand(wrist)) //cmd made thursday by nathan
   }
 
   fun mapTestControls() {}

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

@@ -21,7 +21,6 @@ class WristPositioningCommand(wrist: Wrist) : Command()  {
     }
 
     override fun isFinished(): Boolean {
-        setWristPosition(0.degrees, 5.volts)
-        return false
+        return true
     }
 }

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

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

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

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

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

@@ -34,18 +34,18 @@ object WristConstants {
   val WRIST_STATOR_CURRENT_LIMIT = 40.0.amps
 
   val WRIST_MAX_ROTATION = 120.0.degrees
-  val WRIST_MIN_ROTATION = -60.0.degrees
+  val WRIST_MIN_ROTATION = (-60.0).degrees
 
-  val MAX_WRIST_VELOCITY = 10.0.radians.perSecond
-  val MAX_WRIST_ACCELERATION = 10.0.radians.perSecond.perSecond
+  val MAX_WRIST_VELOCITY = 1.0.radians.perSecond
+  val MAX_WRIST_ACCELERATION = 1.0.radians.perSecond.perSecond
   object PID {
     val REAL_KP: ProportionalGain<Radian, Volt> = 0.001.volts / 1.0.degrees
     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.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.05.volts / (1.0.rotations / 1.0.seconds)
+    val SIM_KD: DerivativeGain<Radian, Volt> = 0.05.volts / (1.0.degrees / 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

@@ -186,7 +186,7 @@ class Wrist(val io: WristIO) : SubsystemBase() {
 
           val postProfileGenerate = Clock.fpgaTime
           Logger.recordOutput(
-            "/Shooter/ProfileGenerationMS",
+            "/Wrist/ProfileGenerationMS",
             postProfileGenerate.inSeconds - preProfileGenerate.inSeconds
           )
 
@@ -196,7 +196,7 @@ class Wrist(val io: WristIO) : SubsystemBase() {
         val timeElapsed = Clock.fpgaTime - timeProfileGeneratedAt
         val setPoint: TrapezoidProfile.State<Radian> = wristProfile.calculate(timeElapsed)
         setWristPosition(setPoint)
-        Logger.recordOutput("Shooter/completedMotionProfile", wristProfile.isFinished(timeElapsed))
+        Logger.recordOutput("Wrist/completedMotionProfile", wristProfile.isFinished(timeElapsed))
         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
@@ -247,7 +247,7 @@ class Wrist(val io: WristIO) : SubsystemBase() {
 
   fun wristPositionCommand(): Command {
     return Commands.runOnce({
-      currentRequest = Request.WristRequest.TargetingPosition(120.degrees)
+      currentRequest = Request.WristRequest.TargetingPosition(45.degrees)
     })
   }
 

src/main/kotlin/com/team4099/robot2023/subsystems/wrist/WristIOSim.kt

@@ -20,12 +20,15 @@ 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.inKilogramsMeterSquared
 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.perSecond
+import kotlin.math.PI
 
 object WristIOSim : WristIO {
 
@@ -80,7 +83,7 @@ object WristIOSim : WristIO {
   private val wristController =
     PIDController(WristConstants.PID.SIM_KP, WristConstants.PID.SIM_KI, WristConstants.PID.SIM_KD)
 
-  val appliedVoltage = 0.volts
+  var appliedVoltage = 0.volts
 
   override fun updateInputs(inputs: WristIO.WristIOInputs) {
     wristSim.update(Constants.Universal.LOOP_PERIOD_TIME.inSeconds)
@@ -102,14 +105,16 @@ object WristIOSim : WristIO {
    * @param voltage the voltage to set the roller motor to
    */
   override fun setWristVoltage(voltage: ElectricalPotential) {
-    wristSim.setInputVoltage(
-      clamp(voltage, -WristConstants.VOLTAGE_COMPENSATION, WristConstants.VOLTAGE_COMPENSATION)
-        .inVolts
-    )
+    val clampedVoltage = clamp(voltage, 5.volts, (WristConstants.VOLTAGE_COMPENSATION))
+    wristSim.setInputVoltage(clampedVoltage.inVolts)
+    println("stupid kotlin")
+    println(appliedVoltage)
+    println(voltage)
+    appliedVoltage = clampedVoltage
   }
 
-  override fun setWristPosition(wristPosition: Angle, feedforward: ElectricalPotential) {
-    val feedback = wristController.calculate(wristSim.angleRads.radians, wristPosition)
+  override fun setWristPosition(position: Angle, feedforward: ElectricalPotential) {
+    val feedback = wristController.calculate(wristSim.angleRads.radians * 180 / PI, position)
     setWristVoltage(feedback + feedforward)
   }