From acd40e2beb8c2b4aea458cab7ae6514ff0325f71 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 9 Nov 2024 18:37:56 -0500 Subject: [PATCH] Write arm PID --- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 7 ++-- .../tuning/roadrunner/MaxVelocityTuner.kt | 2 +- .../subsystems/arm/ArmPIDSubsystem.kt | 34 ++++++++++++++++++ .../teamcode/subsystems/arm/ArmSubsystem.kt | 35 ++++++++++++++----- .../subsystems/intake/IntakeSubsystem.kt | 6 +--- 5 files changed, 66 insertions(+), 18 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt index 65c4586..241c293 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt @@ -41,13 +41,14 @@ class MainTeleOp : CommandOpMode() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_435) + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_435) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435) slidesSubsystem = SlidesSubsystem(elevatorRight, elevatorLeft) - armSubsystem = ArmSubsystem(armRight, armLeft) + armSubsystem = ArmSubsystem(armRight, armLeft, true) driveSubsystem = DriveSubsystem(hardwareMap) spinUpCommand = SpinUpCommand(slidesSubsystem) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt index b679e3f..55b65c3 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt @@ -69,7 +69,7 @@ class MaxVelocityTuner : LinearOpMode() { telemetry.addData("Max Velocity", maxVelocity) telemetry.addData( "Voltage Compensated kF", - effectiveKf * batteryVoltageSensor.getVoltage() / 12 + effectiveKf * batteryVoltageSensor.voltage / 12 ) telemetry.update() diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt new file mode 100644 index 0000000..7f4e5ec --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/ArmPIDSubsystem.kt @@ -0,0 +1,34 @@ +package org.firstinspires.ftc.teamcode.subsystems.arm + +import com.arcrobotics.ftclib.controller.PIDController +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.arcrobotics.ftclib.hardware.motors.MotorGroup +import org.firstinspires.ftc.teamcode.utils.PIDSubsystem + +class ArmPIDSubsystem( + armLeft: Motor, + armRight: Motor +) : PIDSubsystem( + PIDController( + 0.0, + 0.0, + 0.0, + ) +) { + private val turnMotors = MotorGroup(armLeft, armRight) + + init { + armLeft.inverted = true + turnMotors.resetEncoder() + turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + } + + override fun useOutput(output: Double, setpoint: Double) { + TODO("Not yet implemented") + } + + override fun getMeasurement(): Double { + TODO("Not yet implemented") + } + +} \ 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 cd8982e..b164f19 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,23 +1,30 @@ package org.firstinspires.ftc.teamcode.subsystems.arm import com.acmerobotics.dashboard.config.Config -import com.arcrobotics.ftclib.command.SubsystemBase +import com.arcrobotics.ftclib.controller.PIDController +import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA import com.arcrobotics.ftclib.hardware.motors.MotorGroup +import org.firstinspires.ftc.teamcode.utils.PIDSubsystem import kotlin.math.PI -import kotlin.math.cos @Config class ArmSubsystem( // Here I am just declaring the motors and what they are called on our driver hub. - private val armRight: Motor, - private val armLeft: Motor, - - ) : SubsystemBase() { - -//Here I am making a motor group, as the arm motors are going to work together to to turn the slides. + armRight: Motor, + armLeft: Motor, + openLoop: Boolean +) : PIDSubsystem( + PIDController( + 0.0, + 0.0, + 0.0 + ) +) { + //Here I am making a motor group, as the arm motors are going to work together to to turn the slides. + private val feedforward = ArmFeedforward(0.0, kCos, 0.0) private val turnMotors = MotorGroup(armRight, armLeft) val armAngle: Double get() = turnMotors.positions[0] / GoBILDA.RPM_435.cpr * 2 * PI @@ -26,8 +33,18 @@ class ArmSubsystem( armLeft.inverted = true turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + + if (openLoop) + disable() + else + enable() + } + + override fun useOutput(output: Double, setpoint: Double) { + turnMotors.set(output + feedforward.calculate(getMeasurement(), 0.0)) } + override fun getMeasurement(): Double = armAngle // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%. fun clockwise() { @@ -39,7 +56,7 @@ class ArmSubsystem( } fun stop() { - turnMotors.set(kCos * cos(armAngle)) + turnMotors.set(feedforward.calculate(getMeasurement(), 0.0)) } companion object { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt index 5c298e1..0d01382 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt @@ -2,8 +2,4 @@ package org.firstinspires.ftc.teamcode.subsystems.intake import com.arcrobotics.ftclib.command.SubsystemBase -class IntakeSubsystem( - -) : SubsystemBase() { - -} \ No newline at end of file +class IntakeSubsystem : SubsystemBase() \ No newline at end of file