Examples with Drive com.team2753.subsystems.Drive used on opensource projects

Example 6 with Drive

use of com.team2753.subsystems.Drive 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 7 with Drive

use of com.team2753.subsystems.Drive 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 8 with Drive

use of com.team2753.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class FindVA method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    // Drivetrain
    Drive mDrive = new Drive();
    mDrive.init(this, false);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    // plot
    PhoneLogger logger = new PhoneLogger("MaxVelMaxAccTimeData.csv");
    waitForStart();
    // Max Speed
    mDrive.setLeftRightPower(1, 1);
    ElapsedTime time = new ElapsedTime();
    ElapsedTime dt = new ElapsedTime();
    double lastPosition = 0;
    double lastSpeed = 0;
    // Go for 3 seconds
    while (opModeIsActive() && time.seconds() < 3) {
        double currentLeftPos = mDrive.getLeftDistanceInches();
        double left_speed = (currentLeftPos - lastPosition) / dt.seconds();
        double accl = (left_speed - lastSpeed) / dt.seconds();
        // Make sure the file is not 60k large with no data
        if (left_speed != 0 && accl != 0)
            logger.write(time.seconds() + "," + left_speed + "," + accl);
        lastPosition = currentLeftPos;
        lastSpeed = left_speed;
        dt.reset();
    }
    // Close the logger
    logger.close();
}

Example 9 with Drive

use of com.team2753.subsystems.Drive 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;
    }
}

Example 10 with Drive

use of com.team2753.subsystems.Drive in project Relic_Main by TeamOverdrive.

the class NewTestSplines method runOpMode.

@Override
public void runOpMode() throws InterruptedException {
    telemetry.addData("P", Constants.p.getDouble());
    telemetry.addData("D", Constants.d.getDouble());
    telemetry.addData("V", Constants.v.getDouble());
    telemetry.addData("A", Constants.a.getDouble());
    telemetry.update();
    // Path driveOffStone = Line.calculate(defaultTrajectoryConfig,
    // SCurvesStrategy,
    // 0.0,
    // 0.00001,
    // 10,
    // 0.00001);
    // Path fullPath = ScalePath.calculate(driveOffStone,
    // 1.0/Constants.scale.getDouble());
    Trajectory ref = TrajectoryGenerator.generate(defaultTrajectoryConfig, TrajectoryGenerator.SCurvesStrategy, 0, 0, 10, 0, 0.000001);
    // Init our drivetrain
    Drive mDrive = new Drive();
    // true = use gyros and reset the encoders
    mDrive.init(this, true);
    mDrive.setRunMode(DcMotor.RunMode.RUN_USING_ENCODER);
    // Lets go
    telemetry.setAutoClear(false);
    telemetry.addData("Ready to Start", "");
    telemetry.update();
    waitForStart();
    telemetry.setAutoClear(true);
    // New Drive Controller
    TrajectoryDriveController controller = new TrajectoryDriveController(mDrive, defaultFollowerConfig);
    controller.loadProfile(ref, ref, 1, 1);
    ElapsedTime dtTimer = new ElapsedTime();
    while (opModeIsActive() && !controller.onTarget()) {
        // data
        telemetry.addData("kV", Constants.v.getDouble());
        telemetry.addData("kA", Constants.a.getDouble());
        telemetry.addData("Left Distance", mDrive.getLeftDistanceInches());
        telemetry.addData("Right Distance", mDrive.getRightDistanceInches());
        telemetry.addData("Left Speed", controller.wantedLeftSpeed);
        telemetry.addData("Right Speed", controller.wantedRightSpeed);
        telemetry.addData("Speed output", controller.getGoal());
        telemetry.addData("Gyro", mDrive.getGyroAngleDegrees());
        telemetry.addData("dt", dtTimer.seconds());
        telemetry.addData("Number of Segments", controller.getNumSegments());
        telemetry.addData("Current Pos", controller.getFollowerCurrentSegment());
        telemetry.update();
        // Make sure we only update when we told the controller/trajectory we would
        if (dtTimer.seconds() > defaultTrajectoryConfig.dt) {
            controller.update();
            idle();
            dtTimer.reset();
        }
    }
}