Profile le arm

ftc13100 · Nov 28, 2024 · e0f0659 · e0f0659
1 parent 8d92ac1
commit e0f0659
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 17 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,7 +1,8 @@
 package org.firstinspires.ftc.teamcode.constants
 
 enum class SlidesConstants(val value: Double) {
-    kP(0.03),
+    kP(0.02),
     kI(0.0),
-    kD(0.0004),
+    kD(0.00045),
+    kG(0.1)
 }
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
@@ -1,5 +1,7 @@
 package org.firstinspires.ftc.teamcode.opModes.tuning.slides
 
+import com.acmerobotics.dashboard.FtcDashboard
+import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
@@ -19,6 +21,8 @@ class SlidesConstraintsTuner : LinearOpMode() {
 
     private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS)
     override fun runOpMode() {
+        telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry)
+
         leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
         rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
 
@@ -39,6 +43,9 @@ class SlidesConstraintsTuner : LinearOpMode() {
                 maxVel = elevator.slideVelocity.coerceAtLeast(maxVel)
                 maxAccel = dv.coerceAtLeast(maxAccel)
 
+                telemetry.addData("Velocity", elevator.slideVelocity)
+                telemetry.addData("Position", elevator.slidePos)
+
                 telemetry.addData("Max Velocity", maxVel)
                 telemetry.addData("Max Acceleration", maxAccel)
                 telemetry.update()
@@ -55,6 +62,6 @@ class SlidesConstraintsTuner : LinearOpMode() {
 
     companion object {
         @JvmField
-        var TIME_TO_RUN = 2.0
+        var TIME_TO_RUN = 1.5
     }
 }
diff --git a/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/...de/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt
@@ -31,8 +31,8 @@ class SlidesPIDTuner : CommandOpMode() {
         }).perpetually().schedule()
 
         RunCommand({
-            telemetry.addData("Slides Position: ", slidesSubsystem.slidePos)
-            telemetry.addData("Setpoint: ", target)
+            telemetry.addData("Slides Position", slidesSubsystem.slidePos)
+            telemetry.addData("Setpoint", target)
             telemetry.update()
         }).perpetually().schedule()
 

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
@@ -19,8 +19,8 @@ class ElevatorSubsystem(
         SlidesConstants.kI.value,
         SlidesConstants.kD.value,
         TrapezoidProfile.Constraints(
-            0.0,
-            0.0
+            1800.0,
+            1800.0
         )
     )
 
@@ -34,9 +34,9 @@ class ElevatorSubsystem(
         get() = extendMotors.positions[0]
 
     val slideVelocity: Double
-        get() = extendMotors.velocities[0]
+        get() = -extendMotors.velocities[0]
 
-    var enabled = false;
+    var enabled = true
 
     init {
         elevatorRight.inverted = true
@@ -47,11 +47,10 @@ class ElevatorSubsystem(
     }
 
     override fun periodic() {
-        controller.setPID(kP, kI, kD)
         val output = controller.calculate(slidePos)
 
         if (enabled)
-            extendMotors.set(output)
+            extendMotors.set(output + SlidesConstants.kG.value)
     }
 
     fun toggle() {
@@ -63,7 +62,7 @@ class ElevatorSubsystem(
     }
 
     fun stop() {
-        extendMotors.set(0.0)
+        extendMotors.set(kG)
     }
 
     companion object {
@@ -75,5 +74,8 @@ class ElevatorSubsystem(
 
         @JvmField
         var kD = 0.0
+
+        @JvmField
+        var kG = 0.1
     }
 }
diff --git a/...src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/...src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt
@@ -3,17 +3,18 @@ package org.firstinspires.ftc.teamcode.subsystems.slides
 import com.arcrobotics.ftclib.command.SubsystemBase
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.arcrobotics.ftclib.hardware.motors.MotorGroup
+import org.firstinspires.ftc.teamcode.constants.SlidesConstants
 
 class OpenElevatorSubsystem(
     //sets them as a private variable thats a motor (same name as in driver hub)
-    elevatorLeft: Motor,
-    elevatorRight: Motor,
+    elevatorRight : Motor,
+    elevatorLeft : Motor
 ) : SubsystemBase() {
     //makes a motor group bc you have to move them at the same time
-    private val elevatorMotors = MotorGroup(elevatorLeft, elevatorRight)
+    private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft)
 
     init {
-        elevatorLeft.inverted = true
+        elevatorRight.inverted = true
     }
 
     // Functions for moving slides up and down, number being speed, 1 being 100%
@@ -26,7 +27,7 @@ class OpenElevatorSubsystem(
     }
 
     fun stop() {
-        elevatorMotors.set(0.0)
+        elevatorMotors.set(SlidesConstants.kG.value)
     }
 
 }