Intake PID

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/IntakeConstants.kt

@@ -1,10 +1,10 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class IntakeConstants(val value: Double) {
-    kP(0.0),
+    kP(0.6),
     kI(0.0),
     kD(0.0),
 
-    INTAKE_MIN(-2000.0),
-    INTAKE_MAX(2000.0)
+    INTAKE_MAX_VELOCITY(4.0),
+    INTAKE_MAX_ACCELERATION(5.0)
 }

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/intake/IntakeConstraintsTuner.kt

@@ -0,0 +1,63 @@
+package org.firstinspires.ftc.teamcode.opModes.tuning.intake
+
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+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.intake.IntakeSubsystem
+
+@Autonomous
+class IntakeConstraintsTuner : LinearOpMode() {
+    private var maxVel = 0.0
+    private var maxAccel = 0.0
+
+    private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS)
+    override fun runOpMode() {
+        telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry)
+        IntakeSubsystem.initialize(hardwareMap)
+
+        waitForStart()
+
+        var dt = 0.0
+        var prev = 0.0
+
+        timer.reset()
+        while (opModeIsActive()) {
+            while (timer.seconds() < TIME_TO_RUN) {
+                IntakeSubsystem.wristUp()
+
+                dt = timer.seconds() - dt
+
+                val curr = IntakeSubsystem.velocity
+                val dv = curr - prev
+
+                maxVel = IntakeSubsystem.velocity.coerceAtLeast(maxVel)
+                maxAccel = maxAccel.coerceAtLeast(dv / dt)
+
+                prev = curr
+
+                telemetry.addData("Velocity", IntakeSubsystem.velocity)
+                telemetry.addData("Angle", IntakeSubsystem.position)
+
+                telemetry.addData("Max Velocity", maxVel)
+                telemetry.addData("Max Acceleration", maxAccel)
+
+                telemetry.update()
+
+                if (isStopRequested) break
+            }
+            ArmSubsystem.stop()
+
+            telemetry.addData("Final Max Velocity", maxVel)
+            telemetry.addData("Final Max Acceleration", maxAccel)
+            telemetry.update()
+        }
+    }
+
+    companion object {
+        @JvmField
+        var TIME_TO_RUN = 1.5
+    }
+}

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt → TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/intake/IntakeTuner.kt

@@ -1,11 +1,12 @@
-package org.firstinspires.ftc.teamcode.opModes.tuning
+package org.firstinspires.ftc.teamcode.opModes.tuning.intake
 
 import com.acmerobotics.dashboard.FtcDashboard
 import com.acmerobotics.dashboard.config.Config
 import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
 import com.arcrobotics.ftclib.command.CommandOpMode
 import com.arcrobotics.ftclib.command.RunCommand
 import com.qualcomm.robotcore.eventloop.opmode.TeleOp
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 
 @TeleOp
@@ -15,20 +16,21 @@ class IntakeTuner : CommandOpMode() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
         IntakeSubsystem.initialize(hardwareMap)
+        ArmSubsystem.initialize(hardwareMap)
 
         RunCommand({
             IntakeSubsystem.setClaw(claw)
         }).perpetually().schedule()
 
         RunCommand({
-            IntakeSubsystem.target = wristTarget
+            IntakeSubsystem.target = Math.toRadians(wristTarget)
 
             IntakeSubsystem.operateWrist()
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Intake Position", IntakeSubsystem.position)
-            telemetry.addData("Intake Velocity", IntakeSubsystem.velocity)
+            telemetry.addData("Intake Position", Math.toDegrees(IntakeSubsystem.position))
+            telemetry.addData("Intake Velocity", Math.toDegrees(IntakeSubsystem.velocity))
 
             telemetry.update()
         }).perpetually().schedule()

TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeSubsystem.kt

@@ -5,14 +5,15 @@ import com.acmerobotics.roadrunner.ftc.Encoder
 import com.acmerobotics.roadrunner.ftc.OverflowEncoder
 import com.acmerobotics.roadrunner.ftc.RawEncoder
 import com.arcrobotics.ftclib.command.SubsystemBase
-import com.arcrobotics.ftclib.controller.PIDController
 import com.arcrobotics.ftclib.controller.wpilibcontroller.ArmFeedforward
+import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController
 import com.arcrobotics.ftclib.hardware.motors.CRServo
+import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
 import com.qualcomm.robotcore.hardware.DcMotorEx
 import com.qualcomm.robotcore.hardware.HardwareMap
 import com.qualcomm.robotcore.hardware.Servo
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import org.firstinspires.ftc.teamcode.constants.IntakeConstants
 import kotlin.math.PI
 
 @Config
@@ -22,7 +23,7 @@ object IntakeSubsystem : SubsystemBase() {
     private lateinit var intakeEncoder: Encoder
 
     val position
-        get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI
+        get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI - 1.82
 
     val velocity
         get() = intakeEncoder.getPositionAndVelocity().velocity / 8192.0 * 2 * PI
@@ -32,12 +33,20 @@ object IntakeSubsystem : SubsystemBase() {
     @JvmField var kD = 0.0
     @JvmField var kCos = 0.0
 
-    private val controller = PIDController(kP, kI, kD)
+    private val controller = ProfiledPIDController(
+        IntakeConstants.kP.value,
+        IntakeConstants.kI.value,
+        IntakeConstants.kD.value,
+        TrapezoidProfile.Constraints(
+            IntakeConstants.INTAKE_MAX_VELOCITY.value,
+            IntakeConstants.INTAKE_MAX_ACCELERATION.value
+        )
+    )
     private var feedforward = ArmFeedforward(0.0, kCos, 0.0)
 
-    var target = controller.setPoint
+    var target = controller.goal.position
         set(value) {
-            controller.setPoint = value
+            controller.goal = TrapezoidProfile.State(value, 0.0)
             field = value
         }
 
@@ -76,7 +85,8 @@ object IntakeSubsystem : SubsystemBase() {
         controller.setPID(kP, kI, kD)
         feedforward = ArmFeedforward(0.0, kCos, 0.0)
 
-        val output = controller.calculate(position) + feedforward.calculate(position + ArmSubsystem.angle, velocity)
+        val output = controller.calculate(position) +
+                feedforward.calculate(position, velocity)
 
         wrist.set(output)
     }