From fc7777d19deea9843ae18162c1b122f8c8f18bae Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Mon, 16 Dec 2024 00:01:03 -0500 Subject: [PATCH] motion profiling --- .../ftc/teamcode/constants/ArmConstants.kt | 5 +- .../opModes/tuning/arm/ArmConstraintsTuner.kt | 10 ++-- .../opModes/tuning/arm/ArmPIDTuner.kt | 4 +- .../teamcode/subsystems/arm/ArmSubsystem.kt | 59 +++++++++++-------- .../subsystems/slides/ElevatorSubsystem.kt | 2 +- 5 files changed, 47 insertions(+), 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 index e3ad60e..938e4e0 100644 --- 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 } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt index 5ba51ec..12b1441 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/arm/ArmConstraintsTuner.kt +++ b/TeamCode/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,7 +29,7 @@ class ArmConstraintsTuner : LinearOpMode() { dt = timer.seconds() - dt - val curr = ElevatorSubsystem.velocity + val curr = ArmSubsystem.velocity val dv = curr - prev maxVel = ArmSubsystem.velocity.coerceAtLeast(maxVel) @@ -38,8 +37,8 @@ class ArmConstraintsTuner : LinearOpMode() { 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 } } \ No newline at end of file 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 5da3242..d794d28 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 @@ -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 index 42acde3..12b80b8 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,27 +1,21 @@ 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 @@ -29,6 +23,26 @@ object ArmSubsystem : PIDSubsystem( 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 } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt index 29de3fa..488aee5 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt +++ b/TeamCode/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)