Retune Arm PID

ftc13100 · Dec 16, 2024 · b11ea5b · b11ea5b
1 parent 1659d3f
commit b11ea5b
Show file tree

Hide file tree

Showing 4 changed files with 16 additions and 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
@@ -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)
 }
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
@@ -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
@@ -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
@@ -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
 }