Arm PID infra:

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/ArmCommand.kt

@@ -0,0 +1,13 @@
+package org.firstinspires.ftc.teamcode.commands.arm
+
+import com.arcrobotics.ftclib.command.CommandBase
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+
+class ArmCommand(
+    private val setpoint: Double,
+    private val subsystem: ArmSubsystem
+) : CommandBase() {
+    override fun initialize() {
+        subsystem.setpoint = setpoint
+    }
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/ArmCommand.kt → TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/arm/OpenArmCommand.kt

@@ -1,10 +1,10 @@
-package org.firstinspires.ftc.teamcode.commands.drive
+package org.firstinspires.ftc.teamcode.commands.arm
 
 import com.arcrobotics.ftclib.command.CommandBase
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 
-class ArmCommand(
-    private val subsystem: ArmSubsystem,
+class OpenArmCommand(
+    private val subsystem: OpenArmSubsystem,
     private val turn: Boolean,
 ) : CommandBase() {
 

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinDownCommand.kt → TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.commands.drive
+package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/SpinUpCommand.kt → TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt

@@ -1,4 +1,4 @@
-package org.firstinspires.ftc.teamcode.commands.drive
+package org.firstinspires.ftc.teamcode.commands.elevator
 
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt

@@ -6,32 +6,30 @@ import com.arcrobotics.ftclib.gamepad.GamepadEx
 import com.arcrobotics.ftclib.gamepad.GamepadKeys
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
-import org.firstinspires.ftc.teamcode.commands.drive.ArmCommand
+import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand
 import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand
-import org.firstinspires.ftc.teamcode.commands.drive.SpinDownCommand
-import org.firstinspires.ftc.teamcode.commands.drive.SpinUpCommand
+import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand
+import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsystem
 
 @TeleOp
 class MainTeleOp : CommandOpMode() {
-
     private lateinit var elevatorLeft: Motor
     private lateinit var elevatorRight: Motor
     private lateinit var armLeft: Motor
     private lateinit var armRight: Motor
 
-
     private lateinit var slidesSubsystem: SlidesSubsystem
-    private lateinit var armSubsystem: ArmSubsystem
+    private lateinit var armSubsystem: OpenArmSubsystem
     private lateinit var driveSubsystem: DriveSubsystem
 
     private lateinit var spinUpCommand: SpinUpCommand
     private lateinit var spinDownCommand: SpinDownCommand
-    private lateinit var armUpCommand: ArmCommand
-    private lateinit var armDownCommand: ArmCommand
+    private lateinit var armUpCommand: OpenArmCommand
+    private lateinit var armDownCommand: OpenArmCommand
     private lateinit var driveCommand: DriveCommand
 
     private lateinit var driver: GamepadEx
@@ -48,13 +46,13 @@ class MainTeleOp : CommandOpMode() {
         armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435)
 
         slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft)
-        armSubsystem = ArmSubsystem(armRight, armLeft, true)
+        armSubsystem = OpenArmSubsystem(armRight, armLeft)
         driveSubsystem = DriveSubsystem(hardwareMap)
 
         spinUpCommand = SpinUpCommand(slidesSubsystem)
         spinDownCommand = SpinDownCommand(slidesSubsystem)
-        armUpCommand = ArmCommand(armSubsystem, true)
-        armDownCommand = ArmCommand(armSubsystem, false)
+        armUpCommand = OpenArmCommand(armSubsystem, true)
+        armDownCommand = OpenArmCommand(armSubsystem, false)
         driveCommand =
             DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
 

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt

@@ -0,0 +1,4 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.arm
+
+class ArmPIDTuner {
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt

@@ -1,6 +1,5 @@
 package org.firstinspires.ftc.teamcode.subsystems.arm
 
-import com.acmerobotics.dashboard.config.Config
 import com.arcrobotics.ftclib.controller.PIDController
 import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward
 import com.arcrobotics.ftclib.hardware.motors.Motor
@@ -9,58 +8,56 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import org.firstinspires.ftc.teamcode.utils.PIDSubsystem
 import kotlin.math.PI
 
-@Config
 class ArmSubsystem(
-// Here I am just declaring the motors and what they are called on our driver hub.
-    armRight: Motor,
     armLeft: Motor,
-    openLoop: Boolean
+    armRight: Motor
 ) : PIDSubsystem(
     PIDController(
         0.0,
         0.0,
-        0.0
+        0.0,
     )
 ) {
+    private val turnMotors = MotorGroup(armLeft, armRight)
 
-    //Here I am making a motor group, as the arm motors are going to work together to to turn the slides.
-    private val feedforward = ArmFeedforward(0.0, kCos, 0.0)
-    private val turnMotors = MotorGroup(armRight, armLeft)
     val armAngle: Double
         get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI
 
+    val armVelocity: Double
+        get() = turnMotors.velocities[0] / GoBILDA.RPM_435.cpr * 2 * PI
+
+    private val feedforward = ArmFeedforward(0.0, kCos, 0.0);
+
     init {
         armLeft.inverted = true
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
-
-        if (openLoop)
-            disable()
-        else
-            enable()
     }
 
     override fun useOutput(output: Double, setpoint: Double) {
-        turnMotors.set(output + feedforward.calculate(getMeasurement(), 0.0))
-    }
+        // For tuning only
+        super.setpoint = target
+        controller.setPIDF(kP, kI, kD, 0.0)
 
-    override fun getMeasurement(): Double = armAngle
-
-    // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%.
-    fun clockwise() {
-        turnMotors.set(0.35)
-    }
-
-    fun anticlockwise() {
-        turnMotors.set(-0.35)
+        turnMotors.set(output + feedforward.calculate(armAngle, armVelocity))
     }
 
-    fun stop() {
-        turnMotors.set(feedforward.calculate(getMeasurement(), 0.0))
-    }
+    override fun getMeasurement() = armAngle
 
     companion object {
+        @JvmField
+        var target = 0.0
+
         @JvmField
         var kCos = 0.3
+
+        @JvmField
+        var kP = 0.0
+
+        @JvmField
+        var kI = 0.0
+
+        @JvmField
+        var kD = 0.0
     }
 }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt

@@ -0,0 +1,48 @@
+package org.firstinspires.ftc.teamcode.subsystems.arm
+
+import com.acmerobotics.dashboard.config.Config
+import com.arcrobotics.ftclib.command.SubsystemBase
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import kotlin.math.PI
+
+@Config
+class OpenArmSubsystem(
+// Here I am just declaring the motors and what they are called on our driver hub.
+        armRight : Motor,
+        armLeft : Motor,
+
+) : SubsystemBase() {
+
+//Here I am making a motor group, as the arm motors are going to work together to to turn the slides.
+
+    private val turnMotors = MotorGroup(armRight, armLeft)
+    val armAngle: Double
+        get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI
+
+    init {
+        armLeft.inverted = true
+        turnMotors.resetEncoder()
+        turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
+    }
+
+
+// Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%.
+    fun clockwise() {
+        turnMotors.set(0.35)
+    }
+
+    fun anticlockwise() {
+        turnMotors.set(-0.35)
+    }
+
+    fun stop() {
+        turnMotors.set(kCos * Math.cos(armAngle))
+    }
+
+    companion object {
+        @JvmField
+        var kCos = 0.3
+    }
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/utils/PIDSubsystem.kt

@@ -4,10 +4,10 @@ import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.controller.PIDFController
 
 abstract class PIDSubsystem(
-    private val controller: PIDFController,
+    val controller: PIDFController,
 ) : SubsystemBase() {
     private var enabled: Boolean = true
-    private var setpoint: Double = controller.setPoint
+    var setpoint: Double = controller.setPoint
         set(value) {
             controller.setPoint = value
             field = value