-
Notifications
You must be signed in to change notification settings - Fork 2
/
PushbotAutoDriveByGyro_Linear.java
363 lines (308 loc) · 15.9 KB
/
PushbotAutoDriveByGyro_Linear.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
361
362
363
/* Copyright (c) 2017 FIRST. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without modification,
* are permitted (subject to the limitations in the disclaimer below) provided that
* the following conditions are met:
*
* Redistributions of source code must retain the above copyright notice, this list
* of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright notice, this
* list of conditions and the following disclaimer in the documentation and/or
* other materials provided with the distribution.
*
* Neither the name of FIRST nor the names of its contributors may be used to endorse or
* promote products derived from this software without specific prior written permission.
*
* NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED BY THIS
* LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO,
* THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
* FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
* DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
* SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
* OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
package org.firstinspires.ftc.robotcontroller.external.samples;
import com.qualcomm.hardware.modernrobotics.ModernRoboticsI2cGyro;
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.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
/**
* This file illustrates the concept of driving a path based on Gyro heading and encoder counts.
* It uses the common Pushbot hardware class to define the drive on the robot.
* The code is structured as a LinearOpMode
*
* The code REQUIRES that you DO have encoders on the wheels,
* otherwise you would use: PushbotAutoDriveByTime;
*
* This code ALSO requires that you have a Modern Robotics I2C gyro with the name "gyro"
* otherwise you would use: PushbotAutoDriveByEncoder;
*
* This code requires that the drive Motors have been configured such that a positive
* power command moves them forward, and causes the encoders to count UP.
*
* This code uses the RUN_TO_POSITION mode to enable the Motor controllers to generate the run profile
*
* In order to calibrate the Gyro correctly, the robot must remain stationary during calibration.
* This is performed when the INIT button is pressed on the Driver Station.
* This code assumes that the robot is stationary when the INIT button is pressed.
* If this is not the case, then the INIT should be performed again.
*
* Note: in this example, all angles are referenced to the initial coordinate frame set during the
* the Gyro Calibration process, or whenever the program issues a resetZAxisIntegrator() call on the Gyro.
*
* The angle of movement/rotation is assumed to be a standardized rotation around the robot Z axis,
* which means that a Positive rotation is Counter Clock Wise, looking down on the field.
* This is consistent with the FTC field coordinate conventions set out in the document:
* ftc_app\doc\tutorial\FTC_FieldCoordinateSystemDefinition.pdf
*
* Use Android Studios to Copy this Class, and Paste it into your team's code folder with a new name.
* Remove or comment out the @Disabled line to add this opmode to the Driver Station OpMode list
*/
@Autonomous(name="Pushbot: Auto Drive By Gyro", group="Pushbot")
@Disabled
public class PushbotAutoDriveByGyro_Linear extends LinearOpMode {
/* Declare OpMode members. */
HardwarePushbot robot = new HardwarePushbot(); // Use a Pushbot's hardware
ModernRoboticsI2cGyro gyro = null; // Additional Gyro device
static final double COUNTS_PER_MOTOR_REV = 1440 ; // eg: TETRIX Motor Encoder
static final double DRIVE_GEAR_REDUCTION = 2.0 ; // This is < 1.0 if geared UP
static final double WHEEL_DIAMETER_INCHES = 4.0 ; // For figuring circumference
static final double COUNTS_PER_INCH = (COUNTS_PER_MOTOR_REV * DRIVE_GEAR_REDUCTION) /
(WHEEL_DIAMETER_INCHES * 3.1415);
// These constants define the desired driving/control characteristics
// The can/should be tweaked to suite the specific robot drive train.
static final double DRIVE_SPEED = 0.7; // Nominal speed for better accuracy.
static final double TURN_SPEED = 0.5; // Nominal half speed for better accuracy.
static final double HEADING_THRESHOLD = 1 ; // As tight as we can make it with an integer gyro
static final double P_TURN_COEFF = 0.1; // Larger is more responsive, but also less stable
static final double P_DRIVE_COEFF = 0.15; // Larger is more responsive, but also less stable
@Override
public void runOpMode() {
/*
* Initialize the standard drive system variables.
* The init() method of the hardware class does most of the work here
*/
robot.init(hardwareMap);
gyro = (ModernRoboticsI2cGyro)hardwareMap.gyroSensor.get("gyro");
// Ensure the robot it stationary, then reset the encoders and calibrate the gyro.
robot.leftDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
robot.rightDrive.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
// Send telemetry message to alert driver that we are calibrating;
telemetry.addData(">", "Calibrating Gyro"); //
telemetry.update();
gyro.calibrate();
// make sure the gyro is calibrated before continuing
while (!isStopRequested() && gyro.isCalibrating()) {
sleep(50);
idle();
}
telemetry.addData(">", "Robot Ready."); //
telemetry.update();
robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
// Wait for the game to start (Display Gyro value), and reset gyro before we move..
while (!isStarted()) {
telemetry.addData(">", "Robot Heading = %d", gyro.getIntegratedZValue());
telemetry.update();
}
gyro.resetZAxisIntegrator();
// Step through each leg of the path,
// Note: Reverse movement is obtained by setting a negative distance (not speed)
// Put a hold after each turn
gyroDrive(DRIVE_SPEED, 48.0, 0.0); // Drive FWD 48 inches
gyroTurn( TURN_SPEED, -45.0); // Turn CCW to -45 Degrees
gyroHold( TURN_SPEED, -45.0, 0.5); // Hold -45 Deg heading for a 1/2 second
gyroDrive(DRIVE_SPEED, 12.0, -45.0); // Drive FWD 12 inches at 45 degrees
gyroTurn( TURN_SPEED, 45.0); // Turn CW to 45 Degrees
gyroHold( TURN_SPEED, 45.0, 0.5); // Hold 45 Deg heading for a 1/2 second
gyroTurn( TURN_SPEED, 0.0); // Turn CW to 0 Degrees
gyroHold( TURN_SPEED, 0.0, 1.0); // Hold 0 Deg heading for a 1 second
gyroDrive(DRIVE_SPEED,-48.0, 0.0); // Drive REV 48 inches
telemetry.addData("Path", "Complete");
telemetry.update();
}
/**
* Method to drive on a fixed compass bearing (angle), based on encoder counts.
* Move will stop if either of these conditions occur:
* 1) Move gets to the desired position
* 2) Driver stops the opmode running.
*
* @param speed Target speed for forward motion. Should allow for _/- variance for adjusting heading
* @param distance Distance (in inches) to move from current position. Negative distance means move backwards.
* @param angle Absolute Angle (in Degrees) relative to last gyro reset.
* 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
* If a relative angle is required, add/subtract from current heading.
*/
public void gyroDrive ( double speed,
double distance,
double angle) {
int newLeftTarget;
int newRightTarget;
int moveCounts;
double max;
double error;
double steer;
double leftSpeed;
double rightSpeed;
// Ensure that the opmode is still active
if (opModeIsActive()) {
// Determine new target position, and pass to motor controller
moveCounts = (int)(distance * COUNTS_PER_INCH);
newLeftTarget = robot.leftDrive.getCurrentPosition() + moveCounts;
newRightTarget = robot.rightDrive.getCurrentPosition() + moveCounts;
// Set Target and Turn On RUN_TO_POSITION
robot.leftDrive.setTargetPosition(newLeftTarget);
robot.rightDrive.setTargetPosition(newRightTarget);
robot.leftDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
robot.rightDrive.setMode(DcMotor.RunMode.RUN_TO_POSITION);
// start motion.
speed = Range.clip(Math.abs(speed), 0.0, 1.0);
robot.leftDrive.setPower(speed);
robot.rightDrive.setPower(speed);
// keep looping while we are still active, and BOTH motors are running.
while (opModeIsActive() &&
(robot.leftDrive.isBusy() && robot.rightDrive.isBusy())) {
// adjust relative speed based on heading error.
error = getError(angle);
steer = getSteer(error, P_DRIVE_COEFF);
// if driving in reverse, the motor correction also needs to be reversed
if (distance < 0)
steer *= -1.0;
leftSpeed = speed - steer;
rightSpeed = speed + steer;
// Normalize speeds if either one exceeds +/- 1.0;
max = Math.max(Math.abs(leftSpeed), Math.abs(rightSpeed));
if (max > 1.0)
{
leftSpeed /= max;
rightSpeed /= max;
}
robot.leftDrive.setPower(leftSpeed);
robot.rightDrive.setPower(rightSpeed);
// Display drive status for the driver.
telemetry.addData("Err/St", "%5.1f/%5.1f", error, steer);
telemetry.addData("Target", "%7d:%7d", newLeftTarget, newRightTarget);
telemetry.addData("Actual", "%7d:%7d", robot.leftDrive.getCurrentPosition(),
robot.rightDrive.getCurrentPosition());
telemetry.addData("Speed", "%5.2f:%5.2f", leftSpeed, rightSpeed);
telemetry.update();
}
// Stop all motion;
robot.leftDrive.setPower(0);
robot.rightDrive.setPower(0);
// Turn off RUN_TO_POSITION
robot.leftDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
robot.rightDrive.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
}
/**
* Method to spin on central axis to point in a new direction.
* Move will stop if either of these conditions occur:
* 1) Move gets to the heading (angle)
* 2) Driver stops the opmode running.
*
* @param speed Desired speed of turn.
* @param angle Absolute Angle (in Degrees) relative to last gyro reset.
* 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
* If a relative angle is required, add/subtract from current heading.
*/
public void gyroTurn ( double speed, double angle) {
// keep looping while we are still active, and not on heading.
while (opModeIsActive() && !onHeading(speed, angle, P_TURN_COEFF)) {
// Update telemetry & Allow time for other processes to run.
telemetry.update();
}
}
/**
* Method to obtain & hold a heading for a finite amount of time
* Move will stop once the requested time has elapsed
*
* @param speed Desired speed of turn.
* @param angle Absolute Angle (in Degrees) relative to last gyro reset.
* 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
* If a relative angle is required, add/subtract from current heading.
* @param holdTime Length of time (in seconds) to hold the specified heading.
*/
public void gyroHold( double speed, double angle, double holdTime) {
ElapsedTime holdTimer = new ElapsedTime();
// keep looping while we have time remaining.
holdTimer.reset();
while (opModeIsActive() && (holdTimer.time() < holdTime)) {
// Update telemetry & Allow time for other processes to run.
onHeading(speed, angle, P_TURN_COEFF);
telemetry.update();
}
// Stop all motion;
robot.leftDrive.setPower(0);
robot.rightDrive.setPower(0);
}
/**
* Perform one cycle of closed loop heading control.
*
* @param speed Desired speed of turn.
* @param angle Absolute Angle (in Degrees) relative to last gyro reset.
* 0 = fwd. +ve is CCW from fwd. -ve is CW from forward.
* If a relative angle is required, add/subtract from current heading.
* @param PCoeff Proportional Gain coefficient
* @return
*/
boolean onHeading(double speed, double angle, double PCoeff) {
double error ;
double steer ;
boolean onTarget = false ;
double leftSpeed;
double rightSpeed;
// determine turn power based on +/- error
error = getError(angle);
if (Math.abs(error) <= HEADING_THRESHOLD) {
steer = 0.0;
leftSpeed = 0.0;
rightSpeed = 0.0;
onTarget = true;
}
else {
steer = getSteer(error, PCoeff);
rightSpeed = speed * steer;
leftSpeed = -rightSpeed;
}
// Send desired speeds to motors.
robot.leftDrive.setPower(leftSpeed);
robot.rightDrive.setPower(rightSpeed);
// Display it for the driver.
telemetry.addData("Target", "%5.2f", angle);
telemetry.addData("Err/St", "%5.2f/%5.2f", error, steer);
telemetry.addData("Speed.", "%5.2f:%5.2f", leftSpeed, rightSpeed);
return onTarget;
}
/**
* getError determines the error between the target angle and the robot's current heading
* @param targetAngle Desired angle (relative to global reference established at last Gyro Reset).
* @return error angle: Degrees in the range +/- 180. Centered on the robot's frame of reference
* +ve error means the robot should turn LEFT (CCW) to reduce error.
*/
public double getError(double targetAngle) {
double robotError;
// calculate error in -179 to +180 range (
robotError = targetAngle - gyro.getIntegratedZValue();
while (robotError > 180) robotError -= 360;
while (robotError <= -180) robotError += 360;
return robotError;
}
/**
* returns desired steering force. +/- 1 range. +ve = steer left
* @param error Error angle in robot relative degrees
* @param PCoeff Proportional Gain Coefficient
* @return
*/
public double getSteer(double error, double PCoeff) {
return Range.clip(error * PCoeff, -1, 1);
}
}