TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverOpMode.java
/**
     * Gamepad Y and right bumper to trigger the endgame sequence
     */
Gamepad Y and right bumper to trigger the endgame sequence
Gamepad Y and right bumper to trigger the endgame sequence
private void handlePowerBar(){
        if(!yPressed && gamepad1.y) {
            if (gamepad1.right_bumper) {
                DriveConstraints moveFast = new DriveConstraints(30.0, 20.0, 0.0, Math.toRadians(360.0), Math.toRadians(360.0), 0.0);
                DriveConstraints constraints = new DriveConstraints(20.0, 10.0, 0.0, Math.toRadians(360.0), Math.toRadians(360.0), 0.0);
                powerBar = new SequentialComboTask();
                ParallelComboTask par1 = new ParallelComboTask();
                Pose2d pb1, pb2, pb3;
                if (robotVision.isTargetVisible()) {
                    Pose2d p1 = robotVision.getNavigationLocalization();
                    robotHardware.getTrackingWheelLocalizer().setPoseEstimate(p1);
                    Logger.logFile("Update pose with tracking target to " + p1);
                    pb1 = getProfilePose("SHOOT-POWER-BAR-3");
                    pb2 = getProfilePose("SHOOT-POWER-BAR-2");
                    pb3 = getProfilePose("SHOOT-POWER-BAR-1");
                    Trajectory traj2 = robotHardware.getMecanumDrive().trajectoryBuilder(p1, true)
                            .splineToSplineHeading(pb1, pb1.getHeading(), constraints)  
                            .build();
                    par1.addTask(new SplineMoveTask(robotHardware.getMecanumDrive(), traj2));
                }
                else {
                    Logger.logFile(("pose y " + robotHardware.getMecanumDrive().getPoseEstimate().getY()));
                    Pose2d p0 = null, p1 = null;
                    if (robotHardware.getMecanumDrive().getPoseEstimate().getY() > -25) {
                        p0 = getProfilePose("SHOOT-PB-LEFT");
                        p1 = getProfilePose("SHOOT-PB-LEFT-1");
                        pb1 = getProfilePose("SHOOT-POWER-BAR-1");
                        pb2 = getProfilePose("SHOOT-POWER-BAR-2");
                        pb3 = getProfilePose("SHOOT-POWER-BAR-3");
                    }
                    else {
                        p0 = getProfilePose("SHOOT-PB-RIGHT");
                        p1 = getProfilePose("SHOOT-PB-RIGHT-1");
                        pb1 = getProfilePose("SHOOT-POWER-BAR-3");
                        pb2 = getProfilePose("SHOOT-POWER-BAR-2");
                        pb3 = getProfilePose("SHOOT-POWER-BAR-1");
                    }
                    robotHardware.getMecanumDrive().setPoseEstimate(p0);
                    Trajectory traj1 = robotHardware.getMecanumDrive().trajectoryBuilder(p0, constraints)
                            .lineToLinearHeading(p1, constraints)
                            .build();
                    powerBar.addTask(new SplineMoveTask(robotHardware.getMecanumDrive(), traj1));
                    Trajectory traj2 = robotHardware.getMecanumDrive().trajectoryBuilder(p1, moveFast)
                            .splineToSplineHeading(pb1, pb1.getHeading())
                            .build();
                    par1.addTask(new SplineMoveTask(robotHardware.getMecanumDrive(), traj2));
                }
                par1.addTask(new MoveArmTask(robotHardware, robotProfile, RobotHardware.ArmPosition.HOLD, 1));
                par1.addTask(new IntakeMotorTask(robotHardware, robotProfile,  IntakeMotorTask.IntakeMode.STOP));
                par1.addTask(new RingHolderPosTask(robotHardware, robotProfile, RingHolderPosTask.RingHolderPosition.UP));
                par1.addTask(new ShooterMotorTask(robotHardware, robotProfile, true, robotProfile.hardwareSpec.shootBarVelocity));
                powerBar.addTask(par1);
                powerBar.addTask(new ShootOneRingTask(robotHardware, robotProfile));
                Trajectory traj3 = robotHardware.getMecanumDrive().trajectoryBuilder(pb1, constraints)
                        .lineToLinearHeading(pb2, constraints)
                        .build();
                powerBar.addTask(new SplineMoveTask(robotHardware.getMecanumDrive(), traj3));
                powerBar.addTask(new ShootOneRingTask(robotHardware, robotProfile));
                Trajectory traj4 = robotHardware.getMecanumDrive().trajectoryBuilder(pb2, constraints)
                        .lineToLinearHeading(pb3, constraints)
                        .build();
                powerBar.addTask(new SplineMoveTask(robotHardware.getMecanumDrive(), traj4));
                powerBar.addTask(new ShootOneRingTask(robotHardware, robotProfile));
                powerBar.addTask(new ShooterMotorTask(robotHardware, robotProfile, false));
                currentTask = powerBar;
                currentTask.prepare();
                currentMode = ActionMode.SHOOTING;
            }
        }
    }
private void handlePowerBar ( ) { if ( ! yPressed && gamepad1 . y ) { if ( gamepad1 . right_bumper ) { DriveConstraints moveFast = new DriveConstraints ( 30.0 , 20.0 , 0.0 , Math . toRadians ( 360.0 ) , Math . toRadians ( 360.0 ) , 0.0 ) ; DriveConstraints constraints = new DriveConstraints ( 20.0 , 10.0 , 0.0 , Math . toRadians ( 360.0 ) , Math . toRadians ( 360.0 ) , 0.0 ) ; powerBar = new SequentialComboTask ( ) ; ParallelComboTask par1 = new ParallelComboTask ( ) ; Pose2d pb1 , pb2 , pb3 ; if ( robotVision . isTargetVisible ( ) ) { Pose2d p1 = robotVision . getNavigationLocalization ( ) ; robotHardware . getTrackingWheelLocalizer ( ) . setPoseEstimate ( p1 ) ; Logger . logFile ( "Update pose with tracking target to " + p1 ) ; pb1 = getProfilePose ( "SHOOT-POWER-BAR-3" ) ; pb2 = getProfilePose ( "SHOOT-POWER-BAR-2" ) ; pb3 = getProfilePose ( "SHOOT-POWER-BAR-1" ) ; Trajectory traj2 = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( p1 , true ) . splineToSplineHeading ( pb1 , pb1 . getHeading ( ) , constraints ) . build ( ) ; par1 . addTask ( new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj2 ) ) ; } else { Logger . logFile ( ( "pose y " + robotHardware . getMecanumDrive ( ) . getPoseEstimate ( ) . getY ( ) ) ) ; Pose2d p0 = null , p1 = null ; if ( robotHardware . getMecanumDrive ( ) . getPoseEstimate ( ) . getY ( ) > - 25 ) { p0 = getProfilePose ( "SHOOT-PB-LEFT" ) ; p1 = getProfilePose ( "SHOOT-PB-LEFT-1" ) ; pb1 = getProfilePose ( "SHOOT-POWER-BAR-1" ) ; pb2 = getProfilePose ( "SHOOT-POWER-BAR-2" ) ; pb3 = getProfilePose ( "SHOOT-POWER-BAR-3" ) ; } else { p0 = getProfilePose ( "SHOOT-PB-RIGHT" ) ; p1 = getProfilePose ( "SHOOT-PB-RIGHT-1" ) ; pb1 = getProfilePose ( "SHOOT-POWER-BAR-3" ) ; pb2 = getProfilePose ( "SHOOT-POWER-BAR-2" ) ; pb3 = getProfilePose ( "SHOOT-POWER-BAR-1" ) ; } robotHardware . getMecanumDrive ( ) . setPoseEstimate ( p0 ) ; Trajectory traj1 = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( p0 , constraints ) . lineToLinearHeading ( p1 , constraints ) . build ( ) ; powerBar . addTask ( new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj1 ) ) ; Trajectory traj2 = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( p1 , moveFast ) . splineToSplineHeading ( pb1 , pb1 . getHeading ( ) ) . build ( ) ; par1 . addTask ( new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj2 ) ) ; } par1 . addTask ( new MoveArmTask ( robotHardware , robotProfile , RobotHardware . ArmPosition . HOLD , 1 ) ) ; par1 . addTask ( new IntakeMotorTask ( robotHardware , robotProfile , IntakeMotorTask . IntakeMode . STOP ) ) ; par1 . addTask ( new RingHolderPosTask ( robotHardware , robotProfile , RingHolderPosTask . RingHolderPosition . UP ) ) ; par1 . addTask ( new ShooterMotorTask ( robotHardware , robotProfile , true , robotProfile . hardwareSpec . shootBarVelocity ) ) ; powerBar . addTask ( par1 ) ; powerBar . addTask ( new ShootOneRingTask ( robotHardware , robotProfile ) ) ; Trajectory traj3 = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( pb1 , constraints ) . lineToLinearHeading ( pb2 , constraints ) . build ( ) ; powerBar . addTask ( new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj3 ) ) ; powerBar . addTask ( new ShootOneRingTask ( robotHardware , robotProfile ) ) ; Trajectory traj4 = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( pb2 , constraints ) . lineToLinearHeading ( pb3 , constraints ) . build ( ) ; powerBar . addTask ( new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj4 ) ) ; powerBar . addTask ( new ShootOneRingTask ( robotHardware , robotProfile ) ) ; powerBar . addTask ( new ShooterMotorTask ( robotHardware , robotProfile , false ) ) ; currentTask = powerBar ; currentTask . prepare ( ) ; currentMode = ActionMode . SHOOTING ; } } }
Gamepad Y and right bumper to trigger the endgame sequence
Gamepad Y and right bumper to trigger the endgame sequence
// If target is visible, use the vision location to start with // slower move to warm up the shoot motor // assume we are starting from either corner