forked from FIRST-Tech-Challenge/FtcRobotController
-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathAutoBlueObsZone.java
More file actions
191 lines (191 loc) · 10.1 KB
/
AutoBlueObsZone.java
File metadata and controls
191 lines (191 loc) · 10.1 KB
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
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
//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.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 blue obs Zone", group = "Linear OpMode")
//public class AutoBlueObsZone extends LinearOpMode {
//
// protected final ElapsedTime runtime = new ElapsedTime();
// SequentialAction sequence1 = null;
// SequentialAction sequence2 = null;
// SequentialAction sequence3 = null;
// SequentialAction sequence4 = null;
//
// protected boolean pick2Samples = true;
//
// @Override
// public void runOpMode() {
//
// DcMotor leftFrontDrive = hardwareMap.get(DcMotorEx.class, "rightback"); //rightback
// DcMotor leftBackDrive = hardwareMap.get(DcMotorEx.class, "rightfront"); //rightfront`
// DcMotor rightBackDrive = hardwareMap.get(DcMotorEx.class, "leftfront"); //leftfront
// DcMotor rightFrontDrive = hardwareMap.get(DcMotorEx.class, "leftback"); //leftback
// DigitalChannel greenLED = hardwareMap.get(DigitalChannel.class, "greenled"); //6
// DigitalChannel redLED = hardwareMap.get(DigitalChannel.class, "redled"); //7
//
// leftFrontDrive.setDirection(DcMotor.Direction.REVERSE);
// leftBackDrive.setDirection(DcMotor.Direction.REVERSE);
// rightFrontDrive.setDirection(DcMotor.Direction.FORWARD);
// rightBackDrive.setDirection(DcMotor.Direction.FORWARD);
//
// greenLED.setMode(DigitalChannel.Mode.OUTPUT);
// redLED.setMode(DigitalChannel.Mode.OUTPUT);
//
// Viper viper = new Viper(hardwareMap);
// Extendo extendo = new Extendo(hardwareMap);
// viper.initialize();
// extendo.initialize();
// greenLED.setState(true); //SPECIMEN ONLY
// redLED.setState(false);
//
// // GAME FIELD CAN BE DIFFERENT BECAUSE OF HOW IT IS ASSEMBLED OR HOW THE MATS ARE PLACED / CUT
// double startingXPosition = -8;
// double startingYPosition = 62;
// double moveRobotByInches = 26.4;
//
// Pose2d beginPose = new Pose2d(startingXPosition, startingYPosition, Math.toRadians(-90));
// PinpointDrive drive = new PinpointDrive(hardwareMap, beginPose);
//
// // DEFINE X POSITIONS
// double firstSpecimenXPosition = -2;
// double secondSpecimenXPosition = -5;
// double thirdSpecimenXPosition = -6;
// double obsZoneXPosition = -44;
// double obsZoneSpecimenPickupYPositionError = 2;
//
// Action moveToDropFirstSpecimen = drive.actionBuilder(beginPose)
// .strafeTo(new Vector2d(firstSpecimenXPosition, startingYPosition - moveRobotByInches))
// .build();
//
// Action dragSampleFromSpikeMarkToObsZone =
// drive.actionBuilder(new Pose2d(firstSpecimenXPosition, startingYPosition - moveRobotByInches, Math.toRadians(-90))) //35
// .strafeTo(new Vector2d(-34, startingYPosition - moveRobotByInches + 4)) //Y=39
// .strafeTo((new Vector2d(-34, startingYPosition - moveRobotByInches - 15))) //Y=20
// .splineToLinearHeading(new Pose2d(obsZoneXPosition, startingYPosition - 52, Math.toRadians(90)), Math.toRadians(90)) //Y=10
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - 9)) //Y=53
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - 17)) //Y=45 -- go back
// //@TODO: ADD DELAY So Human Player can clip Sample to make a Specimen
// .waitSeconds(pick2Samples ? 4: 0)
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - obsZoneSpecimenPickupYPositionError - 0.5)) //Y=60 >> Moving 50 inches straight
// .build();
//
// sequence1 = new SequentialAction(
// viper.getReadyToDropSpecimen(),
// new SleepAction(3.0),
// moveToDropFirstSpecimen,
// viper.driveToPositionInInches(XBot.DROPPED_SPECIMEN),
// viper.openClaw(),
// viper.driveToPositionInInches(XBot.VIPER_PICK_SPECIMEN),
// dragSampleFromSpikeMarkToObsZone);
//
// Action moveBackAndPickSpecimentAtTheSameTime = drive.actionBuilder(new Pose2d(obsZoneXPosition, startingYPosition - obsZoneSpecimenPickupYPositionError - 0.5, Math.toRadians(90)))
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - 4)) //Y=48
// .afterDisp(.1, viper.getReadyToDropSpecimen()) // viper.driveToPositionInInches(XBot.VIPER_PICK_SPECIMEN + 4)
// .build();
//
// Action moveToDropSecondSpecimen = drive.actionBuilder(new Pose2d(obsZoneXPosition, startingYPosition - 4, Math.toRadians(90)))
// .setReversed(true)
// .splineToLinearHeading(new Pose2d(secondSpecimenXPosition, startingYPosition - moveRobotByInches + 2, Math.toRadians(-90)), Math.toRadians(-90)) //Y=37
// .strafeTo(new Vector2d(secondSpecimenXPosition, startingYPosition - moveRobotByInches)) //Y=35
// .build();
//
// //If pick2Samples = false
// Action splineToPickThirdSpecimenFromObservationZone = drive.actionBuilder(new Pose2d(secondSpecimenXPosition, startingYPosition - moveRobotByInches, Math.toRadians(-90)))
// .setReversed(true)
// .splineToLinearHeading(new Pose2d(obsZoneXPosition, startingYPosition - 8, Math.toRadians(90)), Math.toRadians(90)) //Y=54
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - obsZoneSpecimenPickupYPositionError + 1.5))
// .build();
//
// //If pick2Samples = true
// Action splineToPickSampleFromObservationZoneAndPark = drive.actionBuilder(new Pose2d(secondSpecimenXPosition, startingYPosition - moveRobotByInches, Math.toRadians(-90)))
// .setReversed(true)
// .splineToLinearHeading(new Pose2d(-38, startingYPosition -6 , Math.toRadians(0)), Math.toRadians(180)) //Y=54
// .build();
//
// sequence2 = new SequentialAction(
// viper.closeClaw(), new SleepAction(.4), //Grabs Specimen from the wall
// moveBackAndPickSpecimentAtTheSameTime,
// moveToDropSecondSpecimen,
// viper.driveToPositionInInches(XBot.DROPPED_SPECIMEN),
// viper.openClaw(),
// viper.driveToPositionInInches(XBot.VIPER_PICK_SPECIMEN));
//
// Action moveBackAndPickSpecimentAtTheSameTime1 = drive.actionBuilder(new Pose2d(obsZoneXPosition, startingYPosition - obsZoneSpecimenPickupYPositionError + 1.5, Math.toRadians(90)))
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - 5)) //Y=48
// .afterDisp(.1, viper.getReadyToDropSpecimen()) // viper.driveToPositionInInches(XBot.VIPER_PICK_SPECIMEN + 4)
// .waitSeconds(.1)
// .build();
//
// Action moveToDropThirdSpecimen = drive.actionBuilder(new Pose2d(obsZoneXPosition, startingYPosition - 5, Math.toRadians(90)))
// .setReversed(true)
// .splineToLinearHeading(new Pose2d(thirdSpecimenXPosition, startingYPosition - moveRobotByInches + 2, Math.toRadians(-90)), Math.toRadians(-90)) //Y=37
// .afterDisp(.1, viper.getReadyToDropSpecimen())
// .strafeTo(new Vector2d(thirdSpecimenXPosition, startingYPosition - moveRobotByInches - 1.5)) //Y=35
// .build();
//
// sequence3 = new SequentialAction(
// viper.closeClaw(), new SleepAction(.4), //Grabs Specimen from the wall
// moveBackAndPickSpecimentAtTheSameTime1,
// moveToDropThirdSpecimen,
// viper.driveToPositionInInches(XBot.DROPPED_SPECIMEN),
// viper.openClaw());
//
// Action parkingAction = drive.actionBuilder(new Pose2d(thirdSpecimenXPosition, startingYPosition - moveRobotByInches - 1.5, Math.toRadians(-90)))
// .strafeTo(new Vector2d(obsZoneXPosition, startingYPosition - 5)) //Y=57
// .build();
//
// sequence4 = new SequentialAction(viper.closeClaw(), parkingAction, viper.driveToPositionInInches(XBot.VIPER_HOME));
//
// telemetry.addData("Status", "Initialized");
// telemetry.update();
//
// waitForStart();
// runtime.reset();
//
// ///////////////////////////////////////////////////////////////////////////////
//
// //sequence1 = 1st Specimen dropped, 2nd Specimen in Obs Zone, Robot in Obs Zone
// Actions.runBlocking(sequence1);
// telemetry.addData("First Sequence:", runtime.seconds());
//
// Actions.runBlocking(viper.closeClaw());
//
// //sequence2 = 2nd Specimen dropped, Robot in Obs Zone
// Actions.runBlocking(sequence2);
// telemetry.addData("Second Sequence:", runtime.seconds());
//
// if (pick2Samples) {
// Actions.runBlocking(new SequentialAction(
// splineToPickSampleFromObservationZoneAndPark,
// viper.driveToPositionInInches(XBot.VIPER_HOME),
// extendo.elbowDown()));
// } else {
// Actions.runBlocking(splineToPickThirdSpecimenFromObservationZone);
//
// //sequence3 = 3rd Specimen dropped
// Actions.runBlocking(sequence3); //3nd Specimen dropped
// telemetry.addData("Third Sequence:", runtime.seconds());
//
// //sequence4 = Robot Parked
// Actions.runBlocking(sequence4);
// telemetry.addData("Forth Sequence:", runtime.seconds());
// }
//
// telemetry.update();
// }
//
//}
//
//
//