SMOOTH LIKE BUTTER

ftc13100 · Dec 15, 2024 · 1659d3f · 1659d3f
1 parent f1ea5db
commit 1659d3f
Show file tree

Hide file tree

Showing 5 changed files with 59 additions and 50 deletions.
diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/SlidesConstants.kt
@@ -1,10 +1,13 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class SlidesConstants(val value: Double) {
-    kP(0.7),
+    kP(0.4),
     kI(0.0),
     kD(0.0),
-    kG(0.12),
+    kG(0.0),
+
+    MAX_VELOCITY(36.0), // in/s
+    MAX_ACCELERATION(400.0), // in/s^2
 
     MAX_HEIGHT_TICKS(2081.0),
     MAX_HEIGHT_INCH(33.329)

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
@@ -45,8 +45,6 @@ class MainTeleOp : CommandOpMode() {
 //        outtakeCommand = IntakeCommand(false, intakeSubsystem)
         driveCommand = DriveCommand(DriveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0)
 
-        operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand)
-        operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand)
         operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand)
         operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whileHeld(spinDownCommand)
         operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand)
@@ -57,7 +55,11 @@ class MainTeleOp : CommandOpMode() {
         DriveSubsystem.defaultCommand = driveCommand
 
         RunCommand({
-            telemetry.addData("Arm Position: ", ArmSubsystem.angle)
+            telemetry.addData("Arm Position", ArmSubsystem.angle)
+
+            telemetry.addData("Slides Position", ElevatorSubsystem.position)
+            telemetry.addData("Slides Velocity", ElevatorSubsystem.velocity)
+
             telemetry.update()
         }).perpetually().schedule()
     }

diff --git a/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/...ain/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt
@@ -19,27 +19,31 @@ class SlidesConstraintsTuner : LinearOpMode() {
 
         waitForStart()
 
-        var dt = 0L
-        var dv = 0.0
+        var dt = 0.0
+        var prev = 0.0
 
         timer.reset()
         while (opModeIsActive()) {
             while (timer.seconds() < TIME_TO_RUN) {
                 ElevatorSubsystem.spinUp()
 
-                dt = timer.nanoseconds() - dt
-                dv = ElevatorSubsystem.velocity - dv / dt
+                dt = timer.seconds() - dt
+
+                val curr = ElevatorSubsystem.velocity
+                val dv = curr - prev
 
                 maxVel = ElevatorSubsystem.velocity.coerceAtLeast(maxVel)
-                maxAccel = dv.coerceAtLeast(maxAccel)
+                maxAccel = maxAccel.coerceAtLeast(dv / dt)
+
+                prev = curr
 
                 telemetry.addData("Velocity", ElevatorSubsystem.velocity)
                 telemetry.addData("Position", ElevatorSubsystem.position)
 
                 telemetry.addData("Max Velocity", maxVel)
                 telemetry.addData("Max Acceleration", maxAccel)
-                telemetry.update()
 
+                telemetry.update()
 
                 if (isStopRequested) break
             }
@@ -52,6 +56,6 @@ class SlidesConstraintsTuner : LinearOpMode() {
 
     companion object {
         @JvmField
-        var TIME_TO_RUN = 1.5
+        var TIME_TO_RUN = 0.7
     }
 }
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
@@ -11,7 +11,6 @@ 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
-import kotlin.math.cos
 
 @Config
 object ArmSubsystem : PIDSubsystem(
@@ -41,21 +40,23 @@ object ArmSubsystem : PIDSubsystem(
         turnMotors.resetEncoder()
         turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE)
 
+        disable()
+
         return this
     }
 
 
 // Here are functions that work the motor, and the double is the speed of the motor, 1 being 100%.
     fun clockwise() {
-        turnMotors.set(0.50)
+        turnMotors.set(1.0)
     }
 
     fun anticlockwise() {
-        turnMotors.set(-0.50)
+        turnMotors.set(-1.0)
     }
 
     fun stop() {
-        turnMotors.set(ArmConstants.kCos.value * cos(angle))
+        turnMotors.set(0.0)
     }
 
     override fun useOutput(output: Double, setpoint: Double) {

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
@@ -1,82 +1,81 @@
 package org.firstinspires.ftc.teamcode.subsystems.slides
 
+import com.acmerobotics.dashboard.config.Config
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.controller.wpilibcontroller.ProfiledPIDController
+import com.arcrobotics.ftclib.hardware.motors.Motor
+import com.arcrobotics.ftclib.hardware.motors.MotorGroup
 import com.arcrobotics.ftclib.trajectory.TrapezoidProfile
-import com.qualcomm.robotcore.hardware.DcMotor
-import com.qualcomm.robotcore.hardware.DcMotorEx
+import com.arcrobotics.ftclib.util.MathUtils.clamp
 import com.qualcomm.robotcore.hardware.HardwareMap
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
 import org.firstinspires.ftc.teamcode.constants.SlidesConstants
+import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem
+import kotlin.math.sin
 
+@Config
 object ElevatorSubsystem: SubsystemBase() {
-//    private lateinit var elevatorMotors: MotorGroup
-    private lateinit var elevator: DcMotorEx
+    private lateinit var elevatorMotors: MotorGroup
 
     private val controller = ProfiledPIDController(
         SlidesConstants.kP.value,
         SlidesConstants.kI.value,
         SlidesConstants.kD.value,
         TrapezoidProfile.Constraints(
-            30.0, // in/s
-            30.0 // in/s^2
+            SlidesConstants.MAX_VELOCITY.value,
+            SlidesConstants.MAX_ACCELERATION.value
         )
     )
 
     var setpoint = 0.0
         set(value) {
-//            val clamped = if (ArmSubsystem.angle < Math.toRadians(75.0))
-//                clamp(value, 0.0, 20.0)
-//            else
-//                clamp(value, 0.0, 30.0)
-//
-//            controller.goal = TrapezoidProfile.State(clamped, 0.0)
-//            field = clamped
-            elevator.targetPosition = value.toInt()
-            field = value
+            val clamped = if (ArmSubsystem.angle < Math.toRadians(75.0))
+                clamp(value, 0.0, 20.0)
+            else
+                clamp(value, 0.0, 30.0)
+
+            controller.goal = TrapezoidProfile.State(clamped, 0.0)
+            field = clamped
         }
 
     val position: Double
-//        get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
-        get() = elevator.currentPosition.toDouble()
+        get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
 
     val velocity: Double
-//        get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
-        get() = elevator.velocity
+        get() = -elevatorMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value
 
     var enabled = true
 
     fun initialize(hardwareMap: HardwareMap) {
-//        val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
-//        val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
+        val elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
+        val elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
 
-        val elevator = hardwareMap[DcMotorEx::class.java, ControlBoard.SLIDES_RIGHT.deviceName]
-        elevator.mode = DcMotor.RunMode.RUN_TO_POSITION
+        elevatorRight.inverted = true
+        elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
 
-        elevator.power = 1.0
-//        elevatorLeft.inverted = true
-//        elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
+        elevatorMotors = MotorGroup(elevatorRight, elevatorLeft)
+        elevatorMotors.resetEncoder()
     }
 
     fun spinUp() {
-//        elevatorMotors.set(1.0)
+        elevatorMotors.set(1.0)
     }
 
     fun spinDown() {
-//        elevatorMotors.set(-1.0)
+        elevatorMotors.set(-1.0)
     }
 
     fun stop() {
-//        elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle))
+        elevatorMotors.set(SlidesConstants.kG.value * sin(ArmSubsystem.angle))
     }
 
     override fun periodic() {
-//        controller.setPID(kP, kI, kD)
-
-//        val output = controller.calculate(position)
+        val output = controller.calculate(position) +
+                SlidesConstants.kG.value * sin(ArmSubsystem.angle)
 
-//        if (enabled)
-//            elevatorMotors.set(output + SlidesConstants.kG.value * sin(ArmSubsystem.angle))
+        if (enabled) {
+            elevatorMotors.set(output + SlidesConstants.kG.value * sin(ArmSubsystem.angle))
+        }
     }
 
     fun toggle() {