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 index caea3e9..a10273b 100644 --- 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 index a65b30a..2f1bbd2 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 @@ -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) }