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 09f20fb..fe5a5f0 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 @@ -1,4 +1,22 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.arm -class ArmPIDTuner { +import com.arcrobotics.ftclib.command.CommandOpMode +import com.arcrobotics.ftclib.hardware.motors.Motor +import org.firstinspires.ftc.teamcode.constants.ControlBoard +import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem + +class ArmPIDTuner : CommandOpMode() { + private lateinit var armLeft: Motor + private lateinit var armRight: Motor + + private lateinit var armSubsystem: ArmSubsystem + + override fun initialize() { + 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) + + + } } \ No newline at end of file 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 c60d46a..9d59ab1 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 @@ -36,7 +36,6 @@ class ArmSubsystem( override fun useOutput(output: Double, setpoint: Double) { // For tuning only - super.setpoint = target controller.setPIDF(kP, kI, kD, 0.0) turnMotors.set(output + feedforward.calculate(armAngle, armVelocity))