merge

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

Hide file tree

Showing 2 changed files with 24 additions and 20 deletions.
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,29 +1,18 @@
-import com.acmerobotics.dashboard.FtcDashboard
-import com.acmerobotics.dashboard.telemetry.MultipleTelemetry
+package org.firstinspires.ftc.teamcode.opModes.tuning.slides
+
 import com.arcrobotics.ftclib.hardware.motors.Motor
 import com.qualcomm.robotcore.eventloop.opmode.Autonomous
 import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode
-import com.qualcomm.robotcore.hardware.Servo
-import com.qualcomm.robotcore.hardware.TouchSensor
 import com.qualcomm.robotcore.util.ElapsedTime
-import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.hardwareMap
-import org.firstinspires.ftc.robotcore.external.BlocksOpModeCompanion.telemetry
 import org.firstinspires.ftc.teamcode.constants.ControlBoard
-import org.firstinspires.ftc.teamcode.constants.SlidesConstants
-import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem
+import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem
 
 @Autonomous
 class SlidesConstraintsTuner : LinearOpMode() {
-    private lateinit var flipperServo: Servo
     private lateinit var leftMotor: Motor
     private lateinit var rightMotor: Motor
 
-    private lateinit var limit: TouchSensor
-
-    private lateinit var elevator: OpenElevatorSubsystem
-
-    private var lastPos = 0.0
-    private var lastVel = 0.0
+    private lateinit var elevator: ElevatorSubsystem
 
     private var maxVel = 0.0
     private var maxAccel = 0.0
@@ -33,7 +22,7 @@ class SlidesConstraintsTuner : LinearOpMode() {
         leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName)
         rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName)
 
-        elevator = OpenElevatorSubsystem(leftMotor, rightMotor)
+        elevator = ElevatorSubsystem(rightMotor, leftMotor)
         waitForStart()
 
         var dt = 0L
@@ -42,12 +31,12 @@ class SlidesConstraintsTuner : LinearOpMode() {
         timer.reset()
         while (opModeIsActive()) {
             while (timer.seconds() < TIME_TO_RUN) {
-                elevator.up()
+                elevator.spinUp()
 
                 dt = timer.nanoseconds() - dt
-                dv = elevator.currentVel - dv / dt
+                dv = elevator.slideVelocity - dv / dt
 
-                maxVel = elevator.currentVel.coerceAtLeast(maxVel)
+                maxVel = elevator.slideVelocity.coerceAtLeast(maxVel)
                 maxAccel = dv.coerceAtLeast(maxAccel)
 
                 telemetry.addData("Max Velocity", maxVel)

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
@@ -36,6 +36,8 @@ class ElevatorSubsystem(
     val slideVelocity: Double
         get() = extendMotors.velocities[0]
 
+    var enabled = false;
+
     init {
         elevatorRight.inverted = true
         elevatorRight.encoder.setDirection(Motor.Direction.REVERSE)
@@ -48,7 +50,20 @@ class ElevatorSubsystem(
         controller.setPID(kP, kI, kD)
         val output = controller.calculate(slidePos)
 
-        extendMotors.set(output)
+        if (enabled)
+            extendMotors.set(output)
+    }
+
+    fun toggle() {
+        enabled != enabled
+    }
+
+    fun spinUp() {
+        extendMotors.set(1.0);
+    }
+
+    fun stop() {
+        extendMotors.set(0.0)
     }
 
     companion object {