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