From b11ea5be872d4c4199ba8a750303d10f998f1b8b Mon Sep 17 00:00:00 2001
From: Sachet Korada <sachet.korada@gmail.com>
Date: Sun, 15 Dec 2024 21:55:56 -0500
Subject: [PATCH] Retune Arm PID

---
 .../ftc/teamcode/constants/ArmConstants.kt       |  4 ++--
 .../ftc/teamcode/constants/SlidesConstants.kt    |  4 ++--
 .../ftc/teamcode/opModes/tuning/IntakeTuner.kt   |  3 +--
 .../ftc/teamcode/subsystems/arm/ArmSubsystem.kt  | 16 +++++++++++-----
 4 files changed, 16 insertions(+), 11 deletions(-)

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
index b607006..e3ad60e 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
@@ -1,8 +1,8 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class ArmConstants(val value: Double) {
-    kP(2.0),
+    kP(3.0),
     kI(0.0),
     kD(0.0),
-    kCos(0.02)
+    kCos(0.07)
 }
\ No newline at end of file
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
index 53dc128..04c9ba0 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
@@ -4,10 +4,10 @@ enum class SlidesConstants(val value: Double) {
     kP(0.4),
     kI(0.0),
     kD(0.0),
-    kG(0.0),
+    kG(0.1),
 
     MAX_VELOCITY(36.0), // in/s
-    MAX_ACCELERATION(400.0), // in/s^2
+    MAX_ACCELERATION(420.0), // in/s^2
 
     MAX_HEIGHT_TICKS(2081.0),
     MAX_HEIGHT_INCH(33.329)
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
index 15c9dbc..784a0e9 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
@@ -18,7 +18,6 @@ class IntakeTuner : CommandOpMode() {
     private lateinit var intake: Servo
     private lateinit var intakeBelt: Servo
 
-    private lateinit var intakeSubsystem: IntakeSubsystem
     private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem
 
     override fun initialize() {
@@ -31,7 +30,7 @@ class IntakeTuner : CommandOpMode() {
         intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt)
 
         RunCommand({
-            intakeSubsystem.setSpeed(claw)
+            IntakeSubsystem.setSpeed(claw)
         }).perpetually().schedule()
 
         RunCommand({
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
index 3a9a665..42acde3 100644
--- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
+++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmSubsystem.kt
@@ -24,10 +24,10 @@ object ArmSubsystem : PIDSubsystem(
     private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);
 
     val velocity: Double
-        get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI
+        get() = turnMotors.velocities[0] / GoBILDA.RPM_30.cpr * PI
 
     val angle: Double
-        get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI
+        get() = turnMotors.positions[0] / GoBILDA.RPM_30.cpr * PI
 
     fun initialize(hardwareMap: HardwareMap) : ArmSubsystem {
         val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
@@ -40,8 +40,6 @@ object ArmSubsystem : PIDSubsystem(
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
 
-        disable()
-
         return this
     }
 
@@ -61,10 +59,18 @@ object ArmSubsystem : PIDSubsystem(
 
     override fun useOutput(output: Double, setpoint: Double) {
 //        controller.setPIDF(kP, kI, kD, 0.0)
-//        feedforward = ArmFeedforward(0.0, kCos, 0.0)
+//        feedforward = ArmFeedforward(kS, kCos, kV)
 
         turnMotors.set(output + feedforward.calculate(angle, velocity))
     }
 
+    @JvmField var kP = 0.0
+    @JvmField var kI = 0.0
+    @JvmField var kD = 0.0
+
+    @JvmField var kCos = 0.0
+    @JvmField var kS = 0.0
+    @JvmField var kV = 0.0
+
     override fun getMeasurement() = angle
 }
\ No newline at end of file