PIDArm Kcos and kp tuning

ftc13100 · Nov 15, 2024 · f403649 · f403649
1 parent 190d5c4
commit f403649
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 4 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmPIDTuner.kt
@@ -25,16 +25,19 @@ class ArmPIDTuner : CommandOpMode() {
         armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435)
         armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435)
 
-        armSubsystem = ArmSubsystem(armLeft, armRight)
+        armSubsystem = ArmSubsystem(armRight, armLeft)
 
         InstantCommand({
             armSubsystem.setpoint = Math.toRadians(target)
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Arm Angle: ", armSubsystem.armAngle)
+            telemetry.addData("Arm Angle: ", Math.toDegrees(armSubsystem.armAngle))
             telemetry.addData("Setpoint: ", target)
-        })
+            telemetry.update()
+        }).perpetually().schedule()
+
+        register(armSubsystem)
     }
 
     companion object {

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
@@ -31,7 +31,7 @@ class ArmSubsystem(
     private val feedforward = ArmFeedforward(0.0, kCos, 0.0);
 
     init {
-        armLeft.inverted = true
+        armRight.inverted = true
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
     }