-
Notifications
You must be signed in to change notification settings - Fork 0
/
TeleOp
233 lines (197 loc) · 8.95 KB
/
TeleOp
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
package org.firstinspires.ftc.teamcode;
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.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import java.text.SimpleDateFormat;
import java.util.Date;
/**
* This file contains an example of an iterative (Non-Linear) "OpMode".
* An OpMode is a 'program' that runs in either the autonomous or the teleop period of an FTC match.
* The names of OpModes appear on the menu of the FTC Driver Station.
* When an selection is made from the menu, the corresponding OpMode
* class is instantiated on the Robot Controller and executed.
*
* Remove a @Disabled the on the next line or two (if present) to add this opmode to the Driver Station OpMode list,
* or add a @Disabled annotation to prevent this OpMode from being added to the Driver Station
*/
@TeleOp(name="TeleOp", group="Linear Opmode")
public class Test2 extends OpMode {
/* Declare OpMode members. */
HardwareBase robot = new HardwareBase();
private ElapsedTime runtime = new ElapsedTime();
static final double INCREMENT = 0.01; // amount to slew servo each CYCLE_MS cycle
static final double HEIGHT_INCREMENT = 500;
static final double LIFT_SPEED = 1.0;
static final double OPEN = 0.8; // Maximum rotational position
static final double CLOSE = 0.6; // Minimum rotational position
static final double UNHOOK = 1.0; // Maximum rotational position
static final double HOOK = 0.0; // Minimum rotational position
double position2 = (UNHOOK - HOOK) / 2; // Start at halfway position
double position3 = (UNHOOK - HOOK) / 2;
double position = (OPEN + CLOSE) / 2; // Start at halfway position
boolean x2Pressed = false;
boolean x2Held = false;
boolean y2Pressed = false;
boolean y2Held = false;
@Override
public void init() {
telemetry.addData("Status", "Initialized");
robot.init(hardwareMap);
robot.acq2.setMode(DcMotor.RunMode.RESET_ENCODERS);
//robot.acq1.setMode(DcMotor.RunMode.RESET_ENCODERS);
telemetry.addData("Status", "Initialized");
}
/*
* Code to run REPEATEDLY after the driver hits INIT, but before they hit PLAY
*/
@Override
public void init_loop() {
}
/*
* Code to run ONCE when the driver hits PLAY
*/
@Override
public void start() {
runtime.reset();
}
/*
* Code to run REPEATEDLY after the driver hits PLAY but before they hit STOP
*/
@Override
public void loop() {
///////////////////////////////////////////////////////////////////
//// ////
//// Drive Code ////
//// ////
///////////////////////////////////////////////////////////////////
double y = gamepad1.left_stick_y;
double x = gamepad1.left_stick_x;
double z = 0;
// I wouldn't compare against strictly 0, a lot of times the joysticks have small non-zero values
// when you aren't pressing them. Use some small value and abs. So: if(Math.abs(gamepad1.left_stick_x) > 0.05)
// RCVS
if(Math.abs(gamepad1.left_stick_x) > 0.002)
z = (y/ x);
//Setup a variable for each drive wheel to save power level for telemetry
if( z != 0)
{
robot.right_drive.setPower(scale((y-x)));
robot.right_drive1.setPower(scale((y+x)));
robot.left_drive.setPower(scale((y+x)));
robot.left_drive1.setPower(scale((y-x)));
}
robot.right_drive.setPower(scale((y-x-z)));
robot.right_drive1.setPower(scale((y+x-z)));
robot.left_drive.setPower(scale((y+x+z)));
robot.left_drive1.setPower(scale((y-x+z)));
///////////////////////////////////////////////////////////////////
//// ////
//// Acquisition System program ////
//// ////
///////////////////////////////////////////////////////////////////
x2Pressed = gamepad2.x;
// If x2 was pressed, but not held
if(x2Pressed && !x2Held) {
x2Held = true;
int newHeight = robot.acq2.getCurrentPosition();
newHeight += HEIGHT_INCREMENT;
robot.acq2.setTargetPosition(newHeight);
robot.acq2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq2.setPower(LIFT_SPEED);
} else if(!x2Pressed) {
// This happens if the button is not being pressed at all, which resets that
// we are holding it so the next time it is pressed it will trigger the action
// again.
x2Held = false;
}
y2Pressed = gamepad2.y;
if(y2Pressed && !y2Held) {
int newHeight = robot.acq2.getCurrentPosition();
if( newHeight > 5 ) {
newHeight -= HEIGHT_INCREMENT;
robot.acq2.setTargetPosition(newHeight);
robot.acq2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq2.setPower(LIFT_SPEED);
}
else if(newHeight <= 5) {
robot.acq2.setTargetPosition(newHeight);
robot.acq2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq2.setPower(0);
}
} else if(!y2Pressed) {
y2Held = false;
}
telemetry.addData("Current Position of Rotation", + robot.acq2.getCurrentPosition());
boolean turn = false;
robot.acq1.setPower(gamepad2.left_stick_x);
if(robot.acq2.getCurrentPosition() > 2000 ) {
turn = !turn;
}
if( Math.abs(gamepad2.left_stick_x) > 0.02 && turn) {
if(gamepad2.left_stick_x > 0.02){
robot.acq1.setTargetPosition(2000);
robot.acq1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq1.setPower(1);
} else if(gamepad2.left_stick_x < 0.02){
robot.acq1.setTargetPosition(100);
robot.acq1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq1.setPower(1);
} else if( Math.abs(gamepad2.left_stick_x) > 0.02 && !turn )
robot.acq1.setTargetPosition(100);
robot.acq1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.acq1.setPower(1);
}
telemetry.addData("Current Position of Extension", + robot.acq1.getCurrentPosition() );
telemetry.update();
// Open the acquistion system
if(gamepad2.left_bumper ) {
// Keep stepping up until we hit the max value.
position += INCREMENT ;
if (position >= OPEN ) {
position = OPEN;
}
}
// secure the block
else if(gamepad2.right_bumper ){
// Keep stepping down until we hit the min value.
position -= INCREMENT ;
if (position <= CLOSE ) {
position = CLOSE;
}
}
robot.arm.setPosition(position);
// ///////////////////////////////////////////////////////////////////
// //// ////
// //// Foundation Hooks program ////
// //// ////
// ///////////////////////////////////////////////////////////////////
// Open the acquistion system
if (gamepad2.b) {
position2 = UNHOOK;
position3 = HOOK;
}
// secure the block
else if(gamepad2.a ){
position2 = 0.5;
position3 = 0.5;
}
robot.hook1.setPosition(position2);
robot.hook2.setPosition(position3);
}
public double scale(double n)
{
return n;
}
public void limitforExtension( double power )
{
}
/*
* Code to run ONCE after the driver hits STOP
*/
@Override
public void stop() {
}
}