Write arm PID

ftc13100 · Nov 9, 2024 · acd40e2 · acd40e2
1 parent f5a1d3a
commit acd40e2
Show file tree

Hide file tree

Showing 5 changed files with 66 additions and 18 deletions.
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
@@ -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/.../main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt b/.../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
@@ -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")
+    }
+
+}
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
@@ -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
@@ -2,8 +2,4 @@ package org.firstinspires.ftc.teamcode.subsystems.intake
 
 import com.arcrobotics.ftclib.command.SubsystemBase
 
-class IntakeSubsystem(
-
-) : SubsystemBase() {
-
-}
+class IntakeSubsystem : SubsystemBase()