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 fe5a5f0..7228560 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,10 +1,15 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.arm +import com.acmerobotics.dashboard.config.Config import com.arcrobotics.ftclib.command.CommandOpMode +import com.arcrobotics.ftclib.command.InstantCommand import com.arcrobotics.ftclib.hardware.motors.Motor +import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +@TeleOp +@Config class ArmPIDTuner : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor @@ -17,6 +22,14 @@ class ArmPIDTuner : CommandOpMode() { armSubsystem = ArmSubsystem(armLeft, armRight) + InstantCommand({ + armSubsystem.setpoint = Math.toRadians(target) + }).perpetually().schedule() + } + companion object { + @JvmField + // NOTE THAT THIS VALUE SHOULD BE DEGREES + var target = 0.0 } } \ 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 9d59ab1..a65b30a 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 @@ -1,5 +1,6 @@ package org.firstinspires.ftc.teamcode.subsystems.arm +import com.acmerobotics.dashboard.config.Config import com.arcrobotics.ftclib.controller.PIDController import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward import com.arcrobotics.ftclib.hardware.motors.Motor @@ -8,6 +9,7 @@ import com.arcrobotics.ftclib.hardware.motors.MotorGroup import org.firstinspires.ftc.teamcode.utils.PIDSubsystem import kotlin.math.PI +@Config class ArmSubsystem( armLeft: Motor, armRight: Motor @@ -44,9 +46,6 @@ class ArmSubsystem( override fun getMeasurement() = armAngle companion object { - @JvmField - var target = 0.0 - @JvmField var kCos = 0.3