IntoTheDeep/TeamCode/src/main/java/org/firstinspires/ftc/teamcode/AutonomousREDObservationZone.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
142
143
144
145
146
147
148
149
150
151
//package org.firstinspires.ftc.teamcode;
//
//import com.acmerobotics.roadrunner.Action;
//import com.acmerobotics.roadrunner.Pose2d;
//import com.acmerobotics.roadrunner.SequentialAction;
//import com.acmerobotics.roadrunner.SleepAction;
//import com.acmerobotics.roadrunner.Vector2d;
//import com.acmerobotics.roadrunner.ftc.Actions;
//import com.qualcomm.robotcore.eventloop.opmode.Autonomous;
//import com.qualcomm.robotcore.eventloop.opmode.Disabled;
//import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
//import com.qualcomm.robotcore.hardware.DcMotor;
//import com.qualcomm.robotcore.hardware.DcMotorEx;
//import com.qualcomm.robotcore.hardware.DigitalChannel;
//import com.qualcomm.robotcore.util.ElapsedTime;
//
//@Autonomous(name = "Auto RED Obs Zone", group = "Linear OpMode")
//@Disabled
//public class AutonomousREDObservationZone extends LinearOpMode {
//
//    // Declare OpMode members for each of the 4 motors.
//    private final ElapsedTime runtime = new ElapsedTime();
//    boolean rolling = false;
//    private DcMotor leftFrontDrive = null;
//    private DcMotor leftBackDrive = null;
//    private DcMotor rightFrontDrive = null;
//    private DcMotor rightBackDrive = null;
//
//    private DigitalChannel redLED;
//    private DigitalChannel greenLED;
//
//    @Override
//    public void runOpMode() {
//
//        leftFrontDrive = hardwareMap.get(DcMotorEx.class, "rightback"); //rightback
//        leftBackDrive = hardwareMap.get(DcMotorEx.class, "rightfront"); //rightfront`
//        rightBackDrive = hardwareMap.get(DcMotorEx.class, "leftfront"); //leftfront
//        rightFrontDrive = hardwareMap.get(DcMotorEx.class, "leftback"); //leftback
//
//        redLED = hardwareMap.get(DigitalChannel.class, "redled");//7
//        greenLED = hardwareMap.get(DigitalChannel.class, "greenled");//6
//
//        leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
//        leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
//        rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
//        rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
//
//        telemetry.addData("Status", "Initialized");
//        telemetry.update();
//        // change LED mode from input to output
//        redLED.setMode(DigitalChannel.Mode.OUTPUT);
//        greenLED.setMode(DigitalChannel.Mode.OUTPUT);
//
//        Pose2d beginPose = new Pose2d(8, -62, Math.toRadians(90));
//        PinpointDrive drive = new PinpointDrive(hardwareMap, beginPose);
//        Viper viper = new Viper(hardwareMap);
//        Extendo extendo = new Extendo(hardwareMap);
//        viper.initialize();
//        extendo.initialize();
//
//        waitForStart();
//        runtime.reset();
//
//        Action moveToDropSpecimenLocation = drive.actionBuilder(new Pose2d(8, -62, Math.toRadians(90)))
//                .strafeTo(new Vector2d(2, -36))
//                .build();
//
//        //Move to drop specimen while rising
//        SequentialAction raiseViperWhenMovingToDropSpecimen = new SequentialAction(
//                viper.getReadyToDropSpecimen(),
//                moveToDropSpecimenLocation);
//
//        //drops preloaded specimen on the chamber
//        SequentialAction dropSpecimenSequence = new SequentialAction(
//                raiseViperWhenMovingToDropSpecimen,
//                viper.driveToPositionInInches(XBot.DROPPED_SPECIMEN),
//                viper.openClaw());
//
//        Action moveToThePositionOfFIRSTSample =
//                drive.actionBuilder(new Pose2d(2, -36, Math.toRadians(90)))
//                        .strafeTo(new Vector2d(33, -40)) // drives to the right
//                        .strafeTo((new Vector2d(35, -35))) // gets ready to do a nice spline, without hitting the truss holding up the submersible
//                        .splineToLinearHeading(new Pose2d(46, -10, Math.toRadians(-90)), Math.toRadians(270)) // splines to the first sample
//                        .build();
//
//        //Move the viper slide down while moving to the position to pick next sample
//        SequentialAction moveViperDownWhenMovingToTheFIRSTSample = new SequentialAction(
//                dropSpecimenSequence,
//                viper.driveToPositionInInches(XBot.VIPER_HOME),
//                moveToThePositionOfFIRSTSample);
//
//        Action pushTheSampleIntoTheObservationZone =
//                drive.actionBuilder(new Pose2d(46, -10, Math.toRadians(-90)))
//                        .strafeTo(new Vector2d(46, -60)) // pushes first sample into observation zone
//                        .build();
//
//        SequentialAction SequenceOfActions = new SequentialAction(
//                dropSpecimenSequence,
//                moveViperDownWhenMovingToTheFIRSTSample,
//                pushTheSampleIntoTheObservationZone);
//
//        Actions.runBlocking(SequenceOfActions);
//        telemetry.addData("Time Used", runtime.seconds());
//
////        Action pushSecondSampleToObsZone = drive.actionBuilder(new Pose2d(46, -55, Math.toRadians(360)))
////                .setReversed(true)
////                .splineToConstantHeading(new Vector2d(55, -16), Math.toRadians(360))  // goes to second sample
////                .setReversed(false)
////                .strafeTo(new Vector2d(55, -55)) // pushes 2nd sample into the observation zone
////                .build();
//
////        Action pickSecondSpecimen = drive.actionBuilder(new Pose2d(46, -60, Math.toRadians(-90)))
////                .strafeTo(new Vector2d(62.5, -57)) // goes to hook specimen with the CLAW
////                .build();
//
////        Actions.runBlocking(pushSecondSampleToObsZone);
//
//        SequentialAction pickAction = new SequentialAction(
//                viper.driveToPositionInInches(XBot.VIPER_PICK_SPECIMEN),
//                new SleepAction(.2),
//                viper.closeClaw(),
//                new SleepAction(.2),
//                viper.getReadyToDropSpecimen(),
//                new SleepAction(.2));
//
//        Action moveToDropSecondSpecimen = drive.actionBuilder(new Pose2d(46, -60, Math.toRadians(-90)))
//                .setReversed(true)
//                .splineToLinearHeading(new Pose2d(12, -32, Math.toRadians(90)), Math.toRadians(90)) // splines to chamber to hook second specimen
//                .build();
//
//        Actions.runBlocking(new SequentialAction(pickAction, moveToDropSecondSpecimen,
//                viper.driveToPositionInInches(XBot.DROPPED_SPECIMEN),
//                viper.openClaw()));
//
//        Action parkAction = drive.actionBuilder(new Pose2d(12, -32, Math.toRadians(90)))
//                                .strafeTo(new Vector2d(-44,62)).build();
//
//        Actions.runBlocking(new SequentialAction(parkAction, extendo.elbowVertical(),
//                viper.driveToPositionInInches(XBot.VIPER_HOME)));
//
//        // Telemetry
//        telemetry.addData("Time Used", runtime.seconds());
//
//        // Show the elapsed game time and wheel power.
//        telemetry.update();
//    }
//
//}
//
//
//