Examples with FollowPath com.team2753.splines.FollowPath used on opensource projects

Example 1 with FollowPath

use of com.team2753.splines.FollowPath in project Relic_Main by TeamOverdrive.

the class Drive method driveTrajectory.

public void driveTrajectory(double leftDistance, double rightDistance, long timeout) {
    int lastLeft = getLeftCurrentPosition();
    int lastRight = getRightCurrentPosition();
    Trajectory left;
    Trajectory right;
    left = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(leftDistance), 0, 0);
    right = TrajectoryGenerator.generate(defaultTrajectoryConfig, strategy, 0, 0, Math.abs(rightDistance), 0, 0);
    if (leftDistance < 0) {
        left.scale(-1);
    }
    if (rightDistance < 0) {
        right.scale(-1);
    }
    new FollowPath(linearOpMode, this, new Path("", new Trajectory.Pair(left, right)), timeout, 1, 1);
    setRunMode(DcMotor.RunMode.RUN_TO_POSITION);
    this.setLeftRightTarget((int) ((leftDistance * COUNTS_PER_INCH) + lastLeft), (int) ((rightDistance * COUNTS_PER_INCH) + lastRight));
    setLeftRightPower(0.2, 0.2);
    while (linearOpMode.opModeIsActive() && (leftMotor.isBusy() || rightMotor.isBusy())) Thread.yield();
    setLeftRightPower(0, 0);
    setRunMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}

Example 2 with FollowPath

use of com.team2753.splines.FollowPath in project Relic_Main by TeamOverdrive.

the class Defence_R_Far_Multi method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    HoldMyPaths.calculateRedRightToGlyphPit();
    waitForStart("RFar Defence", AutoParams.AUTO);
    Robot.getDrive().zeroSensors();
    ElapsedTime t = new ElapsedTime();
    SuperHitJewel(this.jewel_Color, Jewel_Color.Red);
    switch(RelicRecoveryVuMark.LEFT) {
        case LEFT:
            // Drive off Stone
            Robot.getDrive().encoderDrive(0.4, 30, 30, 3);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 3);
            // Drive 19 in
            Robot.getDrive().encoderDrive(0.5, -19, -19, 3);
            // Turn 90
            Robot.getDrive().turnCCW(90, 0.4, 3);
            ScorePreloadReleaseIntake();
            sleep(300);
            Robot.getDrive().setLeftRightForTime(0.3, 0.3, 400);
            // Drive Backward
            Robot.getDrive().encoderDrive(0.6, -10, -10, 3);
            Robot.getDrive().turnCW(28, 0.5, 2);
            double distanceLeftChange = 20;
            Robot.getDrive().driveTrajectory(-36 - distanceLeftChange, -36 - distanceLeftChange, 6 * 1000);
            sleep(200);
            Robot.getDrive().turnCW(6, 0.3, 2);
            Robot.getDrive().driveTrajectory(48 + distanceLeftChange, 48 + distanceLeftChange, 5 * 1000);
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING)) idle();
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            sleep((long) Range.clip((28000 - t.milliseconds()), 0, 30000));
            Robot.getDrive().setLeftRightForTime(-0.5, -0.5, 400);
            break;
        case CENTER:
            // Drive off
            Robot.getDrive().encoderDrive(0.6, 31, 31, 3);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 2);
            // Drive 17 in
            Robot.getDrive().encoderDrive(0.5, -17, -17, 2.5);
            // Turn 54
            Robot.getDrive().turnCCW(54, 0.4, 3);
            ScorePreloadReleaseIntake();
            // Drive Backward
            Robot.getDrive().encoderDrive(0.6, -5, -5, 3);
            Robot.getDrive().turnCCW(13, 0.5, 2);
            Robot.getDrive().encoderDrive(0.5, -25, -25, 5);
            sleep(200);
            Robot.getDrive().encoderDrive(0.6, 35, 35, 3);
            while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING) && opModeIsActive()) idle();
            Robot.getIntake().reverse();
            sleep(300);
            while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            Robot.getDrive().encoderDrive(0.3, -6, -6, 3);
            Robot.getDrive().encoderDrive(0.4, 6, 6, 3);
            Robot.getDrive().encoderDrive(0.3, -5, -5, 3);
            break;
        case RIGHT:
            // Drive off
            Robot.getDrive().encoderDrive(0.6, 31, 31, 3);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 2);
            // Drive 11 in
            Robot.getDrive().encoderDrive(0.5, -11, -11, 2.5);
            // Turn 54
            Robot.getDrive().turnCCW(54, 0.4, 3);
            ScorePreloadReleaseIntake();
            new FollowPath(this, Robot.getDrive(), HoldMyPaths.farRedRightToGlyphPit, -1, 1);
            Robot.getDrive().encoderDrive(1, 35, 35, 4);
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING)) idle();
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            Robot.getDrive().encoderDrive(0.3, -6, -6, 3);
            break;
    }
    sleep((long) Range.clip((28000 - t.milliseconds()), 0, 30000));
    while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.INTAKING)) idle();
    sleep((long) Range.clip((29500 - t.milliseconds()), 0, 30000));
}

Example 3 with FollowPath

use of com.team2753.splines.FollowPath in project Relic_Main by TeamOverdrive.

the class R_Far_Multi method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    HoldMyPaths.calculateRedRightToGlyphPit();
    waitForStart("RFar", AutoParams.AUTO);
    Robot.getDrive().zeroSensors();
    ElapsedTime t = new ElapsedTime();
    Robot.getRelic().hold();
    SuperHitJewel(this.jewel_Color, Jewel_Color.Red);
    switch(WhatColumnToScoreIn()) {
        case LEFT:
            // Drive off Stone
            Robot.getDrive().encoderDrive(0.4, 30, 30, 3, this);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 3.5);
            // Drive 19 in
            Robot.getDrive().encoderDrive(0.5, -19, -19, 3, this);
            // Turn 90
            Robot.getDrive().turnCCW(90, 0.4, 3);
            ScorePreloadReleaseIntake();
            sleep(300);
            Robot.getDrive().setLeftRightForTime(0.3, 0.3, 400);
            // Drive Backward
            Robot.getDrive().encoderDrive(0.6, -10, -10, 3);
            Robot.getDrive().turnCW(28, 0.5, 2);
            Robot.getDrive().driveTrajectory(-36, -36, 6 * 1000);
            sleep(200);
            Robot.getDrive().turnCW(6, 0.3, 2);
            Robot.getDrive().driveTrajectory(48, 48, 5 * 1000);
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING)) idle();
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            sleep((long) Range.clip((28000 - t.milliseconds()), 0, 30000));
            Robot.getDrive().setLeftRightForTime(-0.5, -0.5, 400);
            break;
        case CENTER:
            // Drive off
            Robot.getDrive().encoderDrive(0.3, 31, 31, 3, this);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 3.5);
            // Drive 17 in
            Robot.getDrive().encoderDrive(0.5, -17, -17, 3, this);
            // Turn 54
            Robot.getDrive().turnCCW(54, 0.4, 3);
            ScorePreloadReleaseIntake();
            // Drive Backward
            Robot.getDrive().encoderDrive(0.6, -5, -5, 3);
            Robot.getDrive().turnCCW(13, 0.5, 2);
            Robot.getDrive().driveTrajectory(-30, -30, 5 * 1000);
            sleep(200);
            Robot.getDrive().driveTrajectory(37, 37, 5 * 1000);
            while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING) && opModeIsActive()) idle();
            Robot.getIntake().reverse();
            sleep(300);
            while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            Robot.getDrive().encoderDrive(0.3, -6, -6, 3);
            Robot.getDrive().setLeftRightForTime(0.5, 0.5, 800);
            Robot.getDrive().encoderDrive(0.3, -5, -5, 3);
            break;
        case RIGHT:
            // Drive off
            Robot.getDrive().encoderDrive(0.6, 31, 31, 3, this);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 3.5);
            // Drive 11 in
            Robot.getDrive().encoderDrive(0.5, -11, -11, 2.5);
            // Turn 54
            Robot.getDrive().turnCCW(54, 0.4, 3);
            ScorePreloadReleaseIntake();
            new FollowPath(this, Robot.getDrive(), HoldMyPaths.farRedRightToGlyphPit, -1, 1);
            sleep(400);
            Robot.getDrive().turnCW(-10, 0.4, 3);
            Robot.getDrive().driveTrajectory(40, 40, 4 * 1000);
            sleep(200);
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING)) idle();
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING)) idle();
            sleep(300);
            Robot.getDrive().encoderDrive(0.3, -3, -3, 3);
            Robot.getDrive().setLeftRightForTime(0.5, 0.5, 1000);
            Robot.getDrive().setLeftRightForTime(-0.3, -0.3, 500);
            break;
    }
    sleep((long) Range.clip((28000 - t.milliseconds()), 0, 30000));
    while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.INTAKING)) idle();
    sleep((long) Range.clip((29500 - t.milliseconds()), 0, 30000));
}

Example 4 with FollowPath

use of com.team2753.splines.FollowPath in project Relic_Main by TeamOverdrive.

the class BLUE_CLOSE method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    BlueClose_Paths.calculateBlueClose(new JoshuaField());
    Robot.init(this, true);
    telemetry.addData("Here", "");
    telemetry.update();
    waitForStart();
    this.SuperHitJewel(Jewel_Color.Red, Jewel_Color.Blue);
    Robot.getIntake().releaseIntake();
    Thread intake = new Thread(new Runnable() {

        @Override
        public void run() {
            sleep(200);
            while (!Thread.interrupted()) {
                Robot.getIntake().setLeftRightPower(1.0, 1.0);
                sleep(1500);
                Robot.getIntake().setLeftRightPower(-1.0, 1.0);
                sleep(200);
                Thread.yield();
            }
            sleep(400);
            Robot.getIntake().reverse();
        }
    });
    intake.start();
    BlueClose_Paths.pathFromBlueCloseToGlyphPit.goRight();
    new FollowPath(this, Robot.getDrive(), BlueClose_Paths.pathFromBlueCloseToGlyphPit, -1, -1);
    while (!Robot.getDrive().turnToAngle(-90) && !isStopRequested()) Thread.yield();
    new FollowPath(this, Robot.getDrive(), BlueClose_Paths.pathFromBlueCloseGlyphPitToLeftColumn, 1, 1);
    while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.HOLDING) && !isStopRequested()) Thread.yield();
    intake.interrupt();
    while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING) && !isStopRequested()) Thread.yield();
    sleep(1000);
    while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.INTAKING) && !isStopRequested()) Thread.yield();
// Robot.getDrive().encoderDrive(0.3, -5, -5, 2);
}

Example 5 with FollowPath

use of com.team2753.splines.FollowPath in project Relic_Main by TeamOverdrive.

the class RED_FAR method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    RedFar_Paths.calculateRedFar(new JoshuaField());
    waitForStart("Red", true);
    Robot.getDrive().zeroSensors();
    SuperHitJewel(jewel_Color, Jewel_Color.Red);
    RelicRecoveryVuMark useThisVuMark = RelicRecoveryVuMark.CENTER;
    switch(useThisVuMark) {
        case LEFT:
            Robot.getDrive().encoderDrive(0.4, 30, 30, 3);
            // Turn 90
            Robot.getDrive().turnCW(90, 0.3, 3);
            // Drive forward
            Robot.getDrive().encoderDrive(0.4, -18.8, -18.8, 3);
            // Turn 90
            Robot.getDrive().turnCCW(90, 0.3, 4);
            while (!Robot.getSlammer().setStopperState(Slammer.STOPPER_State.OPEN) && opModeIsActive()) Robot.getIntake().releaseIntake();
            sleep(40);
            new Thread(new Runnable() {

                @Override
                public void run() {
                    sleep(1000);
                    Robot.getSlammer().setStopperState(Slammer.STOPPER_State.CLOSED);
                }
            }).start();
            intakeThread.start();
            // From Column to pit
            new FollowPath(this, Robot.getDrive(), leftToGP, -1, 1);
            sleep(500);
            // Drive a little forward
            Robot.getDrive().encoderDrive(0.3, 0, (Constants.WHEEL_BASE * PI * 25) / 180, 3);
            Robot.getDrive().encoderDrive(0.8, 35, 35, 4);
            intakeThread.interrupt();
            while (!Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.SCORING) && opModeIsActive()) Thread.yield();
            sleep(2000);
            Robot.getDrive().encoderDrive(0.2, -4, -4, 2);
            while (opModeIsActive() && !Robot.getSlammer().setSlammerState(Slammer.SLAMMER_State.INTAKING)) idle();
            break;
        case CENTER:
            new FollowPath(this, Robot.getDrive(), bsToCenter, 1, 1);
            while (!Robot.getSlammer().setStopperState(Slammer.STOPPER_State.OPEN)) Robot.getIntake().releaseIntake();
            break;
        case RIGHT:
            new FollowPath(this, Robot.getDrive(), bsToRight, 1, 1);
            while (!Robot.getSlammer().setStopperState(Slammer.STOPPER_State.OPEN)) Robot.getIntake().releaseIntake();
            // sleep(1000);
            break;
        case UNKNOWN:
            break;
    }
}