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() }