motion profiling

ftc13100 · Dec 16, 2024 · fc7777d · fc7777d
1 parent 6300dcd
commit fc7777d
Show file tree

Hide file tree

Showing 5 changed files with 47 additions and 33 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt
@@ -4,5 +4,8 @@ enum class ArmConstants(val value: Double) {
     kP(3.0),
     kI(0.0),
     kD(0.0),
-    kCos(0.07)
+    kCos(0.07),
+
+    MAX_VELOCITY(1.6), // rad/s
+    MAX_ACCELERATION(22.5) // rad/s^2
 }
diff --git a/.../src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt b/.../src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt
@@ -6,7 +6,6 @@ import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
 import com.qualcomm.robotcore.util.ElapsedTime
 import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
-import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @Autonomous
 class ArmConstraintsTuner : LinearOpMode() {
@@ -30,16 +29,16 @@ class ArmConstraintsTuner : LinearOpMode() {
 
                 dt = timer.seconds() - dt
 
-                val curr = ElevatorSubsystem.velocity
+                val curr = ArmSubsystem.velocity
                 val dv = curr - prev
 
                 maxVel = ArmSubsystem.velocity.coerceAtLeast(maxVel)
                 maxAccel = maxAccel.coerceAtLeast(dv / dt)
 
                 prev = curr
 
-                telemetry.addData("Velocity", ElevatorSubsystem.velocity)
-                telemetry.addData("Position", ElevatorSubsystem.position)
+                telemetry.addData("Velocity", ArmSubsystem.velocity)
+                telemetry.addData("Angle", ArmSubsystem.angle)
 
                 telemetry.addData("Max Velocity", maxVel)
                 telemetry.addData("Max Acceleration", maxAccel)
@@ -48,6 +47,7 @@ class ArmConstraintsTuner : LinearOpMode() {
 
                 if (isStopRequested) break
             }
+            ArmSubsystem.stop()
 
             telemetry.addData("Final Max Velocity", maxVel)
             telemetry.addData("Final Max Acceleration", maxAccel)
@@ -57,6 +57,6 @@ class ArmConstraintsTuner : LinearOpMode() {
 
     companion object {
         @JvmField
-        var TIME_TO_RUN = 0.7
+        var TIME_TO_RUN = 1.5
     }
 }
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
@@ -25,8 +25,8 @@ class ArmPIDTuner : CommandOpMode() {
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Arm Angle: ", Math.toDegrees(ArmSubsystem.angle))
-            telemetry.addData("Setpoint: ", Math.toDegrees(ArmSubsystem.setpoint))
+            telemetry.addData("Arm Angle", Math.toDegrees(ArmSubsystem.angle))
+            telemetry.addData("Setpoint", Math.toDegrees(ArmSubsystem.setpoint))
             telemetry.update()
         }).perpetually().schedule()
 

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,34 +1,48 @@
 package org.firstinspires.ftc.teamcode.subsystems.arm
 
 import com.acmerobotics.dashboard.config.Config
-import com.arcrobotics.ftclib.controller.PIDController
+import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward
+import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.teamcode.constants.ArmConstants
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.utils.PIDSubsystem
 import kotlin.math.PI
 
 @Config
-object ArmSubsystem : PIDSubsystem(
-    PIDController(
-        ArmConstants.kP.value,
-        ArmConstants.kI.value,
-        ArmConstants.kD.value
-    )
-) {
+object ArmSubsystem : SubsystemBase() {
     private lateinit var turnMotors: MotorGroup
-    private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);
 
     val velocity: Double
         get() = turnMotors.velocities[0] / GoBILDA.RPM_30.cpr * PI
 
     val angle: Double
         get() = turnMotors.positions[0] / GoBILDA.RPM_30.cpr * PI
 
+    private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0);
+
+    private val controller = ProfiledPIDController(
+        ArmConstants.kP.value,
+        ArmConstants.kI.value,
+        ArmConstants.kD.value,
+        TrapezoidProfile.Constraints(
+            ArmConstants.MAX_VELOCITY.value,
+            ArmConstants.MAX_ACCELERATION.value
+        )
+    )
+
+    var setpoint = controller.goal.position
+        set(value) {
+            controller.goal = TrapezoidProfile.State(value, 0.0)
+            field = value
+        }
+
+    private var enabled = true
+
     fun initialize(hardwareMap: HardwareMap) : ArmSubsystem {
         val armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName)
         val armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName)
@@ -44,7 +58,6 @@ object ArmSubsystem : PIDSubsystem(
     }
 
 
-// Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%.
     fun clockwise() {
         turnMotors.set(1.0)
     }
@@ -57,20 +70,18 @@ object ArmSubsystem : PIDSubsystem(
         turnMotors.set(0.0)
     }
 
-    override fun useOutput(output: Double, setpoint: Double) {
-//        controller.setPIDF(kP, kI, kD, 0.0)
-//        feedforward = ArmFeedforward(kS, kCos, kV)
+    override fun periodic() {
+        val output = controller.calculate(angle) + feedforward.calculate(angle, velocity)
 
-        turnMotors.set(output + feedforward.calculate(angle, velocity))
+        if (enabled)
+            turnMotors.set(output)
     }
 
-    @JvmField var kP = 0.0
-    @JvmField var kI = 0.0
-    @JvmField var kD = 0.0
-
-    @JvmField var kCos = 0.0
-    @JvmField var kS = 0.0
-    @JvmField var kV = 0.0
-
-    override fun getMeasurement() = angle
+//    @JvmField var kP = 0.0
+//    @JvmField var kI = 0.0
+//    @JvmField var kD = 0.0
+//
+//    @JvmField var kCos = 0.0
+//    @JvmField var kS = 0.0
+//    @JvmField var kV = 0.0
 }
diff --git a/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/...ode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt
@@ -27,7 +27,7 @@ object ElevatorSubsystem: SubsystemBase() {
         )
     )
 
-    var setpoint = 0.0
+    var setpoint = controller.goal.position
         set(value) {
             val clamped = if (ArmSubsystem.angle < Math.toRadians(75.0))
                 clamp(value, 0.0, 20.0)