VCSCdecode/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/ScrimAuto.java at master · VCSilverCircuits/VCSCdecode · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
package org.firstinspires.ftc.teamcode;

import com.bylazar.configurables.annotations.Configurable;
import com.bylazar.telemetry.TelemetryManager;
import com.pedropathing.follower.Follower;
import com.pedropathing.geometry.Pose;
import com.pedropathing.paths.PathChain;
import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DcMotorSimple;
import com.qualcomm.robotcore.util.ElapsedTime;

import org.firstinspires.ftc.robotcore.external.Telemetry;

import java.util.FormatFlagsConversionMismatchException;
import java.util.function.Supplier;

@Configurable
@Autonomous(name = "ScrimAuto", group = "competition")
public class ScrimAuto extends LinearOpMode {
    DcMotor Intake;
    DcMotor Launcher;
    DcMotor Output1;
    DcMotor Output2;
    DcMotor leftFront;
    DcMotor rightFront;
    DcMotor leftRear;
    DcMotor rightRear;
    final double driveRatio = (3.6*3.6); //2 4:1 boxes on drivetrain
    final double ticksPerInch = (((2000)*48*3.14159*2)/25.4); //ticks per rotation times circumfrence of odo wheel converted to inches
    ElapsedTime timer = new ElapsedTime();
    //Telemetry telemetry = new Tel;

    @Override
    public void runOpMode() throws InterruptedException {

        Intake = hardwareMap.get(DcMotorEx.class, "Intake");
        Launcher = hardwareMap.get(DcMotor.class, "Launcher");
        Output1 = hardwareMap.get(DcMotor.class, "Output1");
        Output2 = hardwareMap.get(DcMotor.class, "Output2");
        leftFront = hardwareMap.get(DcMotorEx.class, "lf");
        rightFront = hardwareMap.get(DcMotor.class, "rf");
        leftRear = hardwareMap.get(DcMotor.class, "lr");
        rightRear = hardwareMap.get(DcMotor.class, "rr");
        leftFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightFront.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        leftRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightRear.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
        rightFront.setDirection(DcMotorSimple.Direction.FORWARD);
        leftFront.setDirection(DcMotorSimple.Direction.REVERSE);
        rightRear.setDirection(DcMotorSimple.Direction.FORWARD);
        leftRear.setDirection(DcMotorSimple.Direction.REVERSE);
        Output1.setDirection(DcMotorSimple.Direction.REVERSE);
        Intake.setDirection(DcMotorSimple.Direction.REVERSE);
        Launcher.setDirection(DcMotorSimple.Direction.REVERSE);

        waitForStart();
        leftFront.setPower(0.6);
        rightFront.setPower(0.6);
        leftRear.setPower(0.6);
        rightRear.setPower(0.6);
        Output1.setPower(0.8);
        Output2.setPower(0.8);
        timer.reset();
            while(timer.milliseconds() < 1000){

                //telemetry.addLine("waiting");
            }
            leftFront.setPower(0);
            rightFront.setPower(0);
            leftRear.setPower(0);
            rightRear.setPower(0);
            timer.reset();
            while(timer.milliseconds()<1000){
                //telemetry.addLine("waiting");

            }
            Intake.setPower(1);
            Launcher.setPower(1);
            //while(timer.milliseconds()<100) {
                //timer.reset();

            while(timer.milliseconds()<1000) {
                Launcher.setPower(0);
            }
            timer.reset();
            while(timer.milliseconds()<100) {
                Launcher.setPower(1);
            }
            timer.reset();
            while(timer.milliseconds()<1000) {
                Launcher.setPower(0);
            }
            timer.reset();
            while(timer.milliseconds()<100) {
                Launcher.setPower(1);
            }
                timer.reset();
                while(timer.milliseconds()<1000) {
                    Launcher.setPower(0);
                }
                timer.reset();
                while(timer.milliseconds()<100){
                    Launcher.setPower(1);
        }
            Intake.setPower(0);
            Launcher.setPower(0);
            Output1.setPower(0);
            Output2.setPower(0);
            timer.reset();
            while(timer.milliseconds()<1000) {
                leftFront.setPower(0.6);
                rightFront.setPower(-0.6);
                leftRear.setPower(-0.6);
                rightRear.setPower(0.6);
            }
    }
}