IntoTheDeep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutoOp.java at master · IGNITE-Pathways/IntoTheDeep · 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
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
package org.firstinspires.ftc.teamcode;

import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.util.ElapsedTime;

@Autonomous(name = "Auto Op", group = "Linear OpMode")
public class AutoOp extends LinearOpMode {

    protected final ElapsedTime runtime = new ElapsedTime();

    private DcMotor leftFrontDrive = null;
    private DcMotor leftBackDrive = null;
    private DcMotor rightFrontDrive = null;
    private DcMotor rightBackDrive = null;

    Intake intake = null;
    Outtake outtake = null;

    @Override
    public void runOpMode() {

        intake = new Intake(hardwareMap);
        outtake = new Outtake(hardwareMap);

        // Initialize the hardware variables. Note that the strings used here must correspond
        // to the names assigned during the robot configuration step on the DS or RC devices.
        leftFrontDrive = hardwareMap.get(DcMotor.class, "leftfront"); //ehub 0
        leftBackDrive = hardwareMap.get(DcMotor.class, "leftback"); //ehub 1
        rightFrontDrive = hardwareMap.get(DcMotor.class, "rightfront"); //chub 2
        rightBackDrive = hardwareMap.get(DcMotor.class, "rightback"); //chub 3 //Encoder used for ODO Y

        leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
        leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
        rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
        rightBackDrive.setDirection(DcMotor.Direction.FORWARD);

        outtake.closeClaw();
        sleep(100);
        outtake.setOuttakeClawPosition(ClawPosition.CLOSE);

        outtake.setOuttakeArmPosition(OuttakeArmPosition.SPECIMEN_DROP);

        // Telemetry
        telemetry.addData("Arm Left Pos", outtake.getLeftArmPosition());
        telemetry.addData("Arm Right Pos", outtake.getRightArmPosition());
        telemetry.addData("Claw Pos", outtake.getClawPosition());
        telemetry.addData("Status", "Initialized");

        // Show the elapsed game time and wheel power.
        telemetry.update();

        waitForStart();
        runtime.reset();

        outtake.setOuttakeSlidesPositionSync(OuttakeSlidesPosition.HOOK_SPECIMEN_TOP_RUNG);
        sleep(1000);

        moveForward(-.5, 1050);
        moveForward(-.1, 200);

        //Move Slides to Drop
        outtake.setOuttakeArmPosition(OuttakeArmPosition.POST_SPECIMEN_DROP);
        sleep(200);
        //If arm in position
        outtake.setOuttakeSlidesPosition(OuttakeSlidesPosition.POST_HOOK);
        sleep(400);

        outtake.setOuttakeClawPosition(ClawPosition.OPEN);
        sleep(200);

        //Initialization Positions
        outtake.setOuttakeArmPosition(OuttakeArmPosition.FACING_DOWN);
        sleep(400);

        outtake.setOuttakeSlidesPositionSync(OuttakeSlidesPosition.CLOSE);
        sleep(1000);

        moveForward(.5, 500);
        sleep(100);
        strafeLeft(.8, 1000);

         // Telemetry
        telemetry.addData("Time Used", runtime.seconds());

        // Show the elapsed game time and wheel power.
        telemetry.update();
    }

    private void moveForward(double speed, int milliSeconds) {
        leftFrontDrive.setPower(speed);
        rightFrontDrive.setPower(speed);
        leftBackDrive.setPower(speed);
        rightBackDrive.setPower(speed);

        sleep(milliSeconds);

        leftFrontDrive.setPower(0);
        rightFrontDrive.setPower(0);
        leftBackDrive.setPower(0);
        rightBackDrive.setPower(0);
    }

    private void strafeLeft(double speed, int milliSeconds) {
        //inward
        leftFrontDrive.setPower(-speed);
        leftBackDrive.setPower(speed);

        //outward
        rightFrontDrive.setPower(speed);
        rightBackDrive.setPower(-speed * 0.9);

        sleep(milliSeconds);

        // Stop all motors
        leftFrontDrive.setPower(0);
        rightFrontDrive.setPower(0);
        leftBackDrive.setPower(0);
        rightBackDrive.setPower(0);
    }

    private void strafeRight(double speed, int milliSeconds) {
        //Spinning outward
        leftFrontDrive.setPower(speed * .95); //Adjust for bad strafing
        leftBackDrive.setPower(-speed);

        //Spinning inward
        rightFrontDrive.setPower(-speed);
        rightBackDrive.setPower(speed);

        sleep(milliSeconds);

        // Stop all motors
        leftFrontDrive.setPower(0);
        rightFrontDrive.setPower(0);
        leftBackDrive.setPower(0);
        rightBackDrive.setPower(0);
    }

}