diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt index 4587917..9fb0a0b 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinDownCommand.kt @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem class SpinDownCommand( - private val subsystem: OpenSlidesSubsystem + private val subsystem: OpenElevatorSubsystem ) : CommandBase() { diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt index 75781a5..fd8c755 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/elevator/SpinUpCommand.kt @@ -1,10 +1,10 @@ package org.firstinspires.ftc.teamcode.commands.elevator import com.arcrobotics.ftclib.command.CommandBase -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem class SpinUpCommand( - private val subsystem: OpenSlidesSubsystem + private val subsystem: OpenElevatorSubsystem ) : CommandBase() { override fun execute() { 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 615497f..396f5df 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,7 +1,7 @@ package org.firstinspires.ftc.teamcode.constants enum class SlidesConstants(val value: Double) { - kP(0.0), + kP(0.03), kI(0.0), - kD(0.0), + kD(0.0004), } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt index e3bd71d..4abb690 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/BlueHighBasketLeft.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Blue High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") class BlueHighBasketLeft: OpMode() { @@ -23,7 +23,7 @@ class BlueHighBasketLeft: OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { // intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,17 +37,17 @@ class BlueHighBasketLeft: OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) // intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .turn(Math.toRadians(90.0)) - .forward(60.0) + .turn(Math.toRadians(-90.0)) + .back(60.0) .addTemporalMarker(3.0) { armSubsystem.setpoint = Math.toRadians(95.0) } .waitSeconds(1.0) .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 250.0 + elevatorSubsystem.setpoint = 1000.0 } .waitSeconds(2.0) // .addTemporalMarker(5.0){ @@ -55,11 +55,11 @@ class BlueHighBasketLeft: OpMode() { // } .waitSeconds(1.0) .addTemporalMarker(10.0) { - elevatorSubsystem.setpoint = 0.0 + elevatorSubsystem.setpoint = 50.0 } .waitSeconds(2.0) .addTemporalMarker(13.0) { - armSubsystem.setpoint = Math.toRadians(0.0) + armSubsystem.setpoint = Math.toRadians(5.0) } .build() diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt index 5247a0f..df22559 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/right/BlueHighBasketRight.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Blue High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") @@ -25,7 +25,7 @@ class BlueHighBasketRight() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -39,17 +39,17 @@ class BlueHighBasketRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) .turn(Math.toRadians(90.0)) .forward(35.0) .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) + armSubsystem.setpoint = Math.toRadians(90.0) } .waitSeconds(1.0) .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 2.0 + elevatorSubsystem.setpoint = 200.0 } .waitSeconds(2.0) .addTemporalMarker(5.0){ diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt index 3f88cc4..edd74ff 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/left/BlueHighChamberLeft.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Blue High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") class BlueHighChamberLeft() : OpMode() { @@ -23,7 +23,7 @@ class BlueHighChamberLeft() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,7 +37,7 @@ class BlueHighChamberLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt index 8b43c28..8fa8f29 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/chamber/right/BlueHighChamberRight.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Blue High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") class BlueHighChamberRight() : OpMode() { @@ -23,7 +23,7 @@ class BlueHighChamberRight() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,7 +37,7 @@ class BlueHighChamberRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt index 2b92748..2a6f8c3 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/left/RedHighBasketLeft.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Red High Basket Left", group = "Basket", preselectTeleOp = "MainTeleOp") class RedHighBasketLeft() : OpMode() { @@ -24,7 +24,7 @@ class RedHighBasketLeft() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -38,7 +38,7 @@ class RedHighBasketLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) .turn(Math.toRadians(90.0)) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt index 7bb7607..ce72409 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/basket/right/RedHighBasketRight.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Red High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") class RedHighBasketRight() : OpMode() { @@ -23,7 +23,7 @@ class RedHighBasketRight() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,7 +37,7 @@ class RedHighBasketRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) .turn(Math.toRadians(90.0)) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt index c624bea..35bb790 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/left/RedHighChamberLeft.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Red High Chamber Left", group = "Chamber", preselectTeleOp = "MainTeleOp") class RedHighChamberLeft() : OpMode() { @@ -23,7 +23,7 @@ class RedHighChamberLeft() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,7 +37,7 @@ class RedHighChamberLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_LEFT.startPose) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt index e92c9cc..e9c2d49 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/red/chamber/right/RedHighChamberRight.kt @@ -9,7 +9,7 @@ import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Red High Chamber Right", group = "Chamber", preselectTeleOp = "MainTeleOp") class RedHighChamberRight() : OpMode() { @@ -23,7 +23,7 @@ class RedHighChamberRight() : OpMode() { private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var armSubsystem: ArmSubsystem - private lateinit var elevatorSubsystem: SlidesSubsytem + private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) @@ -37,7 +37,7 @@ class RedHighChamberRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = SlidesSubsytem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.RED_RIGHT.startPose) 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 c7b4e29..093c604 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 @@ -15,7 +15,7 @@ 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.IntakeSubsystem -import org.firstinspires.ftc.teamcode.subsystems.slides.OpenSlidesSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.OpenElevatorSubsystem @TeleOp class MainTeleOp : CommandOpMode() { @@ -25,7 +25,7 @@ class MainTeleOp : CommandOpMode() { private lateinit var armRight: Motor //private lateinit var intake: CRServo - private lateinit var slidesSubsystem: OpenSlidesSubsystem + private lateinit var slidesSubsystem: OpenElevatorSubsystem private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -54,7 +54,7 @@ class MainTeleOp : CommandOpMode() { //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - slidesSubsystem = OpenSlidesSubsystem(elevatorRight, elevatorLeft) + slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft) armSubsystem = OpenArmSubsystem(armRight, armLeft) driveSubsystem = DriveSubsystem(hardwareMap) //intakeSubsystem = IntakeSubsystem(intake) 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 dcf5eea..40a6d90 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 @@ -8,7 +8,7 @@ import com.arcrobotics.ftclib.command.RunCommand import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.teamcode.constants.ControlBoard -import org.firstinspires.ftc.teamcode.subsystems.slides.SlidesSubsytem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @TeleOp @Config @@ -16,7 +16,7 @@ class SlidesPIDTuner : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var slidesSubsystem: SlidesSubsytem + private lateinit var slidesSubsystem: ElevatorSubsystem override fun initialize() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) @@ -24,7 +24,7 @@ class SlidesPIDTuner : CommandOpMode() { armLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) armRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) - slidesSubsystem = SlidesSubsytem(armRight, armLeft) + slidesSubsystem = ElevatorSubsystem(armRight, armLeft) RunCommand({ slidesSubsystem.setpoint = target diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt similarity index 79% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt index f1e2f6a..edc615e 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/SlidesSubsytem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt @@ -8,7 +8,7 @@ import org.firstinspires.ftc.teamcode.constants.SlidesConstants import org.firstinspires.ftc.teamcode.utils.PIDSubsystem @Config -class SlidesSubsytem( +class ElevatorSubsystem( elevatorRight : Motor, elevatorLeft : Motor ) : PIDSubsystem( @@ -28,14 +28,27 @@ class SlidesSubsytem( init { elevatorRight.inverted = true + elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) extendMotors.resetEncoder() extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } override fun useOutput(output: Double, setpoint: Double) { + controller.setPIDF(kP, kI, kD, 0.0) extendMotors.set(output) } override fun getMeasurement() = slidePos + + companion object { + @JvmField + var kP = 0.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 + } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt similarity index 96% rename from TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt rename to TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt index 3839f81..59af25c 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenSlidesSubsystem.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt @@ -4,7 +4,7 @@ import com.arcrobotics.ftclib.command.SubsystemBase import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup -class OpenSlidesSubsystem( +class OpenElevatorSubsystem( //sets them as a private variable thats a motor (same name as in driver hub) elevatorLeft: Motor, elevatorRight: Motor,