TeamCode/src/main/java/org/firstinspires/ftc/teamcode/DriverOpMode.java
/**
     * Resets the current localization position based on the tracking target by pressing Y
     * and drives to optimal shooting location by pressing left bumper and Y
     */
Resets the current localization position based on the tracking target by pressing Y
and drives to optimal shooting location by pressing left bumper and Y
Resets the current localization position based on the tracking target by pressing Y and drives to optimal shooting location by pressing left bumper and Y
private void handleVision() {
        if (!yPressed && gamepad1.y) {
            Pose2d currentPosition = robotVision.getNavigationLocalization();
            if (currentPosition != null) {
                robotHardware.getTrackingWheelLocalizer().setPoseEstimate(currentPosition);
                Logger.logFile("PoseEstimate:" + currentPosition);
            }
            else {
                currentPosition = robotHardware.getTrackingWheelLocalizer().getPoseEstimate();
            }
            if (gamepad1.left_bumper) {
                robotHardware.stopIntake();
                robotHardware.startShootMotor();
                robotHardware.ringHolderUp();
                robotHardware.setShooterPosition(true);
                currentMode = ActionMode.SHOOTING;
                double ang = Math.atan2(shootingPose.getX() - currPose.getX(), shootingPose.getY() - currPose.getY());
                double dist = Math.hypot(shootingPose.getX() - currPose.getX(), shootingPose.getY() - currPose.getY());
                if (dist>30 || Math.abs(currPose.getHeading() - shootingPose.getHeading()) < Math.PI / 4) {
                    boolean forward = Math.abs(currPose.getHeading() - ang) < Math.PI / 2;
                    Logger.logFile("Spline From " + currPose + " F:" + forward);
                    Logger.flushToFile();
                    DriveConstraints constraints = new DriveConstraints(30.0, 20.0, 0.0, Math.toRadians(360.0), Math.toRadians(360.0), 0.0);
                    Trajectory traj = robotHardware.getMecanumDrive().trajectoryBuilder(currPose, !forward)
                            .splineToSplineHeading(shootingPose, shootingPose.getHeading(), constraints).build();
                    currentTask = new SplineMoveTask(robotHardware.getMecanumDrive(), traj);
                }
                else {
                    Logger.logFile("Line From " + currPose);
                    Logger.flushToFile();
                    DriveConstraints constraints = new DriveConstraints(30.0, 20.0, 0.0, Math.toRadians(360.0), Math.toRadians(360.0), 0.0);
                    Trajectory traj = robotHardware.getMecanumDrive().trajectoryBuilder(currPose, constraints)
                            .lineToLinearHeading(shootingPose).build();
                    currentTask = new SplineMoveTask(robotHardware.getMecanumDrive(), traj);
                }
                currentTask.prepare();
            }
        }
        yPressed = gamepad1.y;
    }
private void handleVision ( ) { if ( ! yPressed && gamepad1 . y ) { Pose2d currentPosition = robotVision . getNavigationLocalization ( ) ; if ( currentPosition != null ) { robotHardware . getTrackingWheelLocalizer ( ) . setPoseEstimate ( currentPosition ) ; Logger . logFile ( "PoseEstimate:" + currentPosition ) ; } else { currentPosition = robotHardware . getTrackingWheelLocalizer ( ) . getPoseEstimate ( ) ; } if ( gamepad1 . left_bumper ) { robotHardware . stopIntake ( ) ; robotHardware . startShootMotor ( ) ; robotHardware . ringHolderUp ( ) ; robotHardware . setShooterPosition ( true ) ; currentMode = ActionMode . SHOOTING ; double ang = Math . atan2 ( shootingPose . getX ( ) - currPose . getX ( ) , shootingPose . getY ( ) - currPose . getY ( ) ) ; double dist = Math . hypot ( shootingPose . getX ( ) - currPose . getX ( ) , shootingPose . getY ( ) - currPose . getY ( ) ) ; if ( dist > 30 || Math . abs ( currPose . getHeading ( ) - shootingPose . getHeading ( ) ) < Math . PI / 4 ) { boolean forward = Math . abs ( currPose . getHeading ( ) - ang ) < Math . PI / 2 ; Logger . logFile ( "Spline From " + currPose + " F:" + forward ) ; Logger . flushToFile ( ) ; DriveConstraints constraints = new DriveConstraints ( 30.0 , 20.0 , 0.0 , Math . toRadians ( 360.0 ) , Math . toRadians ( 360.0 ) , 0.0 ) ; Trajectory traj = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( currPose , ! forward ) . splineToSplineHeading ( shootingPose , shootingPose . getHeading ( ) , constraints ) . build ( ) ; currentTask = new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj ) ; } else { Logger . logFile ( "Line From " + currPose ) ; Logger . flushToFile ( ) ; DriveConstraints constraints = new DriveConstraints ( 30.0 , 20.0 , 0.0 , Math . toRadians ( 360.0 ) , Math . toRadians ( 360.0 ) , 0.0 ) ; Trajectory traj = robotHardware . getMecanumDrive ( ) . trajectoryBuilder ( currPose , constraints ) . lineToLinearHeading ( shootingPose ) . build ( ) ; currentTask = new SplineMoveTask ( robotHardware . getMecanumDrive ( ) , traj ) ; } currentTask . prepare ( ) ; } } yPressed = gamepad1 . y ; }
Resets the current localization position based on the tracking target by pressing Y
 and drives to optimal shooting location by pressing left bumper and Y
Resets the current localization position based on the tracking target by pressing Y and drives to optimal shooting location by pressing left bumper and Y
// determine if we want to go backward or foward based on currPose // calculate the angle from current position to the shooting position // use spline move if distance to shooting location is greater than 30 inch away or when we are using the tracking target in the front //When is looking at the side target, use the straight line trajectory