Intake system modeling

ftc13100 · Dec 23, 2024 · b561dab · b561dab
1 parent c35b7ac
commit b561dab
Show file tree

Hide file tree

Showing 4 changed files with 20 additions and 14 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/ElevatorCommand.kt
@@ -3,6 +3,10 @@ package org.firstinspires.ftc.teamcode.commands.elevator
 import com.arcrobotics.ftclib.command.CommandBase
 import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
+/**
+ * Elevator Command to set PID position for the elevator
+ * @see ElevatorSubsystem
+ */
 class ElevatorCommand(
     private val setpoint: Double,
     private val subsystem: ElevatorSubsystem,

diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/IntakeConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/IntakeConstants.kt
@@ -3,7 +3,8 @@ package org.firstinspires.ftc.teamcode.constants
 enum class IntakeConstants(val value: Double) {
     kP(0.0),
     kI(0.0),
-    kD(0.0)
-
+    kD(0.0),
 
+    INTAKE_MIN(-2000.0),
+    INTAKE_MAX(2000.0)
 }
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt
@@ -5,24 +5,15 @@ 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 com.qualcomm.robotcore.hardware.Servo
-import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem
 
 @TeleOp
 @Config
 class IntakeTuner : CommandOpMode() {
-    private lateinit var intake: Servo
-    private lateinit var intakeBelt: Servo
-
     override fun initialize() {
         telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry)
 
-        intake = hardwareMap[Servo::class.java, ControlBoard.INTAKE.deviceName]
-        intakeBelt = hardwareMap[Servo::class.java, ControlBoard.INTAKE_BELT.deviceName]
-
         IntakeSubsystem.initialize(hardwareMap)
 
         RunCommand({
@@ -31,6 +22,8 @@ class IntakeTuner : CommandOpMode() {
 
         RunCommand({
             IntakeSubsystem.target = wristTarget
+
+            IntakeSubsystem.operateWrist()
         }).perpetually().schedule()
 
         RunCommand({

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
@@ -1,32 +1,39 @@
 package org.firstinspires.ftc.teamcode.subsystems.intake
 
+import com.acmerobotics.dashboard.config.Config
 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.hardware.motors.CRServo
 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 kotlin.math.PI
 
+@Config
 object IntakeSubsystem : SubsystemBase() {
     private lateinit var claw: Servo
     private lateinit var wrist: CRServo
     private lateinit var intakeEncoder: Encoder
 
     val position
-        get() = intakeEncoder.getPositionAndVelocity().position
+        get() = intakeEncoder.getPositionAndVelocity().position / 8192.0 * 2 * PI
 
     val velocity
-        get() = intakeEncoder.getPositionAndVelocity().velocity
+        get() = intakeEncoder.getPositionAndVelocity().velocity / 8192.0 * 2 * PI
 
     @JvmField var kP = 0.0
     @JvmField var kI = 0.0
     @JvmField var kD = 0.0
+    @JvmField var kCos = 0.0
 
     private val controller = PIDController(kP, kI, kD)
+    private var feedforward = ArmFeedforward(0.0, kCos, 0.0)
 
     var target = controller.setPoint
         set(value) {
@@ -67,8 +74,9 @@ object IntakeSubsystem : SubsystemBase() {
 
     fun operateWrist() {
         controller.setPID(kP, kI, kD)
+        feedforward = ArmFeedforward(0.0, kCos, 0.0)
 
-        val output = controller.calculate(position.toDouble())
+        val output = controller.calculate(position) + feedforward.calculate(position + ArmSubsystem.angle, velocity)
 
         wrist.set(output)
     }