From a217f542a691a9fc9ae8e5fc0f98a2e0dc082a40 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sat, 30 Nov 2024 20:37:11 -0500 Subject: [PATCH] slides pid --- .../teamcode/commands/drive/DriveCommand.kt | 6 +- .../ftc/teamcode/constants/ArmConstants.kt | 2 +- .../ftc/teamcode/constants/SlidesConstants.kt | 10 ++-- .../auto/blue/basket/left/Blue1Plus1.kt | 39 +++++++------ .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 56 ++++++++++++++----- .../opModes/tuning/slides/SlidesPIDTuner.kt | 22 +++++++- .../teamcode/subsystems/arm/ArmSubsystem.kt | 2 +- .../subsystems/arm/OpenArmSubsystem.kt | 7 ++- .../subsystems/intake/IntakeBeltSubsystem.kt | 16 +++++- .../subsystems/intake/IntakeSubsystem.kt | 6 +- .../subsystems/slides/ElevatorSubsystem.kt | 12 ++-- .../slides/OpenElevatorSubsystem.kt | 8 ++- 12 files changed, 124 insertions(+), 62 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt index 653b635..e3f53ef 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/drive/DriveCommand.kt @@ -17,9 +17,9 @@ class DriveCommand( override fun execute() { subsystem.drive( - leftY = zonedDrive(-leftY.invoke() * 0.7, zoneVal).pow(3), - leftX = zonedDrive(leftX.invoke() * 0.7, zoneVal).pow(3), - rightX = zonedDrive(rightX.invoke() * 0.7, zoneVal).pow(3), + leftY = zonedDrive(-leftY.invoke() * 0.9, zoneVal).pow(3), + leftX = zonedDrive(leftX.invoke() * 0.9, zoneVal).pow(3), + rightX = zonedDrive(rightX.invoke() * 0.9, zoneVal).pow(3), ) } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt index 2bb65f2..b607006 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ArmConstants.kt @@ -4,5 +4,5 @@ enum class ArmConstants(val value: Double) { kP(2.0), kI(0.0), kD(0.0), - kCos(0.01) + kCos(0.02) } \ No newline at end of file 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 index 80f460f..f04447e 100644 --- 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,9 +1,11 @@ package org.firstinspires.ftc.teamcode.constants enum class SlidesConstants(val value: Double) { - - kP(0.01), + kP(0.7), kI(0.0), - kD(0.000), - kG(0.115) + kD(0.0), + kG(0.12), + + MAX_HEIGHT_TICKS(2081.0), + MAX_HEIGHT_INCH(33.329) } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt index ade1c44..889d3f5 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt @@ -46,10 +46,14 @@ class Blue1Plus1: OpMode() { elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + .addTemporalMarker(1.0) { + intakeSubsystem.intake() + intakeBeltSubsystem.outtakePos() + } .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) - .back(4.3) + .back(4.0) .addTemporalMarker(3.0) { - armSubsystem.setpoint = Math.toRadians(97.5) + armSubsystem.setpoint = Math.toRadians(92.5) } .waitSeconds(1.0) .addTemporalMarker(4.0) { @@ -68,15 +72,15 @@ class Blue1Plus1: OpMode() { .addTemporalMarker(9.5) { elevatorSubsystem.setpoint = 0.0 } -// .waitSeconds(2.0) + .waitSeconds(2.0) // .addTemporalMarker(13.0) { -// armSubsystem.setpoint = 0.0 -// } -// .waitSeconds(1.0) -// .turn(Math.toRadians(25.0)) -// .addTemporalMarker(14.0) { -// elevatorSubsystem.setpoint = 1500.0 +// armSubsystem.setpoint = 10.0 // } + .waitSeconds(1.0) + .turn(Math.toRadians(25.0)) + .addTemporalMarker(14.0) { + elevatorSubsystem.setpoint = 1500.0 + } // .waitSeconds(2.0) // .addTemporalMarker(16.0) { // intakeBeltSubsystem.intakePos() @@ -104,14 +108,14 @@ class Blue1Plus1: OpMode() { // intakeSubsystem.outtake() // } // .waitSeconds(2.0) - .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) - .addTemporalMarker(12.0) { - armSubsystem.setpoint = 50.0 - } - .waitSeconds(1.0) - .addTemporalMarker(13.0) { - elevatorSubsystem.setpoint = 1000.0 - } +// .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) +// .addTemporalMarker(12.0) { +// armSubsystem.setpoint = 50.0 +// } +// .waitSeconds(1.0) +// .addTemporalMarker(13.0) { +// elevatorSubsystem.setpoint = 1000.0 +// } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose @@ -120,7 +124,6 @@ class Blue1Plus1: OpMode() { override fun loop() { driveSubsystem.update() - elevatorSubsystem.periodic() armSubsystem.periodic() } 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 index bdaadc7..5e733f1 100644 --- 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 @@ -1,5 +1,8 @@ package org.firstinspires.ftc.teamcode.opModes.teleOp +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.ConditionalCommand import com.arcrobotics.ftclib.command.InstantCommand @@ -14,15 +17,17 @@ import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand import org.firstinspires.ftc.teamcode.commands.drive.DriveCommand import org.firstinspires.ftc.teamcode.commands.elevator.SpinDownCommand import org.firstinspires.ftc.teamcode.commands.elevator.SpinUpCommand -import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand +import org.firstinspires.ftc.teamcode.constants.ArmConstants import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem +import kotlin.math.cos @TeleOp +@Config class MainTeleOp : CommandOpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor @@ -38,7 +43,6 @@ class MainTeleOp : CommandOpMode() { private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem - private lateinit var spinUpCommand: SpinUpCommand private lateinit var spinDownCommand: SpinDownCommand @@ -47,9 +51,7 @@ class MainTeleOp : CommandOpMode() { private lateinit var driveCommand: DriveCommand - private lateinit var intakeCommand: IntakeBeltCommand - private lateinit var outtakeCommand: IntakeBeltCommand - + private lateinit var beltCommand: ConditionalCommand private lateinit var clawCommand: ConditionalCommand private lateinit var driver: GamepadEx @@ -59,6 +61,8 @@ class MainTeleOp : CommandOpMode() { private lateinit var operatorRight: Trigger override fun initialize() { + telemetry = MultipleTelemetry(FtcDashboard.getInstance().telemetry, telemetry) + driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) @@ -71,8 +75,10 @@ class MainTeleOp : CommandOpMode() { intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) - armSubsystem = OpenArmSubsystem(armRight, armLeft) + armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 } slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + armSubsystem = OpenArmSubsystem(armRight, armLeft, slidesSubsystem::slidePos) + driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) @@ -83,41 +89,61 @@ class MainTeleOp : CommandOpMode() { armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) - intakeCommand = IntakeBeltCommand(true, intakeBeltSubsystem) - outtakeCommand = IntakeBeltCommand(false, intakeBeltSubsystem) - clawCommand = ConditionalCommand( InstantCommand({ intakeSubsystem.intake() }), InstantCommand({ intakeSubsystem.outtake() }), intakeSubsystem::intakePos ) + beltCommand = ConditionalCommand( + InstantCommand({ intakeBeltSubsystem.intakePos() }), + InstantCommand({ intakeBeltSubsystem.outtakePos() }), + intakeBeltSubsystem::beltPos + ) + 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.RIGHT_BUMPER).whileHeld(armUpCommand) - operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand) + operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand) + operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand) operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(clawCommand) + operator.getGamepadButton(GamepadKeys.Button.A).whenPressed(beltCommand) operatorLeft = Trigger { - operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) != 0.0 + operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) == 1.0 } operatorRight = Trigger { - operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) != 0.0 + operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) == 1.0 } - operatorLeft.whileActiveContinuous(outtakeCommand) - operatorRight.whileActiveContinuous(intakeCommand) + operatorRight.whileActiveContinuous( + InstantCommand({ intakeBeltSubsystem.increasePos() }) + ) + + operatorLeft.whileActiveContinuous( + InstantCommand({ intakeBeltSubsystem.decreasePos() }) + ) driveSubsystem.defaultCommand = driveCommand RunCommand({ telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle)) telemetry.addData("Intake Position", intakeSubsystem.intakePos) + + telemetry.addData("Slide Position", slidesSubsystem.slidePos) + telemetry.addData("Arm Correction", + ArmConstants.kCos.value * cos(armSubsystem.armAngle) + + kT * slidesSubsystem.slidePos + ) telemetry.update() }).perpetually().schedule() } + + companion object { + @JvmField + var kT = 0.05 + } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt index 7bd5966..9c96cd0 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt @@ -5,10 +5,13 @@ 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.arcrobotics.ftclib.gamepad.GamepadEx +import com.arcrobotics.ftclib.gamepad.GamepadKeys import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import org.firstinspires.ftc.teamcode.commands.arm.OpenArmCommand import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem +import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @TeleOp @@ -20,10 +23,17 @@ class SlidesPIDTuner : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var armSubsystem: ArmSubsystem + private lateinit var armSubsystem: OpenArmSubsystem private lateinit var slidesSubsystem: ElevatorSubsystem + private lateinit var armUpCommand: OpenArmCommand + private lateinit var armDownCommand: OpenArmCommand + + private lateinit var operator: GamepadEx + override fun initialize() { + operator = GamepadEx(gamepad2) + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) @@ -32,9 +42,15 @@ class SlidesPIDTuner : CommandOpMode() { slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) - armSubsystem = ArmSubsystem(armRight, armLeft) + armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 } slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle) + armUpCommand = OpenArmCommand(armSubsystem, true) + armDownCommand = OpenArmCommand(armSubsystem, false) + + operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand) + operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand) + RunCommand({ slidesSubsystem.setpoint = target }).perpetually().schedule() 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 index bb2b606..1eea847 100644 --- 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 @@ -13,7 +13,7 @@ import kotlin.math.PI @Config class ArmSubsystem( armRight: Motor, - armLeft: Motor + armLeft: Motor, ) : PIDSubsystem( PIDController( ArmConstants.kP.value, diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt index 385aa45..11d128f 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt @@ -12,8 +12,9 @@ import kotlin.math.cos @Config class OpenArmSubsystem( // Here I am just declaring the motors and what they are called on our driver hub. - armRight : Motor, - armLeft : Motor, + armRight : Motor, + armLeft : Motor, + private val slidePos: () -> Double ) : SubsystemBase() { //Here I am making a motor group, as the arm motors are going to work together to to turn the slides. @@ -44,6 +45,6 @@ class OpenArmSubsystem( companion object { @JvmField - var kCos = 0.5 + var kCos = 0.05 } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt index f9c3ecc..a15f869 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt @@ -6,12 +6,24 @@ import com.qualcomm.robotcore.hardware.Servo class IntakeBeltSubsystem( private val intakeBelt: Servo ) : SubsystemBase() { + var beltPos = false + fun outtakePos() { - intakeBelt.position += 0.01 + intakeBelt.position = 0.25 + beltPos = !beltPos } fun intakePos() { - intakeBelt.position -= 0.01 + intakeBelt.position = 0.0 + beltPos = !beltPos + } + + fun increasePos() { + intakeBelt.position += 0.005 + } + + fun decreasePos() { + intakeBelt.position -= 0.005 } fun setPos(pos: Double) { 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 index 820ff81..b10037c 100644 --- 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 @@ -10,12 +10,12 @@ class IntakeSubsystem( fun intake() { intake.position = 0.6 - intakePos = true + intakePos = !intakePos } fun outtake() { - intake.position = 0.0 - intakePos = false + intake.position = 1.0 + intakePos = !intakePos } fun setSpeed(speed: Double) { intake.position = speed } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt index dce215a..9f07512 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt @@ -21,8 +21,8 @@ class ElevatorSubsystem( SlidesConstants.kI.value, SlidesConstants.kD.value, TrapezoidProfile.Constraints( - 1600.0, - 700.0 + 30.0, // in/s + 30.0 // in/s^2 ) ) @@ -33,10 +33,10 @@ class ElevatorSubsystem( } val slidePos: Double - get() = extendMotors.positions[0] + get() = extendMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value val slideVelocity: Double - get() = -extendMotors.velocities[0] + get() = -extendMotors.velocities[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value var enabled = true @@ -71,7 +71,7 @@ class ElevatorSubsystem( companion object { @JvmField - var kP = 0.01 + var kP = 0.7 // kP = 0.01 @JvmField @@ -81,7 +81,7 @@ class ElevatorSubsystem( var kD = 0.0 @JvmField - var kG = 0.115 + var kG = 0.12 // kG = 0.115 } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt index a7f36fc..716250b 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt @@ -14,18 +14,20 @@ class OpenElevatorSubsystem( ) : SubsystemBase() { //makes a motor group bc you have to move them at the same time private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft) + val slidePos: Double + get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value init { - elevatorRight.inverted = true + elevatorLeft.inverted = true } // Functions for moving slides up and down, number being speed, 1 being 100% fun up() { - elevatorMotors.set(0.7) + elevatorMotors.set(1.0) } fun down() { - elevatorMotors.set(-0.7) + elevatorMotors.set(-1.0) } fun stop() {