From 34accb41cd6409ecc1d0d954b6b03d3056cf3742 Mon Sep 17 00:00:00 2001 From: AdityaP241 Date: Thu, 28 Nov 2024 12:34:13 -0500 Subject: [PATCH 01/21] slides pid tuning --- .../commands/elevator/SpinDownCommand.kt | 4 ++-- .../teamcode/commands/elevator/SpinUpCommand.kt | 4 ++-- .../ftc/teamcode/constants/SlidesConstants.kt | 4 ++-- .../auto/blue/basket/left/BlueHighBasketLeft.kt | 16 ++++++++-------- .../blue/basket/right/BlueHighBasketRight.kt | 10 +++++----- .../blue/chamber/left/BlueHighChamberLeft.kt | 6 +++--- .../blue/chamber/right/BlueHighChamberRight.kt | 6 +++--- .../auto/red/basket/left/RedHighBasketLeft.kt | 6 +++--- .../auto/red/basket/right/RedHighBasketRight.kt | 6 +++--- .../auto/red/chamber/left/RedHighChamberLeft.kt | 6 +++--- .../red/chamber/right/RedHighChamberRight.kt | 6 +++--- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 6 +++--- .../opModes/tuning/slides/SlidesPIDTuner.kt | 6 +++--- .../{SlidesSubsytem.kt => ElevatorSubsystem.kt} | 15 ++++++++++++++- ...idesSubsystem.kt => OpenElevatorSubsystem.kt} | 2 +- 15 files changed, 58 insertions(+), 45 deletions(-) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/{SlidesSubsytem.kt => ElevatorSubsystem.kt} (79%) rename TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/slides/{OpenSlidesSubsystem.kt => OpenElevatorSubsystem.kt} (96%) 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, From da58e149b4d33b6278830035fd668c7266fac2c2 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 28 Nov 2024 12:43:25 -0500 Subject: [PATCH 02/21] merge --- .../tuning/slides/SlidesConstraintsTuner.kt | 71 +++++++++++++++++++ .../subsystems/slides/ElevatorSubsystem.kt | 30 +++++--- 2 files changed, 91 insertions(+), 10 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt new file mode 100644 index 0000000..2300e64 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt @@ -0,0 +1,71 @@ +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 +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 + +@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 var maxVel = 0.0 + private var maxAccel = 0.0 + + private val timer = ElapsedTime(ElapsedTime.Resolution.SECONDS) + override fun runOpMode() { + leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + + elevator = OpenElevatorSubsystem(leftMotor, rightMotor) + waitForStart() + + var dt = 0L + var dv = 0.0 + + timer.reset() + while (opModeIsActive()) { + while (timer.seconds() < TIME_TO_RUN) { + elevator.up() + + dt = timer.nanoseconds() - dt + dv = elevator.currentVel - dv / dt + + maxVel = elevator.currentVel.coerceAtLeast(maxVel) + maxAccel = dv.coerceAtLeast(maxAccel) + + telemetry.addData("Max Velocity", maxVel) + telemetry.addData("Max Acceleration", maxAccel) + telemetry.update() + + + if (isStopRequested) break + } + + telemetry.addData("Final Max Velocity", maxVel) + telemetry.addData("Final Max Acceleration", maxAccel) + telemetry.update() + } + } + + companion object { + @JvmField + var TIME_TO_RUN = 2.0 + } +} \ No newline at end of file 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 edc615e..04f0edd 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 @@ -1,24 +1,34 @@ package org.firstinspires.ftc.teamcode.subsystems.slides import com.acmerobotics.dashboard.config.Config -import com.arcrobotics.ftclib.controller.PIDController +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 org.firstinspires.ftc.teamcode.constants.SlidesConstants -import org.firstinspires.ftc.teamcode.utils.PIDSubsystem @Config class ElevatorSubsystem( elevatorRight : Motor, elevatorLeft : Motor -) : PIDSubsystem( - PIDController( +) : SubsystemBase() { + private val extendMotors = MotorGroup(elevatorRight, elevatorLeft) + private val controller = ProfiledPIDController( SlidesConstants.kP.value, SlidesConstants.kI.value, SlidesConstants.kD.value, + TrapezoidProfile.Constraints( + 0.0, + 0.0 + ) ) -) { - private val extendMotors = MotorGroup(elevatorRight, elevatorLeft) + + var setpoint = 0.0 + set(value) { + controller.goal = TrapezoidProfile.State(value, 0.0) + field = value + } val slidePos: Double get() = extendMotors.positions[0] @@ -34,13 +44,13 @@ class ElevatorSubsystem( extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } - override fun useOutput(output: Double, setpoint: Double) { - controller.setPIDF(kP, kI, kD, 0.0) + override fun periodic() { + controller.setPID(kP, kI, kD) + val output = controller.calculate(slidePos) + extendMotors.set(output) } - override fun getMeasurement() = slidePos - companion object { @JvmField var kP = 0.0 From 8d92ac1577692f536febcb4a41f7d1a3e250c93a Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 28 Nov 2024 12:50:42 -0500 Subject: [PATCH 03/21] merge --- .../tuning/slides/SlidesConstraintsTuner.kt | 27 ++++++------------- .../subsystems/slides/ElevatorSubsystem.kt | 17 +++++++++++- 2 files changed, 24 insertions(+), 20 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt index 2300e64..62abe1c 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt +++ b/TeamCode/src/main/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/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 04f0edd..8d61f06 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 @@ -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 { From e0f0659b9254906629750b49829324014f9476f8 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 28 Nov 2024 14:26:04 -0500 Subject: [PATCH 04/21] Profile le arm --- .../ftc/teamcode/constants/SlidesConstants.kt | 5 +++-- .../tuning/slides/SlidesConstraintsTuner.kt | 9 ++++++++- .../opModes/tuning/slides/SlidesPIDTuner.kt | 4 ++-- .../subsystems/slides/ElevatorSubsystem.kt | 16 +++++++++------- .../subsystems/slides/OpenElevatorSubsystem.kt | 11 ++++++----- 5 files changed, 28 insertions(+), 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 index 396f5df..18cf039 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,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) } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt index 62abe1c..92ce120 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt +++ b/TeamCode/src/main/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 } } \ 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 40a6d90..8416ff1 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 @@ -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/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 8d61f06..463fe52 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 @@ -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 } } \ No newline at end of file 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 59af25c..523ecbf 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 @@ -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) } } \ No newline at end of file From 4a50493461bac976f35266fce2e13d9499c3c3b7 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Thu, 28 Nov 2024 16:01:54 -0500 Subject: [PATCH 05/21] sachet --- .../auto/blue/basket/left/BlueHighBasketLeft.kt | 2 +- .../auto/blue/basket/right/BlueHighBasketRight.kt | 2 +- .../auto/blue/chamber/left/BlueHighChamberLeft.kt | 2 +- .../blue/chamber/right/BlueHighChamberRight.kt | 2 +- .../auto/red/basket/left/RedHighBasketLeft.kt | 2 +- .../auto/red/basket/right/RedHighBasketRight.kt | 2 +- .../auto/red/chamber/left/RedHighChamberLeft.kt | 2 +- .../auto/red/chamber/right/RedHighChamberRight.kt | 2 +- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 4 ++-- .../tuning/slides/SlidesConstraintsTuner.kt | 2 +- .../opModes/tuning/slides/SlidesPIDTuner.kt | 15 ++++++++++++--- .../teamcode/subsystems/arm/OpenArmSubsystem.kt | 7 ++++--- .../subsystems/slides/ElevatorSubsystem.kt | 6 ++++-- .../subsystems/slides/OpenElevatorSubsystem.kt | 6 ++++-- 14 files changed, 35 insertions(+), 21 deletions(-) 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 4abb690..3530290 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 @@ -37,7 +37,7 @@ class BlueHighBasketLeft: OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) // intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .turn(Math.toRadians(-90.0)) 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 df22559..96b881c 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 @@ -39,7 +39,7 @@ class BlueHighBasketRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_RIGHT.startPose) .turn(Math.toRadians(90.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 edd74ff..eee7f89 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 @@ -37,7 +37,7 @@ class BlueHighChamberLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 8fa8f29..ceecd1a 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 @@ -37,7 +37,7 @@ class BlueHighChamberRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 2a6f8c3..74630a7 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 @@ -38,7 +38,7 @@ class RedHighBasketLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 ce72409..657d3ef 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 @@ -37,7 +37,7 @@ class RedHighBasketRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 35bb790..fe3b6a5 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 @@ -37,7 +37,7 @@ class RedHighChamberLeft() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 e9c2d49..671b539 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 @@ -37,7 +37,7 @@ class RedHighChamberRight() : OpMode() { driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) - elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) 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 093c604..6167ae3 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 @@ -54,8 +54,8 @@ class MainTeleOp : CommandOpMode() { //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) - slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft) armSubsystem = OpenArmSubsystem(armRight, armLeft) + slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) driveSubsystem = DriveSubsystem(hardwareMap) //intakeSubsystem = IntakeSubsystem(intake) @@ -80,7 +80,7 @@ class MainTeleOp : CommandOpMode() { driveSubsystem.defaultCommand = driveCommand RunCommand({ - telemetry.addData("Arm Position: ", armSubsystem.armAngle) + telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle)) telemetry.update() }).perpetually().schedule() } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt index 92ce120..c72bf98 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/slides/SlidesConstraintsTuner.kt @@ -26,7 +26,7 @@ class SlidesConstraintsTuner : LinearOpMode() { leftMotor = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) rightMotor = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) - elevator = ElevatorSubsystem(rightMotor, leftMotor) + elevator = ElevatorSubsystem(rightMotor, leftMotor) { 0.0 } waitForStart() var dt = 0L 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 8416ff1..00808a2 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,23 +8,32 @@ 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.arm.ArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @TeleOp @Config class SlidesPIDTuner : CommandOpMode() { + private lateinit var slidesLeft: Motor + private lateinit var slidesRight: Motor + private lateinit var armLeft: Motor private lateinit var armRight: Motor + private lateinit var armSubsystem: ArmSubsystem private lateinit var slidesSubsystem: ElevatorSubsystem override fun initialize() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - armLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) - armRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60) + + slidesLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) + slidesRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) - slidesSubsystem = ElevatorSubsystem(armRight, armLeft) + armSubsystem = ArmSubsystem(armRight, armLeft) + slidesSubsystem = ElevatorSubsystem(slidesRight, slidesLeft, armSubsystem::armAngle) RunCommand({ slidesSubsystem.setpoint = target 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 722f487..385aa45 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 @@ -5,21 +5,22 @@ import com.arcrobotics.ftclib.command.SubsystemBase import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA import com.arcrobotics.ftclib.hardware.motors.MotorGroup +import org.firstinspires.ftc.teamcode.constants.ArmConstants import kotlin.math.PI +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, - ) : SubsystemBase() { //Here I am making a motor group, as the arm motors are going to work together to to turn the slides. private val turnMotors = MotorGroup(armRight, armLeft) val armAngle: Double - get() = turnMotors.positions[0] / GoBILDA.RPM_312.cpr * 2 * PI + get() = turnMotors.positions[0] / GoBILDA.RPM_60.cpr * PI init { armLeft.inverted = true @@ -38,7 +39,7 @@ class OpenArmSubsystem( } fun stop() { - turnMotors.set(kCos * Math.cos(armAngle)) + turnMotors.set(ArmConstants.kCos.value * cos(armAngle)) } companion object { 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 463fe52..6aeee62 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 @@ -7,11 +7,13 @@ import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup import com.arcrobotics.ftclib.trajectory.TrapezoidProfile import org.firstinspires.ftc.teamcode.constants.SlidesConstants +import kotlin.math.sin @Config class ElevatorSubsystem( elevatorRight : Motor, - elevatorLeft : Motor + elevatorLeft : Motor, + private val slideAngle: () -> Double ) : SubsystemBase() { private val extendMotors = MotorGroup(elevatorRight, elevatorLeft) private val controller = ProfiledPIDController( @@ -50,7 +52,7 @@ class ElevatorSubsystem( val output = controller.calculate(slidePos) if (enabled) - extendMotors.set(output + SlidesConstants.kG.value) + extendMotors.set(output + SlidesConstants.kG.value * sin(slideAngle.invoke())) } fun toggle() { 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 523ecbf..e49c72d 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 @@ -4,11 +4,13 @@ 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 +import kotlin.math.sin class OpenElevatorSubsystem( //sets them as a private variable thats a motor (same name as in driver hub) elevatorRight : Motor, - elevatorLeft : Motor + elevatorLeft : Motor, + private val slideAngle: () -> Double ) : SubsystemBase() { //makes a motor group bc you have to move them at the same time private val elevatorMotors = MotorGroup(elevatorRight, elevatorLeft) @@ -27,7 +29,7 @@ class OpenElevatorSubsystem( } fun stop() { - elevatorMotors.set(SlidesConstants.kG.value) + elevatorMotors.set(SlidesConstants.kG.value * sin(slideAngle.invoke())) } } \ No newline at end of file From c78550e3d74d56ff8a83682cb1ce53b2183ed802 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Thu, 28 Nov 2024 18:06:48 -0500 Subject: [PATCH 06/21] Slides Constants --- .../firstinspires/ftc/teamcode/constants/SlidesConstants.kt | 6 +++--- .../ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt | 1 - .../ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt | 5 +++-- 3 files changed, 6 insertions(+), 6 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 index 18cf039..fc108f0 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,8 +1,8 @@ package org.firstinspires.ftc.teamcode.constants enum class SlidesConstants(val value: Double) { - kP(0.02), + kP(0.014), kI(0.0), - kD(0.00045), - kG(0.1) + kD(0.0), + kG(0.2) } \ 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 00808a2..4362548 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 @@ -50,7 +50,6 @@ class SlidesPIDTuner : CommandOpMode() { companion object { @JvmField - // NOTE THAT THIS VALUE SHOULD BE DEGREES var target = 0.0 } } \ No newline at end of file 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 6aeee62..9ca765a 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 @@ -49,6 +49,7 @@ class ElevatorSubsystem( } override fun periodic() { + controller.setPID(kP, kI, kD) val output = controller.calculate(slidePos) if (enabled) @@ -69,7 +70,7 @@ class ElevatorSubsystem( companion object { @JvmField - var kP = 0.0 + var kP = 0.014 @JvmField var kI = 0.0 @@ -78,6 +79,6 @@ class ElevatorSubsystem( var kD = 0.0 @JvmField - var kG = 0.1 + var kG = 0.2 } } \ No newline at end of file From f8185b5f6f15ca44d2189dbbc551566967b8e879 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Thu, 28 Nov 2024 18:56:00 -0500 Subject: [PATCH 07/21] values for slides tuned but with freefall --- .../firstinspires/ftc/teamcode/constants/ArmConstants.kt | 2 +- .../ftc/teamcode/constants/SlidesConstants.kt | 7 ++++--- .../ftc/teamcode/opModes/tuning/slides/SlidesPIDTuner.kt | 1 + .../ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt | 9 +++++---- 4 files changed, 11 insertions(+), 8 deletions(-) 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 198ff1a..44fb0b8 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(1.0), kI(0.0), kD(0.08), - kCos(0.004) + kCos(0.0045) } \ 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 fc108f0..80f460f 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,8 +1,9 @@ package org.firstinspires.ftc.teamcode.constants enum class SlidesConstants(val value: Double) { - kP(0.014), + + kP(0.01), kI(0.0), - kD(0.0), - kG(0.2) + kD(0.000), + kG(0.115) } \ 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 4362548..815923d 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 @@ -42,6 +42,7 @@ class SlidesPIDTuner : CommandOpMode() { RunCommand({ telemetry.addData("Slides Position", slidesSubsystem.slidePos) telemetry.addData("Setpoint", target) + telemetry.addData("arm angle", armSubsystem.armAngle) telemetry.update() }).perpetually().schedule() 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 9ca765a..b3141c9 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( - 1800.0, - 1800.0 + 2000.0, + 1000.0 ) ) @@ -50,10 +50,11 @@ class ElevatorSubsystem( override fun periodic() { controller.setPID(kP, kI, kD) + val output = controller.calculate(slidePos) if (enabled) - extendMotors.set(output + SlidesConstants.kG.value * sin(slideAngle.invoke())) + extendMotors.set(output + kG * sin(slideAngle.invoke())) } fun toggle() { @@ -79,6 +80,6 @@ class ElevatorSubsystem( var kD = 0.0 @JvmField - var kG = 0.2 + var kG = 0.115 } } \ No newline at end of file From 27e19b216135e5b46ffabb9e66edc0a1d7b3c2eb Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Fri, 29 Nov 2024 10:52:48 -0500 Subject: [PATCH 08/21] hardware map --- .../ftc/teamcode/constants/ArmConstants.kt | 2 +- .../ftc/teamcode/constants/ControlBoard.kt | 6 ++--- .../blue/basket/left/BlueHighBasketLeft.kt | 22 ++++++++++--------- .../blue/chamber/left/BlueHighChamberLeft.kt | 2 +- .../opModes/tuning/slides/SlidesPIDTuner.kt | 1 + .../subsystems/intake/IntakeBeltSubsytem.kt | 12 ++++++++++ .../subsystems/slides/ElevatorSubsystem.kt | 14 +++++++----- 7 files changed, 39 insertions(+), 20 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt 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 44fb0b8..50971b8 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(1.0), kI(0.0), kD(0.08), - kCos(0.0045) + kCos(0.01) } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt index f2bf6c0..3d31684 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt @@ -20,8 +20,8 @@ enum class ControlBoard(val deviceName: String) { SLIDES_RIGHT("rightSlideString"), //Intake - INTAKE(""), + INTAKE("rollerIntake"), + INTAKE_BELT("intakeBelt") + - // Camera - CAMERA("lifeCam") } 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 3530290..461264b 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 @@ -26,7 +26,7 @@ class BlueHighBasketLeft: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { -// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) @@ -35,29 +35,31 @@ class BlueHighBasketLeft: OpMode() { elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) driveSubsystem = DriveSubsystem(hardwareMap) -// intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) armSubsystem = ArmSubsystem(armRight, armLeft) elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .turn(Math.toRadians(-90.0)) .back(60.0) + .turn(Math.toRadians(20.0)) + .back(5.0) .addTemporalMarker(3.0) { armSubsystem.setpoint = Math.toRadians(95.0) } .waitSeconds(1.0) - .addTemporalMarker(6.0) { - elevatorSubsystem.setpoint = 1000.0 + .addTemporalMarker(5.0) { + elevatorSubsystem.setpoint = 1900.0 } .waitSeconds(2.0) -// .addTemporalMarker(5.0){ -// intakeSubsystem.outtake() -// } + .addTemporalMarker(6.0){ + intakeSubsystem.outtake() + } .waitSeconds(1.0) - .addTemporalMarker(10.0) { - elevatorSubsystem.setpoint = 50.0 + .addTemporalMarker(9.0) { + elevatorSubsystem.setpoint = 0.0 } - .waitSeconds(2.0) + .waitSeconds(5.0) .addTemporalMarker(13.0) { armSubsystem.setpoint = Math.toRadians(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 eee7f89..6b537df 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 @@ -44,7 +44,7 @@ class BlueHighChamberLeft() : OpMode() { .forward(28.0) .strafeLeft(11.0) .addTemporalMarker(2.0) { - armSubsystem.setpoint = Math.toRadians(150.0) + armSubsystem.setpoint = Math.toRadians(.0) } .waitSeconds(1.0) .addTemporalMarker(3.0) { 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 815923d..7bd5966 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 @@ -43,6 +43,7 @@ class SlidesPIDTuner : CommandOpMode() { telemetry.addData("Slides Position", slidesSubsystem.slidePos) telemetry.addData("Setpoint", target) telemetry.addData("arm angle", armSubsystem.armAngle) + telemetry.addData("slides Velocity", slidesSubsystem.slideVelocity) telemetry.update() }).perpetually().schedule() diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt new file mode 100644 index 0000000..198aff4 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt @@ -0,0 +1,12 @@ +package org.firstinspires.ftc.teamcode.subsystems.intake + +import com.arcrobotics.ftclib.command.SubsystemBase +import com.qualcomm.robotcore.hardware.Servo + +class IntakeBeltSubsytem( + private val intakeBelt: Servo +) : SubsystemBase() { + fun intake() { intakeBelt.position = 0.5 } + + fun outtake() { intakeBelt.position = 0.0 } +} \ No newline at end of file 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 b3141c9..4bab4f8 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( - 2000.0, - 1000.0 + 1600.0, + 700.0 ) ) @@ -45,7 +45,7 @@ class ElevatorSubsystem( elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) extendMotors.resetEncoder() - extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT) } override fun periodic() { @@ -71,7 +71,8 @@ class ElevatorSubsystem( companion object { @JvmField - var kP = 0.014 + var kP = 0.01 + // kP = 0.01 @JvmField var kI = 0.0 @@ -81,5 +82,8 @@ class ElevatorSubsystem( @JvmField var kG = 0.115 + // kG = 0.115 + } -} \ No newline at end of file +} + From 88a8723c5fe4f8e06b3ffe2a8f4f3f6c0a95540b Mon Sep 17 00:00:00 2001 From: aravioli108 Date: Fri, 29 Nov 2024 10:54:35 -0500 Subject: [PATCH 09/21] BLUE 1 + 1 YAYYYY --- .../auto/blue/basket/left/Blue1Plus1.kt | 93 +++++++++++++++++++ 1 file changed, 93 insertions(+) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt 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 new file mode 100644 index 0000000..95048f3 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus1.kt @@ -0,0 +1,93 @@ +package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left + +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.arcrobotics.ftclib.hardware.motors.CRServo +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import org.firstinspires.ftc.teamcode.constants.AutoStartPose +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.ElevatorSubsystem + +@Autonomous(name = "Blue 1 + 3 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") +class Blue1Plus1: OpMode() { + private lateinit var armLeft: Motor + private lateinit var armRight: Motor + private lateinit var elevatorLeft: Motor + private lateinit var elevatorRight: Motor + + private lateinit var intake: CRServo + + private lateinit var driveSubsystem: DriveSubsystem + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var armSubsystem: ArmSubsystem + private lateinit var elevatorSubsystem: ElevatorSubsystem + + override fun init() { +// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) + + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + + driveSubsystem = DriveSubsystem(hardwareMap) +// intakeSubsystem = IntakeSubsystem(intake) + armSubsystem = ArmSubsystem(armRight, armLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + + val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + .addTemporalMarker(0.1) { + armSubsystem.setpoint = Math.toRadians(95.0) + } + .waitSeconds(0.25) + .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .waitSeconds(0.25) + .addTemporalMarker(3.0) { + elevatorSubsystem.setpoint = 1700.0 + //turn belt servo + } + //outtake + .waitSeconds(1.0) + .turn(Math.toRadians(30.0)) + .addTemporalMarker(7.0) { + elevatorSubsystem.setpoint = 0.0 + //belt servo turn + } +// .addTemporalMarker(8.0) { +// armSubsystem.setpoint = Math.toRadians(0.0) +// } +// .addTemporalMarker(9.0) { +// elevatorSubsystem.setpoint = 1700.0 +// //intake +// } +// .addTemporalMarker(12.0) { +// elevatorSubsystem.setpoint = 0.0 +// } +// .addTemporalMarker(15.0) { +// armSubsystem.setpoint = 95.0 +// //turn belt servo +// } +// .addTemporalMarker(18.0) { +// //outtake +// } +// .waitSeconds(2.0) +// .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) +// .turn(Math.toRadians(-30.0)) + .build() + + driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose + driveSubsystem.followTrajectorySequenceAsync(trajectory) + } + + override fun loop() { + driveSubsystem.update() + + elevatorSubsystem.periodic() + armSubsystem.periodic() + } +} \ No newline at end of file From 5d310a026a2ddfb78ca2c06f3e61dd042ea8f817 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Fri, 29 Nov 2024 17:25:24 -0500 Subject: [PATCH 10/21] TeleOp --- .../pathplanning/blue/main/BlueAuto.kt | 126 +++++++++--------- .../commands/intake/IntakeBeltCommand.kt | 22 +++ .../ftc/teamcode/constants/ArmConstants.kt | 4 +- .../ftc/teamcode/constants/ControlBoard.kt | 2 +- .../auto/blue/basket/left/Blue1Plus1.kt | 91 +++++++++---- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 43 ++++-- .../teamcode/opModes/tuning/IntakeTuner.kt | 50 +++++++ .../teamcode/subsystems/arm/ArmSubsystem.kt | 23 +++- .../subsystems/intake/IntakeBeltSubsystem.kt | 19 +++ .../subsystems/intake/IntakeBeltSubsytem.kt | 12 -- .../subsystems/intake/IntakeSubsystem.kt | 2 + .../subsystems/slides/ElevatorSubsystem.kt | 2 +- 12 files changed, 274 insertions(+), 122 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt delete mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt index 87f70a3..eaf5203 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt @@ -29,70 +29,66 @@ object BlueAuto { .addDisplacementMarker { //add command for rollers servo to outtake } - .turn(Math.toRadians(30.0)) // - .addTemporalMarker(6.0) { - //add command for arm to go down - //add command for slides to go out - //add command to belt servo to go at [x] angle - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-30.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .turn(Math.toRadians(55.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm down - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-55.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .turn(Math.toRadians(80.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm down - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to intake - } - .turn(Math.toRadians(-80.0)) // - .addTemporalMarker() { - //add command to bring slides in - //add command to bring arm up - //add command to put slides out - //add command to turn belt servo - } - .addDisplacementMarker { - //add command to outtake - } - .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) - .addDisplacementMarker { - //add command to bring arm up - //add command to bring slides out - } +// .turn(Math.toRadians(30.0)) // +// .addTemporalMarker(6.0) { +// //add command for arm to go down +// //add command for slides to go out +// //add command to belt servo to go at [x] angle +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-30.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } +// .turn(Math.toRadians(55.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm down +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-55.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } +// .turn(Math.toRadians(80.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm down +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to intake +// } +// .turn(Math.toRadians(-80.0)) // +// .addTemporalMarker() { +// //add command to bring slides in +// //add command to bring arm up +// //add command to put slides out +// //add command to turn belt servo +// } +// .addDisplacementMarker { +// //add command to outtake +// } + .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) @@ -101,7 +97,7 @@ object BlueAuto { } var img: Image? = null try { - img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) + img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) } catch (_ : IOException) { } diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt new file mode 100644 index 0000000..9d0f345 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt @@ -0,0 +1,22 @@ +package org.firstinspires.ftc.teamcode.commands.intake + +import com.arcrobotics.ftclib.command.CommandBase +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem + +class IntakeBeltCommand( + private val intakeBelt: Boolean, + private val intakeBeltSubsystem: IntakeBeltSubsystem +) : CommandBase() { + override fun execute() { + if (intakeBelt) { + intakeBeltSubsystem.intakePos() + } + else { + intakeBeltSubsystem.outtakePos() + } + } + + /* override fun end(interrupted: Boolean) { + intakeBeltSubsystem.stop() + } +*/} \ No newline at end of file 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 50971b8..2bb65f2 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 @@ -1,8 +1,8 @@ package org.firstinspires.ftc.teamcode.constants enum class ArmConstants(val value: Double) { - kP(1.0), + kP(2.0), kI(0.0), - kD(0.08), + kD(0.0), kCos(0.01) } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt index 3d31684..07234d5 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/ControlBoard.kt @@ -20,7 +20,7 @@ enum class ControlBoard(val deviceName: String) { SLIDES_RIGHT("rightSlideString"), //Intake - INTAKE("rollerIntake"), + INTAKE("intakeRoller"), INTAKE_BELT("intakeBelt") 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 95048f3..3f7a4f7 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 @@ -5,14 +5,16 @@ import com.arcrobotics.ftclib.hardware.motors.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose 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.IntakeBeltSubsystem import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem -@Autonomous(name = "Blue 1 + 3 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") +@Autonomous(name = "Blue 1 + 1 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") class Blue1Plus1: OpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor @@ -20,14 +22,17 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorRight: Motor private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem private lateinit var armSubsystem: ArmSubsystem private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { -// intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) @@ -36,48 +41,82 @@ class Blue1Plus1: OpMode() { elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) driveSubsystem = DriveSubsystem(hardwareMap) -// intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) armSubsystem = ArmSubsystem(armRight, armLeft) elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .addTemporalMarker(0.1) { - armSubsystem.setpoint = Math.toRadians(95.0) - } - .waitSeconds(0.25) .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) - .waitSeconds(0.25) + .back(4.3) .addTemporalMarker(3.0) { - elevatorSubsystem.setpoint = 1700.0 - //turn belt servo + armSubsystem.setpoint = Math.toRadians(97.5) } - //outtake .waitSeconds(1.0) - .turn(Math.toRadians(30.0)) + .addTemporalMarker(4.0) { + elevatorSubsystem.setpoint = 2000.0 + } + .waitSeconds(2.0) .addTemporalMarker(7.0) { + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(8.0) { + intakeSubsystem.outtake() + } + .waitSeconds(0.5) + .addTemporalMarker(8.5) { + intakeSubsystem.stop() + } + .waitSeconds(1.0) + .forward(4.0) + .addTemporalMarker(9.5) { elevatorSubsystem.setpoint = 0.0 - //belt servo turn } -// .addTemporalMarker(8.0) { -// armSubsystem.setpoint = Math.toRadians(0.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 // } -// .addTemporalMarker(9.0) { -// elevatorSubsystem.setpoint = 1700.0 -// //intake +// .waitSeconds(2.0) +// .addTemporalMarker(16.0) { +// intakeBeltSubsystem.intakePos() +// } +// .waitSeconds(1.0) +// .addTemporalMarker(17.0) { +// intakeSubsystem.intake() // } -// .addTemporalMarker(12.0) { -// elevatorSubsystem.setpoint = 0.0 +// .waitSeconds(2.0) +// .addTemporalMarker(20.0) { +// elevatorSubsystem.setpoint = 100.0 // } -// .addTemporalMarker(15.0) { +// .waitSeconds(2.0) +// .turn(Math.toRadians(-25.0)) +// .addTemporalMarker(24.0) { // armSubsystem.setpoint = 95.0 -// //turn belt servo +// intakeBeltSubsystem.outtakePos() +// // } -// .addTemporalMarker(18.0) { -// //outtake +// .addTemporalMarker(25.0) { +// elevatorSubsystem.setpoint = 2000.0 // } // .waitSeconds(2.0) -// .splineToSplineHeading(Pose2d(36.0, 12.0, Math.toRadians(-180.0)), Math.toRadians(-90.0)) -// .turn(Math.toRadians(-30.0)) +// .addTemporalMarker(28.0) { +// 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 + } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.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 6167ae3..2245b19 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 @@ -4,16 +4,20 @@ 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.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import com.qualcomm.robotcore.hardware.Servo 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.commands.intake.IntakeCommand 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 @@ -23,21 +27,29 @@ class MainTeleOp : CommandOpMode() { private lateinit var elevatorRight: Motor private lateinit var armLeft: Motor private lateinit var armRight: Motor - //private lateinit var intake: CRServo + + private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo private lateinit var slidesSubsystem: OpenElevatorSubsystem private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem + private lateinit var spinUpCommand: SpinUpCommand private lateinit var spinDownCommand: SpinDownCommand + private lateinit var armUpCommand: OpenArmCommand private lateinit var armDownCommand: OpenArmCommand + private lateinit var driveCommand: DriveCommand + private lateinit var intakeCommand: IntakeCommand private lateinit var outtakeCommand: IntakeCommand - + private lateinit var intakeBeltCommand: IntakeBeltCommand + private lateinit var outtakeBeltCommand: IntakeBeltCommand private lateinit var driver: GamepadEx private lateinit var operator: GamepadEx @@ -46,25 +58,30 @@ class MainTeleOp : CommandOpMode() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) - elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_435) - elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_435) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName, Motor.GoBILDA.RPM_1150) + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName, Motor.GoBILDA.RPM_1150) - armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_435) - armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_435) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60) + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) - //intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armSubsystem = OpenArmSubsystem(armRight, armLeft) slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) driveSubsystem = DriveSubsystem(hardwareMap) - //intakeSubsystem = IntakeSubsystem(intake) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + spinUpCommand = SpinUpCommand(slidesSubsystem) spinDownCommand = SpinDownCommand(slidesSubsystem) armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) - //intakeCommand = IntakeCommand(true, intakeSubsystem) - //outtakeCommand = IntakeCommand(false, intakeSubsystem) + intakeCommand = IntakeCommand(true, intakeSubsystem) + outtakeCommand = IntakeCommand(false, intakeSubsystem) + intakeBeltCommand = IntakeBeltCommand(true, intakeBeltSubsystem) + outtakeBeltCommand = IntakeBeltCommand(false, intakeBeltSubsystem) driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) operator.getGamepadButton(GamepadKeys.Button.DPAD_UP).whileHeld(spinUpCommand) @@ -73,8 +90,10 @@ class MainTeleOp : CommandOpMode() { 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.Y).whenHeld(intakeCommand) - // operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) + operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand) + operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) + operator.getGamepadButton(GamepadKeys.Button.X).whileHeld(intakeBeltCommand) + operator.getGamepadButton(GamepadKeys.Button.B).whileHeld(outtakeBeltCommand) driveSubsystem.defaultCommand = driveCommand diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt new file mode 100644 index 0000000..a74aec4 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -0,0 +1,50 @@ +package org.firstinspires.ftc.teamcode.opModes.tuning + +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.RunCommand +import com.arcrobotics.ftclib.hardware.motors.CRServo +import com.qualcomm.robotcore.eventloop.opmode.TeleOp +import com.qualcomm.robotcore.hardware.Servo +import org.firstinspires.ftc.teamcode.constants.ControlBoard +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeBeltSubsystem +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem + +@TeleOp +@Config +class IntakeTuner : CommandOpMode() { + private lateinit var intake: CRServo + private lateinit var intakeBelt: Servo + + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem + + override fun initialize() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + + intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) + + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + + RunCommand({ + intakeSubsystem.setSpeed(speed) + }).perpetually().schedule() + + RunCommand({ + intakeBeltSubsystem.setPos(position) + }).perpetually().schedule() + + } + + companion object { + @JvmField + var speed = 0.0 + + @JvmField + var position = 1.0 + } +} \ No newline at end of file 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 2973060..bb2b606 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 @@ -29,19 +29,36 @@ class ArmSubsystem( val armVelocity: Double get() = turnMotors.velocities[0] / GoBILDA.RPM_60.cpr * PI - private val feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); + private var feedforward = ArmFeedforward(0.0, ArmConstants.kCos.value, 0.0); init { - armRight.inverted = true - armRight.encoder.setDirection(Motor.Direction.REVERSE) + armLeft.inverted = true turnMotors.resetEncoder() turnMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } override fun useOutput(output: Double, setpoint: Double) { +// controller.setPIDF(kP, kI, kD, 0.0) +// feedforward = ArmFeedforward(0.0, kCos, 0.0) + turnMotors.set(output + feedforward.calculate(armAngle, armVelocity)) } override fun getMeasurement() = armAngle + + companion object { + @JvmField + var kP = 2.0 + + @JvmField + var kI = 0.0 + + @JvmField + var kD = 0.0 + + @JvmField + var kCos = 0.1 + + } } \ 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 new file mode 100644 index 0000000..a9797db --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsystem.kt @@ -0,0 +1,19 @@ +package org.firstinspires.ftc.teamcode.subsystems.intake + +import com.arcrobotics.ftclib.command.SubsystemBase +import com.qualcomm.robotcore.hardware.Servo + +class IntakeBeltSubsystem( + private val intakeBelt: Servo +) : SubsystemBase() { + fun intakePos() { + intakeBelt.position = 0.0 + } + fun outtakePos() { + intakeBelt.position = 0.6 + } + + fun setPos(pos: Double) { + intakeBelt.position = pos + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt deleted file mode 100644 index 198aff4..0000000 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/subsystems/intake/IntakeBeltSubsytem.kt +++ /dev/null @@ -1,12 +0,0 @@ -package org.firstinspires.ftc.teamcode.subsystems.intake - -import com.arcrobotics.ftclib.command.SubsystemBase -import com.qualcomm.robotcore.hardware.Servo - -class IntakeBeltSubsytem( - private val intakeBelt: Servo -) : SubsystemBase() { - fun intake() { intakeBelt.position = 0.5 } - - fun outtake() { intakeBelt.position = 0.0 } -} \ No newline at end of file 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 216211c..1d360fd 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,5 +10,7 @@ class IntakeSubsystem( fun outtake() = intake.set(-0.5) + fun setSpeed(speed: Double) = intake.set(speed) + fun stop() = intake.stop() } \ No newline at end of file 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 4bab4f8..4a78f77 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 @@ -45,7 +45,7 @@ class ElevatorSubsystem( elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) extendMotors.resetEncoder() - extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.FLOAT) + extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } override fun periodic() { From 3bc780d2bf2b8a8e89d994e19e28d67c802f7aff Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Fri, 29 Nov 2024 18:46:42 -0500 Subject: [PATCH 11/21] Rework to simpler control --- .../teamcode/commands/drive/DriveCommand.kt | 6 +-- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 42 +++++++++++++------ .../subsystems/intake/IntakeBeltSubsystem.kt | 12 ++++-- .../subsystems/slides/ElevatorSubsystem.kt | 2 +- 4 files changed, 43 insertions(+), 19 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 bdba1b3..653b635 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(), zoneVal).pow(3), - leftX = zonedDrive(leftX.invoke(), zoneVal).pow(3), - rightX = zonedDrive(rightX.invoke(), zoneVal).pow(3), + 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), ) } 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 2245b19..1d8b37a 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,7 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.teleOp import com.arcrobotics.ftclib.command.CommandOpMode +import com.arcrobotics.ftclib.command.ConditionalCommand +import com.arcrobotics.ftclib.command.InstantCommand import com.arcrobotics.ftclib.command.RunCommand +import com.arcrobotics.ftclib.command.button.Trigger import com.arcrobotics.ftclib.gamepad.GamepadEx import com.arcrobotics.ftclib.gamepad.GamepadKeys import com.arcrobotics.ftclib.hardware.motors.CRServo @@ -12,7 +15,6 @@ 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.commands.intake.IntakeCommand import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem @@ -48,12 +50,15 @@ class MainTeleOp : CommandOpMode() { private lateinit var intakeCommand: IntakeCommand private lateinit var outtakeCommand: IntakeCommand - private lateinit var intakeBeltCommand: IntakeBeltCommand - private lateinit var outtakeBeltCommand: IntakeBeltCommand + + private lateinit var intakeBeltCommand: ConditionalCommand private lateinit var driver: GamepadEx private lateinit var operator: GamepadEx + private lateinit var operatorLeft: Trigger + private lateinit var operatorRight: Trigger + override fun initialize() { driver = GamepadEx(gamepad1) operator = GamepadEx(gamepad2) @@ -73,33 +78,46 @@ class MainTeleOp : CommandOpMode() { intakeSubsystem = IntakeSubsystem(intake) intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) - spinUpCommand = SpinUpCommand(slidesSubsystem) spinDownCommand = SpinDownCommand(slidesSubsystem) + armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) + intakeCommand = IntakeCommand(true, intakeSubsystem) outtakeCommand = IntakeCommand(false, intakeSubsystem) - intakeBeltCommand = IntakeBeltCommand(true, intakeBeltSubsystem) - outtakeBeltCommand = IntakeBeltCommand(false, intakeBeltSubsystem) + + intakeBeltCommand = ConditionalCommand( + InstantCommand({ intakeBeltSubsystem.intakePos() }), + InstantCommand({ intakeBeltSubsystem.outtakePos() }), + intakeBeltSubsystem::intakePos + ) + 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) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand) - operator.getGamepadButton(GamepadKeys.Button.Y).whenHeld(intakeCommand) - operator.getGamepadButton(GamepadKeys.Button.A).whileHeld(outtakeCommand) - operator.getGamepadButton(GamepadKeys.Button.X).whileHeld(intakeBeltCommand) - operator.getGamepadButton(GamepadKeys.Button.B).whileHeld(outtakeBeltCommand) + operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(intakeBeltCommand) + + operatorLeft = Trigger { + operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) != 0.0 + } + + operatorRight = Trigger { + operator.getTrigger(GamepadKeys.Trigger.RIGHT_TRIGGER) != 0.0 + } + + operatorLeft.whileActiveContinuous(outtakeCommand) + operatorRight.whileActiveContinuous(intakeCommand) driveSubsystem.defaultCommand = driveCommand RunCommand({ telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle)) + telemetry.addData("Intake Position", intakeBeltSubsystem.intakePos) telemetry.update() }).perpetually().schedule() } 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 a9797db..63e1c00 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,13 +6,19 @@ import com.qualcomm.robotcore.hardware.Servo class IntakeBeltSubsystem( private val intakeBelt: Servo ) : SubsystemBase() { - fun intakePos() { - intakeBelt.position = 0.0 - } + var intakePos = false + fun outtakePos() { + intakeBelt.position = 0.0 + intakePos = true + } + + fun intakePos() { intakeBelt.position = 0.6 + intakePos = false } + fun setPos(pos: Double) { intakeBelt.position = pos } 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 4a78f77..dce215a 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 @@ -11,7 +11,7 @@ import kotlin.math.sin @Config class ElevatorSubsystem( - elevatorRight : Motor, + private val elevatorRight : Motor, elevatorLeft : Motor, private val slideAngle: () -> Double ) : SubsystemBase() { From ce13986856ecf5a6659975f1b63e8ee76806d378 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 30 Nov 2024 10:31:10 -0500 Subject: [PATCH 12/21] Refactor CRServo to Servo --- .../commands/intake/IntakeBeltCommand.kt | 6 +----- .../teamcode/commands/intake/IntakeCommand.kt | 4 ---- .../opModes/auto/blue/basket/left/Blue1Plus1.kt | 11 +++-------- .../auto/blue/basket/left/BlueHighBasketLeft.kt | 6 +++--- .../blue/basket/right/BlueHighBasketRight.kt | 10 +++++----- .../blue/chamber/left/BlueHighChamberLeft.kt | 7 ++++--- .../blue/chamber/right/BlueHighChamberRight.kt | 7 ++++--- .../auto/red/basket/left/RedHighBasketLeft.kt | 7 ++++--- .../auto/red/basket/right/RedHighBasketRight.kt | 7 ++++--- .../auto/red/chamber/left/RedHighChamberLeft.kt | 7 ++++--- .../red/chamber/right/RedHighChamberRight.kt | 7 ++++--- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 5 ++--- .../ftc/teamcode/opModes/tuning/IntakeTuner.kt | 6 +++--- .../roadrunner/AutomaticFeedforwardTuner.kt | 1 - .../opModes/tuning/roadrunner/BackAndForth.kt | 1 - .../tuning/roadrunner/DriveVelocityPIDTuner.kt | 1 - .../tuning/roadrunner/FollowerPIDTuner.kt | 1 - .../tuning/roadrunner/LocalizationTest.kt | 1 - .../tuning/roadrunner/ManualFeedforwardTuner.kt | 1 - .../tuning/roadrunner/MaxAngularVeloTuner.kt | 1 - .../tuning/roadrunner/MaxVelocityTuner.kt | 1 - .../tuning/roadrunner/MotorDirectionDebugger.kt | 1 - .../opModes/tuning/roadrunner/SplineTest.kt | 1 - .../opModes/tuning/roadrunner/StrafeTest.kt | 1 - .../opModes/tuning/roadrunner/StraightTest.kt | 1 - .../opModes/tuning/roadrunner/TrackWidthTuner.kt | 1 - .../TrackingWheelForwardOffsetTuner.kt | 1 - .../TrackingWheelLateralDistanceTuner.kt | 1 - .../opModes/tuning/roadrunner/TurnTest.kt | 1 - .../subsystems/intake/IntakeBeltSubsystem.kt | 4 ++-- .../subsystems/intake/IntakeSubsystem.kt | 16 +++++++++------- .../subsystems/slides/OpenElevatorSubsystem.kt | 4 ++-- 32 files changed, 54 insertions(+), 76 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt index 9d0f345..6f0ab52 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeBeltCommand.kt @@ -15,8 +15,4 @@ class IntakeBeltCommand( intakeBeltSubsystem.outtakePos() } } - - /* override fun end(interrupted: Boolean) { - intakeBeltSubsystem.stop() - } -*/} \ No newline at end of file +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt index b994fd8..bd9429c 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/commands/intake/IntakeCommand.kt @@ -15,8 +15,4 @@ class IntakeCommand( subsystem.outtake() } } - - override fun end(interrupted: Boolean) { - subsystem.stop() - } } \ 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 3f7a4f7..ade1c44 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 @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left import com.acmerobotics.roadrunner.geometry.Pose2d -import com.arcrobotics.ftclib.hardware.motors.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode @@ -21,7 +20,7 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var intakeBelt: Servo private lateinit var driveSubsystem: DriveSubsystem @@ -31,7 +30,7 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) @@ -64,11 +63,7 @@ class Blue1Plus1: OpMode() { .addTemporalMarker(8.0) { intakeSubsystem.outtake() } - .waitSeconds(0.5) - .addTemporalMarker(8.5) { - intakeSubsystem.stop() - } - .waitSeconds(1.0) + .waitSeconds(1.5) .forward(4.0) .addTemporalMarker(9.5) { elevatorSubsystem.setpoint = 0.0 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 461264b..27e2886 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 @@ -1,9 +1,9 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left -import com.arcrobotics.ftclib.hardware.motors.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +18,7 @@ class BlueHighBasketLeft: OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +26,7 @@ class BlueHighBasketLeft: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 96b881c..f95d0c7 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.right -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -13,14 +14,13 @@ import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem @Autonomous(name = "Blue High Basket Right", group = "Basket", preselectTeleOp = "MainTeleOp") -class BlueHighBasketRight() : OpMode() { - +class BlueHighBasketRight : OpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -28,7 +28,7 @@ class BlueHighBasketRight() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 6b537df..c98fab0 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.left -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +19,7 @@ class BlueHighChamberLeft() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +27,7 @@ class BlueHighChamberLeft() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 ceecd1a..bd59302 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.chamber.right -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +19,7 @@ class BlueHighChamberRight() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +27,7 @@ class BlueHighChamberRight() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 74630a7..774470f 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.left -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -19,7 +20,7 @@ class RedHighBasketLeft() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -27,7 +28,7 @@ class RedHighBasketLeft() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 657d3ef..ea4cd44 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.red.basket.right -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +19,7 @@ class RedHighBasketRight() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +27,7 @@ class RedHighBasketRight() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 fe3b6a5..d59365a 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.left -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +19,7 @@ class RedHighChamberLeft() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +27,7 @@ class RedHighChamberLeft() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 671b539..7bb7257 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 @@ -1,9 +1,10 @@ package org.firstinspires.ftc.teamcode.opModes.auto.red.chamber.right -import com.arcrobotics.ftclib.hardware.motors.CRServo + import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.AutoStartPose import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.ArmSubsystem @@ -18,7 +19,7 @@ class RedHighChamberRight() : OpMode() { private lateinit var elevatorLeft: Motor private lateinit var elevatorRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem @@ -26,7 +27,7 @@ class RedHighChamberRight() : OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) 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 1d8b37a..7d2332b 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 @@ -7,7 +7,6 @@ import com.arcrobotics.ftclib.command.RunCommand import com.arcrobotics.ftclib.command.button.Trigger import com.arcrobotics.ftclib.gamepad.GamepadEx import com.arcrobotics.ftclib.gamepad.GamepadKeys -import com.arcrobotics.ftclib.hardware.motors.CRServo import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.Servo @@ -30,7 +29,7 @@ class MainTeleOp : CommandOpMode() { private lateinit var armLeft: Motor private lateinit var armRight: Motor - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var intakeBelt: Servo private lateinit var slidesSubsystem: OpenElevatorSubsystem @@ -69,7 +68,7 @@ class MainTeleOp : CommandOpMode() { armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName, Motor.GoBILDA.RPM_60) armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName, Motor.GoBILDA.RPM_60) - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armSubsystem = OpenArmSubsystem(armRight, armLeft) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt index a74aec4..acd7294 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -5,7 +5,7 @@ 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.hardware.motors.CRServo + import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.Servo import org.firstinspires.ftc.teamcode.constants.ControlBoard @@ -15,7 +15,7 @@ import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem @TeleOp @Config class IntakeTuner : CommandOpMode() { - private lateinit var intake: CRServo + private lateinit var intake: Servo private lateinit var intakeBelt: Servo private lateinit var intakeSubsystem: IntakeSubsystem @@ -24,7 +24,7 @@ class IntakeTuner : CommandOpMode() { override fun initialize() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) - intake = CRServo(hardwareMap, ControlBoard.INTAKE.deviceName) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) intakeSubsystem = IntakeSubsystem(intake) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt index ca3f37a..e61f24a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/AutomaticFeedforwardTuner.kt @@ -6,7 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.util.NanoClock import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.RobotLog import org.firstinspires.ftc.robotcore.internal.system.Misc diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt index 7441dfb..ad60cfc 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/BackAndForth.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.dashboard.config.Config import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt index 1448fbe..521f1ba 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/DriveVelocityPIDTuner.kt @@ -9,7 +9,6 @@ import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimple import com.acmerobotics.roadrunner.profile.MotionState import com.acmerobotics.roadrunner.util.NanoClock import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.util.RobotLog diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt index cd31e54..7f99231 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/FollowerPIDTuner.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.dashboard.config.Config import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt index c94f85d..634d631 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/LocalizationTest.kt @@ -1,7 +1,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.roadrunner.geometry.Pose2d -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.DcMotor diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt index 2a731d9..ff1b6dc 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/ManualFeedforwardTuner.kt @@ -10,7 +10,6 @@ import com.acmerobotics.roadrunner.profile.MotionProfileGenerator.generateSimple import com.acmerobotics.roadrunner.profile.MotionState import com.acmerobotics.roadrunner.util.NanoClock import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.RobotLog import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt index f61aaac..66cfa58 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxAngularVeloTuner.kt @@ -5,7 +5,6 @@ import com.acmerobotics.dashboard.config.Config import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.util.ElapsedTime diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt index 93dd051..b31c201 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MaxVelocityTuner.kt @@ -4,7 +4,6 @@ import com.acmerobotics.dashboard.FtcDashboard import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.hardware.DcMotor import com.qualcomm.robotcore.hardware.VoltageSensor diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt index e035dcc..a865cd0 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/MotorDirectionDebugger.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.dashboard.FtcDashboard import com.acmerobotics.dashboard.config.Config import com.acmerobotics.dashboard.telemetry.MultipleTelemetry -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import org.firstinspires.ftc.robotcore.external.Telemetry diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt index afff7a9..0849b10 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/SplineTest.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.geometry.Vector2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt index cf637cd..5e6765a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StrafeTest.kt @@ -5,7 +5,6 @@ import com.acmerobotics.dashboard.config.Config import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt index cd63379..8088288 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/StraightTest.kt @@ -5,7 +5,6 @@ import com.acmerobotics.dashboard.config.Config import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt index 740b73b..0f849d2 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackWidthTuner.kt @@ -6,7 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.util.Angle.norm import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.MovingStatistics import org.firstinspires.ftc.robotcore.internal.system.Misc diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt index d21ba97..14e12ff 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelForwardOffsetTuner.kt @@ -6,7 +6,6 @@ import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.util.Angle.norm import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.util.MovingStatistics import com.qualcomm.robotcore.util.RobotLog diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt index 32e4c47..0d52efe 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TrackingWheelLateralDistanceTuner.kt @@ -3,7 +3,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.dashboard.config.Config import com.acmerobotics.roadrunner.geometry.Pose2d import com.acmerobotics.roadrunner.util.Angle.normDelta -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.util.RobotLog diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt index 5345621..c82dcc9 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/roadrunner/TurnTest.kt @@ -2,7 +2,6 @@ package org.firstinspires.ftc.teamcode.opModes.tuning.roadrunner import com.acmerobotics.dashboard.config.Config import com.qualcomm.robotcore.eventloop.opmode.Autonomous -import com.qualcomm.robotcore.eventloop.opmode.Disabled import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem 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 63e1c00..7c4fbeb 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 @@ -9,12 +9,12 @@ class IntakeBeltSubsystem( var intakePos = false fun outtakePos() { - intakeBelt.position = 0.0 + intakeBelt.position = 0.25 intakePos = true } fun intakePos() { - intakeBelt.position = 0.6 + intakeBelt.position = 0.02 intakePos = false } 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 1d360fd..b06f22b 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 @@ -1,16 +1,18 @@ package org.firstinspires.ftc.teamcode.subsystems.intake import com.arcrobotics.ftclib.command.SubsystemBase -import com.arcrobotics.ftclib.hardware.motors.CRServo +import com.qualcomm.robotcore.hardware.Servo class IntakeSubsystem( - private val intake: CRServo + private val intake: Servo ) : SubsystemBase() { - fun intake() = intake.set(1.0) + fun intake() { + intake.position = 1.0 + } - fun outtake() = intake.set(-0.5) + fun outtake() { + intake.position = 0.0 + } - fun setSpeed(speed: Double) = intake.set(speed) - - fun stop() = intake.stop() + fun setSpeed(speed: Double) { intake.position = speed } } \ No newline at end of file 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 e49c72d..a7f36fc 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 @@ -21,11 +21,11 @@ class OpenElevatorSubsystem( // Functions for moving slides up and down, number being speed, 1 being 100% fun up() { - elevatorMotors.set(1.0) + elevatorMotors.set(0.7) } fun down() { - elevatorMotors.set(-1.0) + elevatorMotors.set(-0.7) } fun stop() { From 50f537f6ff8c9f2859ef02ad79839bedba256794 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sat, 30 Nov 2024 12:52:33 -0500 Subject: [PATCH 13/21] New arm command --- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 24 +++++++++---------- .../subsystems/intake/IntakeBeltSubsystem.kt | 9 ++----- .../subsystems/intake/IntakeSubsystem.kt | 6 ++++- 3 files changed, 19 insertions(+), 20 deletions(-) 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 7d2332b..bdaadc7 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 @@ -14,7 +14,7 @@ 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.IntakeCommand +import org.firstinspires.ftc.teamcode.commands.intake.IntakeBeltCommand import org.firstinspires.ftc.teamcode.constants.ControlBoard import org.firstinspires.ftc.teamcode.subsystems.arm.OpenArmSubsystem import org.firstinspires.ftc.teamcode.subsystems.drive.DriveSubsystem @@ -47,10 +47,10 @@ class MainTeleOp : CommandOpMode() { private lateinit var driveCommand: DriveCommand - private lateinit var intakeCommand: IntakeCommand - private lateinit var outtakeCommand: IntakeCommand + private lateinit var intakeCommand: IntakeBeltCommand + private lateinit var outtakeCommand: IntakeBeltCommand - private lateinit var intakeBeltCommand: ConditionalCommand + private lateinit var clawCommand: ConditionalCommand private lateinit var driver: GamepadEx private lateinit var operator: GamepadEx @@ -83,13 +83,13 @@ class MainTeleOp : CommandOpMode() { armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) - intakeCommand = IntakeCommand(true, intakeSubsystem) - outtakeCommand = IntakeCommand(false, intakeSubsystem) + intakeCommand = IntakeBeltCommand(true, intakeBeltSubsystem) + outtakeCommand = IntakeBeltCommand(false, intakeBeltSubsystem) - intakeBeltCommand = ConditionalCommand( - InstantCommand({ intakeBeltSubsystem.intakePos() }), - InstantCommand({ intakeBeltSubsystem.outtakePos() }), - intakeBeltSubsystem::intakePos + clawCommand = ConditionalCommand( + InstantCommand({ intakeSubsystem.intake() }), + InstantCommand({ intakeSubsystem.outtake() }), + intakeSubsystem::intakePos ) driveCommand = DriveCommand(driveSubsystem, driver::getLeftX, driver::getLeftY, driver::getRightX, 0.0) @@ -99,7 +99,7 @@ class MainTeleOp : CommandOpMode() { operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armUpCommand) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armDownCommand) - operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(intakeBeltCommand) + operator.getGamepadButton(GamepadKeys.Button.B).whenPressed(clawCommand) operatorLeft = Trigger { operator.getTrigger(GamepadKeys.Trigger.LEFT_TRIGGER) != 0.0 @@ -116,7 +116,7 @@ class MainTeleOp : CommandOpMode() { RunCommand({ telemetry.addData("Arm Position: ", Math.toDegrees(armSubsystem.armAngle)) - telemetry.addData("Intake Position", intakeBeltSubsystem.intakePos) + telemetry.addData("Intake Position", intakeSubsystem.intakePos) telemetry.update() }).perpetually().schedule() } 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 7c4fbeb..f9c3ecc 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,19 +6,14 @@ import com.qualcomm.robotcore.hardware.Servo class IntakeBeltSubsystem( private val intakeBelt: Servo ) : SubsystemBase() { - var intakePos = false - fun outtakePos() { - intakeBelt.position = 0.25 - intakePos = true + intakeBelt.position += 0.01 } fun intakePos() { - intakeBelt.position = 0.02 - intakePos = false + intakeBelt.position -= 0.01 } - fun setPos(pos: Double) { intakeBelt.position = pos } 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 b06f22b..820ff81 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 @@ -6,12 +6,16 @@ import com.qualcomm.robotcore.hardware.Servo class IntakeSubsystem( private val intake: Servo ) : SubsystemBase() { + var intakePos = false + fun intake() { - intake.position = 1.0 + intake.position = 0.6 + intakePos = true } fun outtake() { intake.position = 0.0 + intakePos = false } fun setSpeed(speed: Double) { intake.position = speed } From a217f542a691a9fc9ae8e5fc0f98a2e0dc082a40 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sat, 30 Nov 2024 20:37:11 -0500 Subject: [PATCH 14/21] 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() { From cc8b86492115756c8e6a4cd1fcc3fc98f9d8a48d Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sat, 30 Nov 2024 22:30:07 -0500 Subject: [PATCH 15/21] more updates --- .../firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 2 +- .../firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt | 2 +- .../ftc/teamcode/subsystems/arm/OpenArmSubsystem.kt | 4 +++- .../ftc/teamcode/subsystems/slides/OpenElevatorSubsystem.kt | 5 ++++- 4 files changed, 9 insertions(+), 4 deletions(-) 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 5e733f1..d26b03d 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 @@ -144,6 +144,6 @@ class MainTeleOp : CommandOpMode() { companion object { @JvmField - var kT = 0.05 + var kT = 0.005 } } \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt index acd7294..6fa8a7a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -45,6 +45,6 @@ class IntakeTuner : CommandOpMode() { var speed = 0.0 @JvmField - var position = 1.0 + var position = 0.0 } } \ No newline at end of file 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 11d128f..7877fd7 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 @@ -6,6 +6,7 @@ import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.Motor.GoBILDA import com.arcrobotics.ftclib.hardware.motors.MotorGroup import org.firstinspires.ftc.teamcode.constants.ArmConstants +import org.firstinspires.ftc.teamcode.opModes.teleOp.MainTeleOp.Companion.kT import kotlin.math.PI import kotlin.math.cos @@ -40,7 +41,8 @@ class OpenArmSubsystem( } fun stop() { - turnMotors.set(ArmConstants.kCos.value * cos(armAngle)) + turnMotors.set(ArmConstants.kCos.value * cos(armAngle) + + kT * slidePos.invoke()) } companion object { 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 716250b..3cdd8f2 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 @@ -18,7 +18,10 @@ class OpenElevatorSubsystem( get() = elevatorMotors.positions[0] * SlidesConstants.MAX_HEIGHT_INCH.value / SlidesConstants.MAX_HEIGHT_TICKS.value init { - elevatorLeft.inverted = true + elevatorRight.inverted = true + elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) + + elevatorMotors.resetEncoder() } // Functions for moving slides up and down, number being speed, 1 being 100% From 16a0d1c57b3e5cac41af691183d2bb3259ac3446 Mon Sep 17 00:00:00 2001 From: Ishaan Ghaskadbi Date: Sun, 1 Dec 2024 14:56:33 -0500 Subject: [PATCH 16/21] part of auto --- .../pathplanning/blue/main/BlueAuto.kt | 7 +- .../auto/blue/basket/left/Blue1Plus1.kt | 102 ++++++++++-------- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 25 +++-- .../teamcode/opModes/tuning/IntakeTuner.kt | 8 +- .../subsystems/intake/IntakeBeltSubsystem.kt | 4 +- .../subsystems/intake/IntakeSubsystem.kt | 4 +- .../subsystems/slides/ElevatorSubsystem.kt | 10 +- 7 files changed, 90 insertions(+), 70 deletions(-) diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt index eaf5203..2ee8232 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt @@ -2,7 +2,6 @@ package com.example.pathplanning.blue.main import com.acmerobotics.roadrunner.geometry.Pose2d import com.noahbres.meepmeep.MeepMeep -import com.noahbres.meepmeep.core.toRadians import com.noahbres.meepmeep.roadrunner.DefaultBotBuilder import java.awt.Image import java.io.File @@ -19,13 +18,15 @@ object BlueAuto { DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) .followTrajectorySequence { drive -> - drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians())) + drive.trajectorySequenceBuilder(Pose2d( + -12.0, 62.0, Math.toRadians(270.0) + )) .addTemporalMarker(0.1,) { //add the command for the arm to go up //add command for belt servo to go backwards //add the command to add the slides go up } - .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .splineToLinearHeading(Pose2d(53.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .addDisplacementMarker { //add command for rollers servo to outtake } 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 889d3f5..98162a7 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 @@ -1,5 +1,7 @@ package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left +import com.acmerobotics.dashboard.FtcDashboard +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry import com.acmerobotics.roadrunner.geometry.Pose2d import com.arcrobotics.ftclib.hardware.motors.Motor import com.qualcomm.robotcore.eventloop.opmode.Autonomous @@ -30,6 +32,8 @@ class Blue1Plus1: OpMode() { private lateinit var elevatorSubsystem: ElevatorSubsystem override fun init() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) @@ -48,74 +52,75 @@ class Blue1Plus1: OpMode() { val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .addTemporalMarker(1.0) { intakeSubsystem.intake() - intakeBeltSubsystem.outtakePos() + intakeBeltSubsystem.intakePos() } - .splineToLinearHeading(Pose2d(55.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .back(4.0) .addTemporalMarker(3.0) { - armSubsystem.setpoint = Math.toRadians(92.5) + armSubsystem.setpoint = Math.toRadians(97.5) } .waitSeconds(1.0) .addTemporalMarker(4.0) { - elevatorSubsystem.setpoint = 2000.0 + elevatorSubsystem.setpoint = 30.0 + } + .waitSeconds(1.0) + .addTemporalMarker(5.0) { + intakeBeltSubsystem.outtakePos() } .waitSeconds(2.0) .addTemporalMarker(7.0) { - intakeBeltSubsystem.intakePos() - } - .waitSeconds(1.0) - .addTemporalMarker(8.0) { intakeSubsystem.outtake() } + .waitSeconds(0.5) + .addTemporalMarker(7.5) { + intakeBeltSubsystem.intakePos() + elevatorSubsystem.setpoint = 0.0 + } .waitSeconds(1.5) - .forward(4.0) .addTemporalMarker(9.5) { - elevatorSubsystem.setpoint = 0.0 + armSubsystem.setpoint = 0.0 + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.0) + .turn(Math.toRadians(38.0)) + .addTemporalMarker(11.0) { + elevatorSubsystem.setpoint = 31.0 } .waitSeconds(2.0) -// .addTemporalMarker(13.0) { -// armSubsystem.setpoint = 10.0 -// } + .addTemporalMarker(13.0) { + intakeBeltSubsystem.intakePos() + } .waitSeconds(1.0) - .turn(Math.toRadians(25.0)) .addTemporalMarker(14.0) { - elevatorSubsystem.setpoint = 1500.0 + intakeSubsystem.intake() } -// .waitSeconds(2.0) -// .addTemporalMarker(16.0) { -// intakeBeltSubsystem.intakePos() -// } -// .waitSeconds(1.0) -// .addTemporalMarker(17.0) { -// intakeSubsystem.intake() -// } -// .waitSeconds(2.0) -// .addTemporalMarker(20.0) { -// elevatorSubsystem.setpoint = 100.0 -// } -// .waitSeconds(2.0) -// .turn(Math.toRadians(-25.0)) -// .addTemporalMarker(24.0) { -// armSubsystem.setpoint = 95.0 + .waitSeconds(1.0) + .addTemporalMarker(15.0) { + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(2.0) + .turn(Math.toRadians(-40.0)) + .addTemporalMarker(17.0) { + armSubsystem.setpoint = 97.5 + } + .waitSeconds(1.0) + .back(0.5) + .addTemporalMarker(18.0) { + elevatorSubsystem.setpoint = 33.0 + } +// .waitSeconds(3.0) +// .addTemporalMarker(21.0) { // intakeBeltSubsystem.outtakePos() -// -// } -// .addTemporalMarker(25.0) { -// elevatorSubsystem.setpoint = 2000.0 -// } -// .waitSeconds(2.0) -// .addTemporalMarker(28.0) { -// 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(22.0) { +// intakeSubsystem.outtake() // } +//// .waitSeconds(1.0) +//// .addTemporalMarker(13.0) { +//// elevatorSubsystem.setpoint = 1000.0 +//// } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose @@ -124,7 +129,12 @@ class Blue1Plus1: OpMode() { override fun loop() { driveSubsystem.update() + elevatorSubsystem.periodic() armSubsystem.periodic() + + telemetry.addData("Arm Position", armSubsystem.armAngle) + telemetry.addData("Slides Position", elevatorSubsystem.slidePos) + telemetry.update() } } \ No newline at end of file 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 d26b03d..2a402fc 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,15 +15,13 @@ import com.qualcomm.robotcore.eventloop.opmode.TeleOp import com.qualcomm.robotcore.hardware.Servo 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.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 org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem import kotlin.math.cos @TeleOp @@ -37,14 +35,14 @@ class MainTeleOp : CommandOpMode() { private lateinit var intake: Servo private lateinit var intakeBelt: Servo - private lateinit var slidesSubsystem: OpenElevatorSubsystem + private lateinit var slidesSubsystem: ElevatorSubsystem private lateinit var armSubsystem: OpenArmSubsystem private lateinit var driveSubsystem: DriveSubsystem private lateinit var intakeSubsystem: IntakeSubsystem private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem - private lateinit var spinUpCommand: SpinUpCommand - private lateinit var spinDownCommand: SpinDownCommand + private lateinit var spinUpCommand: InstantCommand + private lateinit var spinDownCommand: InstantCommand private lateinit var armUpCommand: OpenArmCommand private lateinit var armDownCommand: OpenArmCommand @@ -76,15 +74,20 @@ class MainTeleOp : CommandOpMode() { intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) armSubsystem = OpenArmSubsystem(armRight, armLeft) { 0.0 } - slidesSubsystem = OpenElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + slidesSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) armSubsystem = OpenArmSubsystem(armRight, armLeft, slidesSubsystem::slidePos) driveSubsystem = DriveSubsystem(hardwareMap) intakeSubsystem = IntakeSubsystem(intake) intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) - spinUpCommand = SpinUpCommand(slidesSubsystem) - spinDownCommand = SpinDownCommand(slidesSubsystem) + spinUpCommand = InstantCommand({ + slidesSubsystem.setpoint = 30.0 + }) + + spinDownCommand = InstantCommand({ + slidesSubsystem.setpoint = 0.0 + }) armUpCommand = OpenArmCommand(armSubsystem, true) armDownCommand = OpenArmCommand(armSubsystem, false) @@ -103,8 +106,8 @@ class MainTeleOp : CommandOpMode() { 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).whenPressed(spinUpCommand) + operator.getGamepadButton(GamepadKeys.Button.DPAD_DOWN).whenPressed(spinDownCommand) operator.getGamepadButton(GamepadKeys.Button.LEFT_BUMPER).whileHeld(armUpCommand) operator.getGamepadButton(GamepadKeys.Button.RIGHT_BUMPER).whileHeld(armDownCommand) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt index 6fa8a7a..016ee4a 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/tuning/IntakeTuner.kt @@ -31,20 +31,20 @@ class IntakeTuner : CommandOpMode() { intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) RunCommand({ - intakeSubsystem.setSpeed(speed) + intakeSubsystem.setSpeed(claw) }).perpetually().schedule() RunCommand({ - intakeBeltSubsystem.setPos(position) + intakeBeltSubsystem.setPos(belt) }).perpetually().schedule() } companion object { @JvmField - var speed = 0.0 + var claw = 0.75 @JvmField - var position = 0.0 + var belt = 0.0 } } \ 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 a15f869..8596dd5 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 @@ -9,12 +9,12 @@ class IntakeBeltSubsystem( var beltPos = false fun outtakePos() { - intakeBelt.position = 0.25 + intakeBelt.position = 0.75 beltPos = !beltPos } fun intakePos() { - intakeBelt.position = 0.0 + intakeBelt.position = 0.5 beltPos = !beltPos } 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 b10037c..dd2541a 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 @@ -9,12 +9,12 @@ class IntakeSubsystem( var intakePos = false fun intake() { - intake.position = 0.6 + intake.position = 1.0 intakePos = !intakePos } fun outtake() { - intake.position = 1.0 + intake.position = 0.6 intakePos = !intakePos } 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 9f07512..63d49b6 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 @@ -6,6 +6,7 @@ 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.arcrobotics.ftclib.util.MathUtils.clamp import org.firstinspires.ftc.teamcode.constants.SlidesConstants import kotlin.math.sin @@ -28,8 +29,13 @@ class ElevatorSubsystem( var setpoint = 0.0 set(value) { - controller.goal = TrapezoidProfile.State(value, 0.0) - field = value + val clamped = if (slideAngle.invoke() < Math.toRadians(75.0)) + clamp(value, 0.0, 20.0) + else + value + + controller.goal = TrapezoidProfile.State(clamped, 0.0) + field = clamped } val slidePos: Double From 33a1d1392bfe509be3cf6fa000d2d7428f633b44 Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Mon, 2 Dec 2024 22:34:28 -0500 Subject: [PATCH 17/21] add safety check --- .../ftc/teamcode/subsystems/slides/ElevatorSubsystem.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 63d49b6..e36427a 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 @@ -32,7 +32,7 @@ class ElevatorSubsystem( val clamped = if (slideAngle.invoke() < Math.toRadians(75.0)) clamp(value, 0.0, 20.0) else - value + clamp(value, 0.0, 30.0) controller.goal = TrapezoidProfile.State(clamped, 0.0) field = clamped From dc7f0d21a7fad56f0f566931e5a435b98dbd50fd Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Mon, 2 Dec 2024 22:53:01 -0500 Subject: [PATCH 18/21] Update dashboard dependency --- .idea/misc.xml | 3 ++- build.dependencies.gradle | 2 +- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/.idea/misc.xml b/.idea/misc.xml index 4d5607a..3c82750 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -1,3 +1,4 @@ + @@ -6,7 +7,7 @@ - + diff --git a/build.dependencies.gradle b/build.dependencies.gradle index 951adb2..a76c18b 100644 --- a/build.dependencies.gradle +++ b/build.dependencies.gradle @@ -16,5 +16,5 @@ dependencies { //noinspection GradleDependency implementation 'androidx.appcompat:appcompat:1.2.0' - implementation 'com.acmerobotics.dashboard:dashboard:0.4.12' + implementation 'com.acmerobotics.dashboard:dashboard:0.4.16' } From 1e2dc867cb90cc32c68694f6e6b795664b3912de Mon Sep 17 00:00:00 2001 From: aravioli108 Date: Fri, 6 Dec 2024 22:17:03 -0500 Subject: [PATCH 19/21] auto 1+2 final --- .idea/misc.xml | 2 +- .../pathplanning/blue/main/BlueAuto.kt | 72 +----- .../pathplanning/blue/main/BlueAutoLeft.kt | 24 +- .../ftc/teamcode/constants/AutoStartPose.kt | 2 +- .../auto/blue/basket/left/Blue1Plus1.kt | 32 +-- .../auto/blue/basket/left/Blue1Plus2.kt | 212 ++++++++++++++++++ .../auto/blue/basket/left/Blue1Plus2V2.kt | 194 ++++++++++++++++ .../subsystems/intake/IntakeBeltSubsystem.kt | 2 +- .../subsystems/slides/ElevatorSubsystem.kt | 3 +- .../slides/OpenElevatorSubsystem.kt | 1 - 10 files changed, 434 insertions(+), 110 deletions(-) create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt create mode 100644 TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt diff --git a/.idea/misc.xml b/.idea/misc.xml index 3c82750..5f770b0 100644 --- a/.idea/misc.xml +++ b/.idea/misc.xml @@ -7,7 +7,7 @@ - + diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt index 2ee8232..2f3d00e 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAuto.kt @@ -18,79 +18,9 @@ object BlueAuto { DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) .followTrajectorySequence { drive -> - drive.trajectorySequenceBuilder(Pose2d( - -12.0, 62.0, Math.toRadians(270.0) + drive.trajectorySequenceBuilder(Pose2d(36.0, 62.0, Math.toRadians(270.0) )) - .addTemporalMarker(0.1,) { - //add the command for the arm to go up - //add command for belt servo to go backwards - //add the command to add the slides go up - } .splineToLinearHeading(Pose2d(53.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) - .addDisplacementMarker { - //add command for rollers servo to outtake - } -// .turn(Math.toRadians(30.0)) // -// .addTemporalMarker(6.0) { -// //add command for arm to go down -// //add command for slides to go out -// //add command to belt servo to go at [x] angle -// } -// .addDisplacementMarker { -// //add command to intake -// } -// .turn(Math.toRadians(-30.0)) // -// .addTemporalMarker() { -// //add command to bring slides in -// //add command to bring arm up -// //add command to put slides out -// //add command to turn belt servo -// } -// .addDisplacementMarker { -// //add command to outtake -// } -// .turn(Math.toRadians(55.0)) // -// .addTemporalMarker() { -// //add command to bring slides in -// //add command to bring arm down -// //add command to put slides out -// //add command to turn belt servo -// } -// .addDisplacementMarker { -// //add command to intake -// } -// .turn(Math.toRadians(-55.0)) // -// .addTemporalMarker() { -// //add command to bring slides in -// //add command to bring arm up -// //add command to put slides out -// //add command to turn belt servo -// } -// .addDisplacementMarker { -// //add command to outtake -// } -// .turn(Math.toRadians(80.0)) // -// .addTemporalMarker() { -// //add command to bring slides in -// //add command to bring arm down -// //add command to put slides out -// //add command to turn belt servo -// } -// .addDisplacementMarker { -// //add command to intake -// } -// .turn(Math.toRadians(-80.0)) // -// .addTemporalMarker() { -// //add command to bring slides in -// //add command to bring arm up -// //add command to put slides out -// //add command to turn belt servo -// } -// .addDisplacementMarker { -// //add command to outtake -// } - .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) - diff --git a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt index 3071ac1..5505278 100644 --- a/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt +++ b/PathPlanning/src/main/kotlin/com/example/pathplanning/blue/main/BlueAutoLeft.kt @@ -19,27 +19,15 @@ object BlueAutoLeft { DefaultBotBuilder(meepMeep) // Set bot constraints: maxVel, maxAccel, maxAngVel, maxAngAccel, track width .setConstraints(30.0, 30.0, Math.toRadians(170.0), Math.toRadians(170.0), 13.94) .followTrajectorySequence { drive -> - drive.trajectorySequenceBuilder(Pose2d(-12.0, 62.0, (270.0).toRadians())) - .turn((90.0).toRadians()) - .forward(60.0) - .waitSeconds(4.0) - .turn((-90.0).toRadians()) - .waitSeconds(6.0) - .turn((90.0).toRadians()) - .waitSeconds(4.0) - .turn((-90.0).toRadians()) - .waitSeconds(6.0) - .turn((90.0).toRadians()) - .waitSeconds(4.0) - .turn((-90.0).toRadians()) - .waitSeconds(6.0) - .turn((90.0).toRadians()) - .waitSeconds(4.0) + drive.trajectorySequenceBuilder(Pose2d(36.0, 62.0, (270.0).toRadians())) + .splineToLinearHeading(Pose2d(53.0, 55.0, Math.toRadians(225.0)), 0.0) + .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0) + .back(5.0) .build() } var img: Image? = null try { - img = ImageIO.read(File("/Users/ishaanghaskadbi/Downloads/field.png")) + img = ImageIO.read(File("C://Users//arava//Downloads//field.png/")) } catch (_ : IOException) { } @@ -52,4 +40,4 @@ object BlueAutoLeft { .start() } } -} +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt index e0797f7..b741ca0 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/constants/AutoStartPose.kt @@ -5,7 +5,7 @@ import com.acmerobotics.roadrunner.geometry.Pose2d enum class AutoStartPose(val startPose: Pose2d) { BLUE_LEFT( Pose2d( - -12.0, 62.0, Math.toRadians(270.0) + 38.0, 62.0, Math.toRadians(270.0) ) ), 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 98162a7..c0c61cf 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 @@ -50,14 +50,14 @@ class Blue1Plus1: OpMode() { elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) - .addTemporalMarker(1.0) { + .addTemporalMarker(0.1) { intakeSubsystem.intake() intakeBeltSubsystem.intakePos() } .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .back(4.0) .addTemporalMarker(3.0) { - armSubsystem.setpoint = Math.toRadians(97.5) + armSubsystem.setpoint = Math.toRadians(98.0) } .waitSeconds(1.0) .addTemporalMarker(4.0) { @@ -101,26 +101,26 @@ class Blue1Plus1: OpMode() { .waitSeconds(2.0) .turn(Math.toRadians(-40.0)) .addTemporalMarker(17.0) { - armSubsystem.setpoint = 97.5 + armSubsystem.setpoint = Math.toRadians(98.0) } .waitSeconds(1.0) .back(0.5) .addTemporalMarker(18.0) { elevatorSubsystem.setpoint = 33.0 } -// .waitSeconds(3.0) -// .addTemporalMarker(21.0) { -// intakeBeltSubsystem.outtakePos() -// } -// .waitSeconds(1.0) -//// .lineToSplineHeading(Pose2d(36.0, 8.0, Math.toRadians(180.0))) -// .addTemporalMarker(22.0) { -// intakeSubsystem.outtake() -// } -//// .waitSeconds(1.0) -//// .addTemporalMarker(13.0) { -//// elevatorSubsystem.setpoint = 1000.0 -//// } + .waitSeconds(3.0) + .addTemporalMarker(21.0) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(22.0) { + intakeSubsystem.outtake() + } + .waitSeconds(1.0) + .addTemporalMarker(23.0) { + elevatorSubsystem.setpoint = 0.0 + } + .splineToSplineHeading(Pose2d(25.0, 10.0, Math.toRadians(0.0)), Math.toRadians(180.0)) .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt new file mode 100644 index 0000000..b74cf04 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2.kt @@ -0,0 +1,212 @@ +package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left + +import com.acmerobotics.dashboard.FtcDashboard +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo +import org.firstinspires.ftc.teamcode.constants.AutoStartPose +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.IntakeBeltSubsystem +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem + +@Autonomous(name = "Blue 1 + 2 High Basket", group = "Basket", preselectTeleOp = "MainTeleOp") +class Blue1Plus2: OpMode() { + private lateinit var armLeft: Motor + private lateinit var armRight: Motor + private lateinit var elevatorLeft: Motor + private lateinit var elevatorRight: Motor + + private lateinit var intake: Servo + private lateinit var intakeBelt: Servo + + private lateinit var driveSubsystem: DriveSubsystem + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem + private lateinit var armSubsystem: ArmSubsystem + private lateinit var elevatorSubsystem: ElevatorSubsystem + + override fun init() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) + + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) + + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + + driveSubsystem = DriveSubsystem(hardwareMap) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + armSubsystem = ArmSubsystem(armRight, armLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + + val trajectory = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + .addTemporalMarker(2.0) { + intakeSubsystem.intake() + intakeBeltSubsystem.intakePos() + } + .splineToLinearHeading(Pose2d(54.0, 55.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .back(4.0) + .addTemporalMarker(1.5) { + armSubsystem.setpoint = Math.toRadians(98.0) + } + .waitSeconds(1.0) + .addTemporalMarker(4.0) { + elevatorSubsystem.setpoint = 30.0 + } + .waitSeconds(1.0) + .addTemporalMarker(4.5) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.5) + .addTemporalMarker(6.0) { + intakeSubsystem.outtake() + } + .waitSeconds(0.5) + .addTemporalMarker(6.5) { + intakeBeltSubsystem.intakePos() + elevatorSubsystem.setpoint = 0.0 + } + .turn(Math.toRadians(32.0)) + .waitSeconds(1.0) + .forward(3.0) + .addTemporalMarker(7.5) { + armSubsystem.setpoint = Math.toRadians(0.0) + } + .waitSeconds(1.0) + .addTemporalMarker(8.5) { + elevatorSubsystem.setpoint = 30.0 + } + .waitSeconds(1.5) + .addTemporalMarker(10.0) { + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.5) + .addTemporalMarker(11.5) { + intakeSubsystem.intake() + } + .waitSeconds(0.5) + .addTemporalMarker(12.0) { + intakeBeltSubsystem.outtakePos() + elevatorSubsystem.setpoint = 0.0 + } + .turn(Math.toRadians(-30.0)) + .waitSeconds(1.0) + .back(2.0) + .addTemporalMarker(13.0) { + armSubsystem.setpoint = Math.toRadians(98.0) + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(15.0) { + elevatorSubsystem.setpoint = 33.0 + } + .waitSeconds(2.0) + .addTemporalMarker(17.0) { + intakeBeltSubsystem.outtakePos() + } + .turn(Math.toRadians(52.0)) + .waitSeconds(1.5) + .addTemporalMarker(18.5) { + intakeSubsystem.outtake() + } + .waitSeconds(0.5) + .addTemporalMarker(19.0) { + intakeBeltSubsystem.intakePos() + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(1.5) + .addTemporalMarker(20.5) { + intakeBeltSubsystem.outtakePos() + armSubsystem.setpoint = 0.0 + } + .waitSeconds(1.0) + .forward(2.0) + .addTemporalMarker(21.5) { + elevatorSubsystem.setpoint = 30.0 + intakeBeltSubsystem.intakePos() + } + .turn(Math.toRadians(-60.0)) + .waitSeconds(2.0) + .addTemporalMarker(23.5) { + intakeSubsystem.intake() + } + .waitSeconds(0.5) + .addTemporalMarker(24.0) { + intakeBeltSubsystem.outtakePos() + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(0.5) + .back(2.0) + .addTemporalMarker(24.5) { + armSubsystem.setpoint = Math.toRadians(98.0) + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.5) + .addTemporalMarker(27.0) { + elevatorSubsystem.setpoint = 33.0 + } + .waitSeconds(1.5) + .addTemporalMarker(28.5) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(29.0) { + intakeSubsystem.outtake() + } + // .strafetolinearheading + +// .waitSeconds(0.5) +// .addTemporalMarker(6.0) { +// intakeBeltSubsystem.outtakePos() +// armSubsystem.setpoint = 0.0 +// } +// .turn(Math.toRadians(40.0)) +// .waitSeconds(0.5) +// .addTemporalMarker(6.5) { +// elevatorSubsystem.setpoint = 30.0 +// } +// .waitSeconds(1.0) +// .addTemporalMarker(7.5) { +// intakeBeltSubsystem.intakePos() +// } +// .waitSeconds(0.5) +// .addTemporalMarker(8.0) { +// intakeSubsystem.intake() +// } +// .waitSeconds(0.5) +// .addTemporalMarker(8.5) { +// elevatorSubsystem.setpoint = 0.0 +// intakeBeltSubsystem.outtakePos() +// } +// .turn(Math.toRadians(-40.0)) +// .waitSeconds(1.0) +// .addTemporalMarker(9.5) { +// armSubsystem.setpoint = Math.toRadians(98.0) +// } + .build() + + driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose + driveSubsystem.followTrajectorySequenceAsync(trajectory) + } + + override fun loop() { + driveSubsystem.update() + + elevatorSubsystem.periodic() + armSubsystem.periodic() + + telemetry.addData("Arm Position", armSubsystem.armAngle) + telemetry.addData("Slides Position", elevatorSubsystem.slidePos) + telemetry.update() + } +} \ No newline at end of file diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt new file mode 100644 index 0000000..a2b5676 --- /dev/null +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt @@ -0,0 +1,194 @@ +package org.firstinspires.ftc.teamcode.opModes.auto.blue.basket.left + +import com.acmerobotics.dashboard.FtcDashboard +import com.acmerobotics.dashboard.telemetry.MultipleTelemetry +import com.acmerobotics.roadrunner.geometry.Pose2d +import com.arcrobotics.ftclib.hardware.motors.Motor +import com.qualcomm.robotcore.eventloop.opmode.Autonomous +import com.qualcomm.robotcore.eventloop.opmode.OpMode +import com.qualcomm.robotcore.hardware.Servo +import org.firstinspires.ftc.teamcode.constants.AutoStartPose +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.IntakeBeltSubsystem +import org.firstinspires.ftc.teamcode.subsystems.intake.IntakeSubsystem +import org.firstinspires.ftc.teamcode.subsystems.slides.ElevatorSubsystem + +@Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp") +class Blue1Plus2V2: OpMode() { + private lateinit var armLeft: Motor + private lateinit var armRight: Motor + private lateinit var elevatorLeft: Motor + private lateinit var elevatorRight: Motor + + private lateinit var intake: Servo + private lateinit var intakeBelt: Servo + + private lateinit var driveSubsystem: DriveSubsystem + private lateinit var intakeSubsystem: IntakeSubsystem + private lateinit var intakeBeltSubsystem: IntakeBeltSubsystem + private lateinit var armSubsystem: ArmSubsystem + private lateinit var elevatorSubsystem: ElevatorSubsystem + + override fun init() { + telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) + + intake = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE.deviceName) + intakeBelt = hardwareMap.get(Servo::class.java, ControlBoard.INTAKE_BELT.deviceName) + + armLeft = Motor(hardwareMap, ControlBoard.ARM_LEFT.deviceName) + armRight = Motor(hardwareMap, ControlBoard.ARM_RIGHT.deviceName) + + elevatorLeft = Motor(hardwareMap, ControlBoard.SLIDES_LEFT.deviceName) + elevatorRight = Motor(hardwareMap, ControlBoard.SLIDES_RIGHT.deviceName) + + driveSubsystem = DriveSubsystem(hardwareMap) + intakeSubsystem = IntakeSubsystem(intake) + intakeBeltSubsystem = IntakeBeltSubsystem(intakeBelt) + armSubsystem = ArmSubsystem(armRight, armLeft) + elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) + + val toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0)) + .addTemporalMarker(0.1) { + intakeSubsystem.intake() + armSubsystem.setpoint = Math.toRadians(98.0) + intakeBeltSubsystem.intakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(1.0) { + elevatorSubsystem.setpoint = 34.0 + intakeBeltSubsystem.intakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(1.5) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(2.0) { + intakeSubsystem.outtake() + } + .waitSeconds(0.1) + .addTemporalMarker(2.1) { + intakeBeltSubsystem.outtakePos() + elevatorSubsystem.setpoint = 0.0 + } + .turn(Math.toRadians(37.0)) + .waitSeconds(2.0) + .addTemporalMarker(4.1) { + armSubsystem.setpoint = Math.toRadians(0.0) + } + .waitSeconds(0.5) + .addTemporalMarker(4.6) { + elevatorSubsystem.setpoint = 19.0 + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.25) + .addTemporalMarker(6.0) { + intakeSubsystem.intake() + } + .waitSeconds(0.25) + .turn(Math.toRadians(-37.0)) + .addTemporalMarker(6.25) { + intakeBeltSubsystem.outtakePos() + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(1.0) + .addTemporalMarker(7.05) { + armSubsystem.setpoint = Math.toRadians(98.0) + intakeBeltSubsystem.intakePos() + } + .waitSeconds(0.75) + .addTemporalMarker(7.8) { + elevatorSubsystem.setpoint = 31.0 + } + .waitSeconds(1.0) + .addTemporalMarker(8.8) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.25) + .turn(Math.toRadians(55.0)) + .addTemporalMarker(10.05) { + intakeSubsystem.outtake() + } + .waitSeconds(0.25) + .addTemporalMarker(10.3) { + intakeBeltSubsystem.intakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(10.8) { + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(1.5) + .addTemporalMarker(12.3) { + armSubsystem.setpoint = 0.0 + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(13.3) { + elevatorSubsystem.setpoint = 19.0 + } + .waitSeconds(1.0) + .addTemporalMarker(14.3) { + intakeBeltSubsystem.intakePos() + } + .turn(Math.toRadians(-55.0)) + .waitSeconds(1.2) + .back(2.0) + .addTemporalMarker(15.5) { + intakeSubsystem.intake() + } + .waitSeconds(0.5) + .addTemporalMarker(16.0) { + intakeBeltSubsystem.outtakePos() + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(1.0) + .addTemporalMarker(17.0) { + armSubsystem.setpoint = Math.toRadians(98.0) + intakeBeltSubsystem.intakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(18.0) { + elevatorSubsystem.setpoint = 29.0 + } + .waitSeconds(1.5) + .addTemporalMarker(19.5) { + intakeBeltSubsystem.outtakePos() + } + .waitSeconds(1.0) + .addTemporalMarker(20.5) { + intakeSubsystem.outtake() + } + .waitSeconds(0.5) + .addTemporalMarker(21.0) { + intakeBeltSubsystem.intakePos() + } + .waitSeconds(0.5) + .addTemporalMarker(21.5) { + elevatorSubsystem.setpoint = 0.0 + } + .waitSeconds(1.0) + .addTemporalMarker(22.5) { + armSubsystem.setpoint = 0.0 + } + .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0) + .back(5.0) + .build() + driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose + driveSubsystem.followTrajectorySequenceAsync(toBasket1) + } + + + override fun loop() { + driveSubsystem.update() + + elevatorSubsystem.periodic() + armSubsystem.periodic() + + telemetry.addData("Arm Position", armSubsystem.armAngle) + telemetry.addData("Slides Position", elevatorSubsystem.slidePos) + telemetry.update() + } +} \ 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 8596dd5..7964e8c 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 @@ -14,7 +14,7 @@ class IntakeBeltSubsystem( } fun intakePos() { - intakeBelt.position = 0.5 + intakeBelt.position = 0.47 beltPos = !beltPos } 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 e36427a..03518b5 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 @@ -7,6 +7,8 @@ import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup import com.arcrobotics.ftclib.trajectory.TrapezoidProfile import com.arcrobotics.ftclib.util.MathUtils.clamp +import com.qualcomm.robotcore.hardware.DcMotor +import com.qualcomm.robotcore.hardware.DcMotorEx import org.firstinspires.ftc.teamcode.constants.SlidesConstants import kotlin.math.sin @@ -49,7 +51,6 @@ class ElevatorSubsystem( init { elevatorRight.inverted = true elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) - extendMotors.resetEncoder() extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) } 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 3cdd8f2..fd8441e 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 @@ -20,7 +20,6 @@ class OpenElevatorSubsystem( init { elevatorRight.inverted = true elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) - elevatorMotors.resetEncoder() } From 8d66e18cff1962cfbb8f8d788d2178b7c166ea5b Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sun, 8 Dec 2024 10:19:45 -0500 Subject: [PATCH 20/21] Hang yourself --- .../auto/blue/basket/left/Blue1Plus2V2.kt | 9 ++++++++- .../ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 10 ++++++++++ .../subsystems/slides/ElevatorSubsystem.kt | 18 ++++++++++++++---- 3 files changed, 32 insertions(+), 5 deletions(-) diff --git a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt index a2b5676..67197c4 100644 --- a/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt +++ b/TeamCode/src/main/kotlin/org/firstinspires/ftc/teamcode/opModes/auto/blue/basket/left/Blue1Plus2V2.kt @@ -14,6 +14,7 @@ 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.ElevatorSubsystem +import org.firstinspires.ftc.teamcode.utils.roadrunner.trajectorysequence.TrajectorySequence @Autonomous(name = "V2 Blue 1 + 2", group = "Basket", preselectTeleOp = "MainTeleOp") class Blue1Plus2V2: OpMode() { @@ -31,6 +32,9 @@ class Blue1Plus2V2: OpMode() { private lateinit var armSubsystem: ArmSubsystem private lateinit var elevatorSubsystem: ElevatorSubsystem + private lateinit var toBasket1: TrajectorySequence + private lateinit var toBasket2: TrajectorySequence + override fun init() { telemetry = MultipleTelemetry(telemetry, FtcDashboard.getInstance().telemetry) @@ -49,7 +53,7 @@ class Blue1Plus2V2: OpMode() { armSubsystem = ArmSubsystem(armRight, armLeft) elevatorSubsystem = ElevatorSubsystem(elevatorRight, elevatorLeft, armSubsystem::armAngle) - val toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) + toBasket1 = driveSubsystem.trajectorySequenceBuilder(AutoStartPose.BLUE_LEFT.startPose) .splineToLinearHeading(Pose2d(54.0, 56.0, Math.toRadians(225.0)), Math.toRadians(0.0)) .addTemporalMarker(0.1) { intakeSubsystem.intake() @@ -175,6 +179,9 @@ class Blue1Plus2V2: OpMode() { } .splineToSplineHeading(Pose2d(28.0, 10.0, Math.toRadians(0.0)), 90.0) .back(5.0) + .addDisplacementMarker { + driveSubsystem.followTrajectorySequenceAsync(toBasket2) + } .build() driveSubsystem.poseEstimate = AutoStartPose.BLUE_LEFT.startPose driveSubsystem.followTrajectorySequenceAsync(toBasket1) 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 2a402fc..f42f9de 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 @@ -130,6 +130,16 @@ class MainTeleOp : CommandOpMode() { InstantCommand({ intakeBeltSubsystem.decreasePos() }) ) + driver.getGamepadButton(GamepadKeys.Button.Y) + .whenPressed( + RunCommand({ + slidesSubsystem.disable() + slidesSubsystem.spinDown() + armSubsystem.anticlockwise() + }) + ) + + driveSubsystem.defaultCommand = driveCommand RunCommand({ 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 03518b5..45f3c20 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 @@ -7,14 +7,12 @@ import com.arcrobotics.ftclib.hardware.motors.Motor import com.arcrobotics.ftclib.hardware.motors.MotorGroup import com.arcrobotics.ftclib.trajectory.TrapezoidProfile import com.arcrobotics.ftclib.util.MathUtils.clamp -import com.qualcomm.robotcore.hardware.DcMotor -import com.qualcomm.robotcore.hardware.DcMotorEx import org.firstinspires.ftc.teamcode.constants.SlidesConstants import kotlin.math.sin @Config class ElevatorSubsystem( - private val elevatorRight : Motor, + elevatorRight : Motor, elevatorLeft : Motor, private val slideAngle: () -> Double ) : SubsystemBase() { @@ -51,8 +49,12 @@ class ElevatorSubsystem( init { elevatorRight.inverted = true elevatorRight.encoder.setDirection(Motor.Direction.REVERSE) + extendMotors.resetEncoder() extendMotors.setZeroPowerBehavior(Motor.ZeroPowerBehavior.BRAKE) + + setpoint = 0.0 + enabled = true } override fun periodic() { @@ -68,8 +70,16 @@ class ElevatorSubsystem( enabled != enabled } + fun disable() { + enabled = false + } + fun spinUp() { - extendMotors.set(1.0); + extendMotors.set(1.0) + } + + fun spinDown() { + extendMotors.set(-1.0) } fun stop() { From 1e229e50248327f9b311c3bcc08fb90d6a9b82ae Mon Sep 17 00:00:00 2001 From: Sachet Korada Date: Sun, 8 Dec 2024 10:20:03 -0500 Subject: [PATCH 21/21] Hang yourself --- .../org/firstinspires/ftc/teamcode/opModes/teleOp/MainTeleOp.kt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 f42f9de..ad985c2 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 @@ -136,7 +136,7 @@ class MainTeleOp : CommandOpMode() { slidesSubsystem.disable() slidesSubsystem.spinDown() armSubsystem.anticlockwise() - }) + }).perpetually() )