-
Notifications
You must be signed in to change notification settings - Fork 0
/
BaseOpMode.java
360 lines (320 loc) · 16.4 KB
/
BaseOpMode.java
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
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.robotcore.hardware.ColorSensor;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.HardwareMap;
import com.qualcomm.robotcore.hardware.IrSeekerSensor;
import com.qualcomm.robotcore.hardware.Servo;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.eventloop.opmode.LinearOpMode;
import java.util.EnumMap;
import java.util.Map;
import java.util.HashMap;
import java.util.HashSet;
/**
* Created by Shaffer_904573 on 11/10/2016.
*/
public class BaseOpMode extends LinearOpMode {
/** START CONFIGURATIONS **/
public static final float BUTTON_THRESHOLD = 0.01f;
public static final String BUTTON_TOGGLE_COLLECT = "g2_a";
public static final String BUTTON_TOGGLE_FIRE = "g2_b";
public static final String BUTTON_PUSH_LEFT = "g2_lbump";
public static final String BUTTON_PUSH_RIGHT = "g2_rbump";
public static final String BUTTON_PUSH_BOTH = "g2_x";
public static final String BUTTON_PUSH_RIGHT_INC = "g2_rsticky";
public static final String BUTTON_PUSH_LEFT_INC = "g2_lsticky";
public static final String BUTTON_STOP_ALL_MOTORS = "g1_b";
public static final String BUTTON_DRIVE_LEFT = "g1_lsticky";
public static final String BUTTON_DRIVE_RIGHT = "g1_rsticky";
public static final String CONFIG_FL_DRIVE = "fl_drive";
public static final String CONFIG_FR_DRIVE = "fr_drive";
public static final String CONFIG_BL_DRIVE = "bl_drive";
public static final String CONFIG_BR_DRIVE = "br_drive";
public static final String CONFIG_COLLECT = "collect";
public static final String CONFIG_CANNON = "cannon";
public static final String CONFIG_COLOR = "color";
public static final String CONFIG_IR_SEEKER = "ir_seeker";
public static final String CONFIG_SERVO_CAP = "capball";
public static final String CONFIG_SERVO_PUSHL = "pushl";
public static final String CONFIG_SERVO_PUSHR = "pushr";
public static final DcMotor.RunMode MODE_DRIVE = DcMotor.RunMode.RUN_USING_ENCODER;
public static final DcMotor.RunMode MODE_AUX = DcMotor.RunMode.RUN_WITHOUT_ENCODER;
public static final IrSeekerSensor.Mode MODE_IR = IrSeekerSensor.Mode.MODE_600HZ;
public static final DcMotor.ZeroPowerBehavior MODE_ZERO = DcMotor.ZeroPowerBehavior.FLOAT;
public static final MotorMode MOTOR_MODE = MotorMode.PARALLEL;
public static final double SERVO_PUSHL_INIT = 0.0;
public static final double SERVO_PUSHL_MIN = 0.0;
public static final double SERVO_PUSHL_MAX = 1.0;
public static final double SERVO_PUSHR_INIT = 0.0;
public static final double SERVO_PUSHR_MIN = 0.0;
public static final double SERVO_PUSHR_MAX = 1.0;
public static final double SERVO_CAP_INIT = 0.0;
public static final double SERVO_CAP_MIN = 0.0;
public static final double SERVO_CAP_MAX = 1.0;
public static final boolean HAS_PUSH = false;
public static final boolean HAS_CAP = false;
public static final boolean HAS_COLOR = false;
public static final boolean HAS_IR = false;
public static final float GAMEPAD_ONE_DEADZONE = 1.0f;
public static final float GAMEPAD_TWO_DEADZONE = 1.0f;
public static final boolean GAMEPAD1_INVERSE_STICKS = false;
public static final boolean GAMEPAD2_INVERSE_STICKS = false;
public static final boolean GAMEPAD1_INVERSE_TRIGGERS = false;
public static final boolean GAMEPAD2_INVERSE_TRIGGERS = false;
public static final boolean TEL_AUTO_CLEAR = true;
public static final int TEL_MS_INTERVAL = 100;
public static final String TEL_ITEM_SEPARATOR = "|";
public static final String TEL_CAPTION_SEPARATOR = ":";
public static final double COLLECTION_SPEED = 1.0;
public static final double CANNON_SPEED = 1.0;
public static final double PUSHR_POS = 1.0;
public static final double PUSHL_POS = 1.0;
/** END CONFIGURATIONS **/
public static DcMotor frontLeft;
public static DcMotor frontRight;
public static DcMotor backLeft;
public static DcMotor backRight;
public static DcMotor collection;
public static DcMotor lowOrbitIonCannon;
public static ColorSensor colorSensor;
public static IrSeekerSensor irSeekerSensor;
public static Servo pushButtonL;
public static Servo pushButtonR;
public static Servo capBall;
public static HardwareMap hardwareMap;
public static ElapsedTime runtime;
enum Function {
COLLECT, FIRE, PUSH_LEFT, PUSH_RIGHT, PUSH_BOTH, PUSH_RIGHT_INC,
PUSH_LEFT_INC, STOP_ALL_MOTORS, DRIVE_LEFT, DRIVE_RIGHT
}
enum MotorMode {
PARALLEL, SQUARE
}
private static final Map<Function, String> buttonMap = new EnumMap<Function, String>(Function.class) {{
put(Function.COLLECT, BUTTON_TOGGLE_COLLECT);
put(Function.FIRE, BUTTON_TOGGLE_FIRE);
put(Function.PUSH_LEFT, BUTTON_PUSH_LEFT);
put(Function.PUSH_RIGHT, BUTTON_PUSH_RIGHT);
put(Function.PUSH_BOTH, BUTTON_PUSH_BOTH);
put(Function.PUSH_RIGHT_INC, BUTTON_PUSH_RIGHT_INC);
put(Function.PUSH_LEFT_INC, BUTTON_PUSH_LEFT_INC);
put(Function.STOP_ALL_MOTORS, BUTTON_STOP_ALL_MOTORS);
put(Function.DRIVE_LEFT, BUTTON_DRIVE_LEFT);
put(Function.DRIVE_RIGHT, BUTTON_DRIVE_RIGHT);
}};
public boolean isActive(Function f) {
switch(buttonMap.get(f)) {
case "g1_a": return gamepad1.a;
case "g1_b": return gamepad1.b;
case "g1_x": return gamepad1.x;
case "g1_y": return gamepad1.y;
case "g2_a": return gamepad2.a;
case "g2_b": return gamepad2.b;
case "g2_x": return gamepad2.x;
case "g2_y": return gamepad2.y;
case "g1_back": return gamepad1.back;
case "g2_back": return gamepad2.back;
case "g1_dpdown": return gamepad1.dpad_down;
case "g2_dpdown": return gamepad2.dpad_down;
case "g1_dpleft": return gamepad1.dpad_left;
case "g2_dpleft": return gamepad2.dpad_left;
case "g1_dpright": return gamepad1.dpad_right;
case "g2_dpright": return gamepad2.dpad_right;
case "g1_dpup": return gamepad1.dpad_up;
case "g2_dpup": return gamepad2.dpad_up;
case "g1_lbump": return gamepad1.left_bumper;
case "g2_lbump": return gamepad2.left_bumper;
case "g1_rbump": return gamepad1.right_bumper;
case "g2_rbump": return gamepad2.right_bumper;
case "g1_rstickbut": return gamepad1.right_stick_button;
case "g2_rstickbut": return gamepad2.right_stick_button;
case "g1_lstickbut": return gamepad1.left_stick_button;
case "g2_lstickbut": return gamepad2.left_stick_button;
case "g1_lstickx": return Math.abs(gamepad1.left_stick_x) > BUTTON_THRESHOLD;
case "g2_lstickx": return Math.abs(gamepad2.left_stick_x) > BUTTON_THRESHOLD;
case "g1_lsticky": return Math.abs(gamepad1.left_stick_y) > BUTTON_THRESHOLD;
case "g2_lsticky": return Math.abs(gamepad2.left_stick_y) > BUTTON_THRESHOLD;
case "g1_rtrigger": return Math.abs(gamepad1.right_trigger) > BUTTON_THRESHOLD;
case "g2_rtrigger": return Math.abs(gamepad2.right_trigger) > BUTTON_THRESHOLD;
case "g1_ltrigger": return Math.abs(gamepad1.left_trigger) > BUTTON_THRESHOLD;
case "g2_ltrigger": return Math.abs(gamepad2.left_trigger) > BUTTON_THRESHOLD;
case "g1_start": return gamepad1.start;
case "g2_start": return gamepad2.start;
case "g1_guide": return gamepad1.guide;
case "g2_guide": return gamepad2.guide;
default: return false;
}
}
public float getValue(Function f) {
switch(buttonMap.get(f)) {
case "g1_a": return gamepad1.a ? 1.0f : 0.0f;
case "g1_b": return gamepad1.b ? 1.0f : 0.0f;
case "g1_x": return gamepad1.x ? 1.0f : 0.0f;
case "g1_y": return gamepad1.y ? 1.0f : 0.0f;
case "g2_a": return gamepad2.a ? 1.0f : 0.0f;
case "g2_b": return gamepad2.b ? 1.0f : 0.0f;
case "g2_x": return gamepad2.x ? 1.0f : 0.0f;
case "g2_y": return gamepad2.y ? 1.0f : 0.0f;
case "g1_back": return gamepad1.back ? 1.0f : 0.0f;
case "g2_back": return gamepad2.back ? 1.0f : 0.0f;
case "g1_dpdown": return gamepad1.dpad_down ? 1.0f : 0.0f;
case "g2_dpdown": return gamepad2.dpad_down ? 1.0f : 0.0f;
case "g1_dpleft": return gamepad1.dpad_left ? 1.0f : 0.0f;
case "g2_dpleft": return gamepad2.dpad_left ? 1.0f : 0.0f;
case "g1_dpright": return gamepad1.dpad_right ? 1.0f : 0.0f;
case "g2_dpright": return gamepad2.dpad_right ? 1.0f : 0.0f;
case "g1_dpup": return gamepad1.dpad_up ? 1.0f : 0.0f;
case "g2_dpup": return gamepad2.dpad_up ? 1.0f : 0.0f;
case "g1_lbump": return gamepad1.left_bumper ? 1.0f : 0.0f;
case "g2_lbump": return gamepad2.left_bumper ? 1.0f : 0.0f;
case "g1_rbump": return gamepad1.right_bumper ? 1.0f : 0.0f;
case "g2_rbump": return gamepad2.right_bumper ? 1.0f : 0.0f;
case "g1_rstickbut": return gamepad1.right_stick_button ? 1.0f : 0.0f;
case "g2_rstickbut": return gamepad2.right_stick_button ? 1.0f : 0.0f;
case "g1_lstickbut": return gamepad1.left_stick_button ? 1.0f : 0.0f;
case "g2_lstickbut": return gamepad2.left_stick_button ? 1.0f : 0.0f;
case "g1_lstickx": return gamepad1.left_stick_x * (GAMEPAD1_INVERSE_STICKS? -1 : 1);
case "g2_lstickx": return gamepad2.left_stick_x * (GAMEPAD2_INVERSE_STICKS? -1 : 1);
case "g1_lsticky": return gamepad1.left_stick_y * (GAMEPAD1_INVERSE_STICKS? -1 : 1);
case "g2_lsticky": return gamepad2.left_stick_y * (GAMEPAD2_INVERSE_STICKS? -1 : 1);
case "g1_rtrigger": return gamepad1.right_trigger * (GAMEPAD1_INVERSE_TRIGGERS? -1 : 1);
case "g2_rtrigger": return gamepad2.right_trigger * (GAMEPAD2_INVERSE_TRIGGERS? -1 : 1);
case "g1_ltrigger": return gamepad1.left_trigger * (GAMEPAD1_INVERSE_TRIGGERS? -1 : 1);
case "g2_ltrigger": return gamepad2.left_trigger * (GAMEPAD2_INVERSE_TRIGGERS? -1 : 1);
case "g1_start": return gamepad1.start ? 1.0f : 0.0f;
case "g2_start": return gamepad2.start ? 1.0f : 0.0f;
case "g1_guide": return gamepad1.guide ? 1.0f : 0.0f;
case "g2_guide": return gamepad2.guide ? 1.0f : 0.0f;
default: return 0.0f;
}
}
static final Map<DcMotor, Double> motorSpeeds = new HashMap<DcMotor, Double>() {{
put(collection, COLLECTION_SPEED);
put(lowOrbitIonCannon, CANNON_SPEED);
}};
public static void run(DcMotor motor) {
if(!motorSpeeds.containsKey(motor)) return;
motor.setPower(motorSpeeds.get(motor));
}
public static void run(DcMotor motor, double speed) {
if(speed < BUTTON_THRESHOLD) return;
motor.setPower(speed);
}
static final class MotorSet {
HashSet<DcMotor> motors;
public MotorSet() { motors = new HashSet<DcMotor>(); }
public MotorSet(DcMotor one, DcMotor two) { this(); add(one); add(two); }
public void add(DcMotor m) { motors.add(m); }
public void run(double speed) {
for(DcMotor m : motors)
m.setPower(speed);
}
public HashSet<DcMotor> getMotors() { return motors; }
}
public static void runMotorSet(MotorSet s, double d) {
for(DcMotor m : s.getMotors()) {
m.setPower(d);
}
}
public static MotorSet getMotorSet(DcMotor motor) {
switch(MOTOR_MODE) {
default:
case PARALLEL:
if(motor == frontLeft || motor == backLeft)
return new MotorSet(frontLeft, backLeft);
else
return new MotorSet(frontRight, backRight);
case SQUARE:
if(motor == backLeft || motor == frontRight)
return new MotorSet(backLeft, frontRight);
else
return new MotorSet(frontLeft, backRight);
}
}
public static void stopAllMotors() {
for(Map.Entry<DcMotor, Double> m : motorSpeeds.entrySet())
m.getKey().setPower(0.0);
}
static final Map<Servo, Double> servoPositions = new HashMap<Servo, Double>() {{
put(pushButtonL, PUSHL_POS);
put(pushButtonR, PUSHR_POS);
}};
public static void set(Servo servo) {
if(!servoPositions.containsKey(servo)) return;
servo.setPosition(servoPositions.get(servo));
}
public static void set(Servo servo, double position) {
servo.setPosition(position);
}
public final void init(HardwareMap hm) {
hardwareMap = hm;
runtime = new ElapsedTime();
frontLeft = hm.dcMotor.get(CONFIG_FL_DRIVE);
frontRight = hm.dcMotor.get(CONFIG_FR_DRIVE);
backLeft = hm.dcMotor.get(CONFIG_BL_DRIVE);
backRight = hm.dcMotor.get(CONFIG_BR_DRIVE);
collection = hm.dcMotor.get(CONFIG_COLLECT);
lowOrbitIonCannon = hm.dcMotor.get(CONFIG_CANNON);
if(HAS_COLOR) colorSensor = hm.colorSensor.get(CONFIG_COLOR);
if(HAS_IR) irSeekerSensor = hm.irSeekerSensor.get(CONFIG_IR_SEEKER);
if(HAS_PUSH) pushButtonL = hm.servo.get(CONFIG_SERVO_PUSHL);
if(HAS_PUSH) pushButtonR = hm.servo.get(CONFIG_SERVO_PUSHR);
if(HAS_CAP) capBall = hm.servo.get(CONFIG_SERVO_CAP);
frontLeft.setPower(0.0);
frontRight.setPower(0.0);
backLeft.setPower(0.0);
backRight.setPower(0.0);
collection.setPower(0.0);
lowOrbitIonCannon.setPower(0.0);
if(HAS_PUSH) pushButtonL.scaleRange(SERVO_PUSHL_MIN, SERVO_PUSHL_MAX);
if(HAS_PUSH) pushButtonR.scaleRange(SERVO_PUSHR_MIN, SERVO_PUSHR_MAX);
if(HAS_CAP) capBall.scaleRange(SERVO_CAP_MIN, SERVO_CAP_MAX);
frontLeft.setMode(MODE_DRIVE);
frontRight.setMode(MODE_DRIVE);
backLeft.setMode(MODE_DRIVE);
backRight.setMode(MODE_DRIVE);
collection.setMode(MODE_AUX);
lowOrbitIonCannon.setMode(MODE_AUX);
if(HAS_IR) irSeekerSensor.setMode(MODE_IR);
if(HAS_PUSH) pushButtonL.setPosition(SERVO_PUSHL_INIT);
if(HAS_PUSH) pushButtonR.setPosition(SERVO_PUSHR_INIT);
if(HAS_CAP) capBall.setPosition(SERVO_CAP_INIT);
frontLeft.setZeroPowerBehavior(MODE_ZERO);
frontRight.setMode(MODE_DRIVE);
backLeft.setMode(MODE_DRIVE);
backRight.setMode(MODE_DRIVE);
collection.setMode(MODE_AUX);
lowOrbitIonCannon.setMode(MODE_AUX);
gamepad1.setJoystickDeadzone(GAMEPAD_ONE_DEADZONE);
gamepad2.setJoystickDeadzone(GAMEPAD_TWO_DEADZONE);
telemetry.setItemSeparator(TEL_ITEM_SEPARATOR);
telemetry.setAutoClear(TEL_AUTO_CLEAR);
telemetry.setMsTransmissionInterval(TEL_MS_INTERVAL);
telemetry.setCaptionValueSeparator(TEL_CAPTION_SEPARATOR);
}
public void runOpMode() {
while(opModeIsActive()) {
for(Function f : Function.values()) {
if(isActive(f)) {
switch(f) {
case COLLECT: run(collection); break;
case FIRE: run(lowOrbitIonCannon); break;
case PUSH_LEFT: set(pushButtonL); break;
case PUSH_RIGHT: set(pushButtonR); break;
case PUSH_BOTH: set(pushButtonL); set(pushButtonR); break;
case PUSH_RIGHT_INC: set(pushButtonR,
getValue(Function.PUSH_RIGHT_INC)); break;
case PUSH_LEFT_INC: set(pushButtonL,
getValue(Function.PUSH_LEFT_INC)); break;
case STOP_ALL_MOTORS: stopAllMotors(); break;
case DRIVE_LEFT: runMotorSet(getMotorSet(frontLeft),
getValue(Function.DRIVE_LEFT)); break;
case DRIVE_RIGHT: runMotorSet(getMotorSet(frontRight),
getValue(Function.DRIVE_RIGHT)); break;
default: break;
}
}
}
}
}
}