-
Notifications
You must be signed in to change notification settings - Fork 2
/
DashboardEnhancedTeleOp.java
318 lines (270 loc) · 14.3 KB
/
DashboardEnhancedTeleOp.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
package org.firstinspires.ftc.teamcode;
import com.acmerobotics.dashboard.FtcDashboard;
import com.acmerobotics.dashboard.config.Config;
import com.acmerobotics.dashboard.telemetry.MultipleTelemetry;
import com.qualcomm.hardware.lynx.LynxModule;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.DigitalChannel;
import com.qualcomm.robotcore.hardware.Gamepad;
import com.qualcomm.robotcore.hardware.PIDFCoefficients;
import com.qualcomm.robotcore.util.ElapsedTime;
import com.qualcomm.robotcore.util.Range;
import org.firstinspires.ftc.robotcore.external.Telemetry;
import org.firstinspires.ftc.teamcode.util.ProfileTrapezoidal;
import org.firstinspires.ftc.teamcode.util.SmoothDelay;
import java.util.List;
@Config
@TeleOp(name="Dashboard Enhanced TeleOp")
@Disabled
public class DashboardEnhancedTeleOp extends OpMode {
private FtcDashboard dashboard;
private MultipleTelemetry telemetry;
private final ElapsedTime runtime = new ElapsedTime();
private DcMotorEx rearRightDrive = null;
private DcMotorEx rearLeftDrive = null;
private DcMotorEx frontRightDrive = null;
private DcMotorEx frontLeftDrive = null;
private final DcMotorEx[] driveMotors = new DcMotorEx[4];
private static final int MAX_VELOCITY_TPS = 2400;
private DcMotorEx armLift;
private DcMotor clawLeft;
private DcMotor clawRight;
private DigitalChannel armLimit;
private boolean lastArmLimitState;
private boolean armLimitState;
private double armTargetRaw;
private Manipulator.ArmPosition lastArmPosition = Manipulator.ArmPosition.UNKNOWN;
private Manipulator.ArmPosition armPosition = Manipulator.ArmPosition.UNKNOWN;
private ProfileTrapezoidal trap;
private ElapsedTime dt = new ElapsedTime(ElapsedTime.Resolution.MILLISECONDS);
// disable config values by setting them to private, enable by setting to public
public static boolean APPLY = false;
public static double RL_VELOCITY = 50;
public static double RR_VELOCITY = 50;
public static double FL_VELOCITY = 50;
public static double FR_VELOCITY = 50;
public static double DRIVE_VELOCITY_P = 10;
public static double DRIVE_VELOCITY_I = 3;
public static double DRIVE_VELOCITY_D = 0;
public static double DRIVE_VELOCITY_F = 0;
private static double ARM_VELOCITY_P = 20;
private static double ARM_VELOCITY_I = 3;
private static double ARM_VELOCITY_D = 2;
private static double ARM_VELOCITY_F = 0.00029574;
private static double ARM_POSITION_P = 10;
private static double PROFILE_SPEED = 2000;
private static double PROFILE_ACCEL = 4000;
private static int LEFT_STICK_DELAY_STEPS = 1;
private static int RIGHT_STICK_DELAY_STEPS = 1;
private static double DEFAULT_SPEED = 0.6;
private static double FAST_MODE_SPEED = 0.85;
private SmoothDelay leftStickSmoothDelay = new SmoothDelay(LEFT_STICK_DELAY_STEPS);
private SmoothDelay rightStickSmoothDelay = new SmoothDelay(RIGHT_STICK_DELAY_STEPS);
@Override
public void init() {
dashboard = FtcDashboard.getInstance();
telemetry = new MultipleTelemetry(super.telemetry, dashboard.getTelemetry());
telemetry.addData("Status", "Initializing");
telemetry.update();
List<LynxModule> allHubs = hardwareMap.getAll(LynxModule.class);
for (LynxModule hub : allHubs) {
hub.setBulkCachingMode(LynxModule.BulkCachingMode.AUTO);
}
// Initialize the hardware variables. Note that the strings used here as parameters
// to 'get' must correspond to the names assigned during the robot configuration
// step (using the FTC Robot Controller app on the phone).
// get DcMotorEx or get DcMotor and cast to DcMotorEx
driveMotors[0] = rearRightDrive = hardwareMap.get(DcMotorEx.class, "rear_right_drive");
driveMotors[1] = rearLeftDrive = hardwareMap.get(DcMotorEx.class, "rear_left_drive");
driveMotors[2] = frontRightDrive = hardwareMap.get(DcMotorEx.class, "front_right_drive");
driveMotors[3] = frontLeftDrive = hardwareMap.get(DcMotorEx.class, "front_left_drive");
armLift = hardwareMap.get(DcMotorEx.class, "arm_lift");
armLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
armLift.setDirection(DcMotor.Direction.FORWARD);
armLift.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
armLimit = hardwareMap.get(DigitalChannel.class, "arm_limit");
armLimit.setMode(DigitalChannel.Mode.INPUT);
// TODO tuning! leaving these commented to start since Caleb reports the setpoint *is* being reached; the defaults are ok?
// need to set both velocity and position coefficients since the position controller just sets velocity goals
//armLift.setVelocityPIDFCoefficients(1.0, 0.1, 0.1, 0.1) // p, i, d, f
//armLift.setPositionPIDFCoefficients(1.0) // p
// max speed - 1800, max acceleration 3600
// testing - 200, 1000
// some shuddering on descent, not very smooth; going up is fine
trap = new ProfileTrapezoidal(PROFILE_SPEED, PROFILE_ACCEL); // cruise speed, acceleration TODO adjust these
dt.reset();
clawLeft = hardwareMap.get(DcMotor.class, "claw_left");
clawRight = hardwareMap.get(DcMotor.class, "claw_right");
clawLeft.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
clawRight.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
clawRight.setDirection(DcMotor.Direction.REVERSE);
for (DcMotorEx motor : driveMotors) {
motor.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
motor.setZeroPowerBehavior(DcMotor.ZeroPowerBehavior.BRAKE);
motor.setMode(DcMotor.RunMode.RUN_USING_ENCODER);
}
// Most robots need the motor on one side to be reversed to drive forward
// Reverse the motor that runs backwards when connected directly to the battery
rearRightDrive.setDirection(DcMotor.Direction.REVERSE);
rearLeftDrive.setDirection(DcMotor.Direction.FORWARD);
frontRightDrive.setDirection(DcMotor.Direction.REVERSE);
frontLeftDrive.setDirection(DcMotor.Direction.FORWARD);
// Tell the driver that initialization is complete.
telemetry.addData("Status", "Initialized");
telemetry.log().setDisplayOrder(Telemetry.Log.DisplayOrder.OLDEST_FIRST);
telemetry.log().add("Edit config values at http://192.168.43.1:8080/dash");
telemetry.log().add("Don't forget to press apply on the dashboard (or press right bumper)");
telemetry.log().add("You can also graph telemetry data");
telemetry.update();
}
@Override
public void start() {
telemetry.log().clear();
runtime.reset();
}
@Override
public void loop() {
if (APPLY || gamepad1.right_bumper) {
// set arm PID gains
armLift.setVelocityPIDFCoefficients(ARM_VELOCITY_P, ARM_VELOCITY_I, ARM_VELOCITY_D, ARM_VELOCITY_F); // stability limit is p = 40; reduce p or apply damping
PIDFCoefficients armVelocityGains = armLift.getPIDFCoefficients(DcMotor.RunMode.RUN_USING_ENCODER);
telemetry.addData("arm velocity", "p (%.2f), i (%.2f), d (%.2f), f (%.2f)", armVelocityGains.p, armVelocityGains.i, armVelocityGains.d, armVelocityGains.f) ;
armLift.setPositionPIDFCoefficients(ARM_POSITION_P);
PIDFCoefficients armPositionGains = armLift.getPIDFCoefficients(DcMotor.RunMode.RUN_TO_POSITION);
telemetry.addData("arm position", "p (%.2f), i (%.2f), d (%.2f), f (%.2f)", armPositionGains.p, armPositionGains.i, armPositionGains.d, armPositionGains.f) ;
// set drive PID gains
rearLeftDrive.setVelocityPIDFCoefficients(DRIVE_VELOCITY_P, DRIVE_VELOCITY_I, DRIVE_VELOCITY_D, DRIVE_VELOCITY_F);
rearRightDrive.setVelocityPIDFCoefficients(DRIVE_VELOCITY_P, DRIVE_VELOCITY_I, DRIVE_VELOCITY_D, DRIVE_VELOCITY_F);
frontLeftDrive.setVelocityPIDFCoefficients(DRIVE_VELOCITY_P, DRIVE_VELOCITY_I, DRIVE_VELOCITY_D, DRIVE_VELOCITY_F);
frontRightDrive.setVelocityPIDFCoefficients(DRIVE_VELOCITY_P, DRIVE_VELOCITY_I, DRIVE_VELOCITY_D, DRIVE_VELOCITY_F);
telemetry.addData("drive velocity","p (%.2f), i (%.2f), d (%.2f), f (%.2f)", DRIVE_VELOCITY_P, DRIVE_VELOCITY_I, DRIVE_VELOCITY_D, DRIVE_VELOCITY_F);
trap = new ProfileTrapezoidal(PROFILE_SPEED, PROFILE_ACCEL);
leftStickSmoothDelay = new SmoothDelay(LEFT_STICK_DELAY_STEPS);
rightStickSmoothDelay = new SmoothDelay(RIGHT_STICK_DELAY_STEPS);
telemetry.log().add("Changes applied");
APPLY = false;
}
int armEncoder = armLift.getCurrentPosition();
armLimitState = armLimit.getState();
double fastMode = gamepad1.left_stick_button ? FAST_MODE_SPEED : DEFAULT_SPEED;
if (fastMode > DEFAULT_SPEED) { // move arm up if fast mode is on
armPosition = Manipulator.ArmPosition.BOTTOM;
}
// POV Mode uses left stick to go forward, and right stick to turn.
// apply smooth delay
double leftStickY = leftStickSmoothDelay.profileSmoothDelaySmooth(gamepad1.left_stick_y);
double rightStickX = rightStickSmoothDelay.profileSmoothDelaySmooth(gamepad1.right_stick_x);
leftStickY = 0.2;
rightStickX = 0;
// calculate drive and turn
double drive = -leftStickY*Math.abs(leftStickY);
double turn = rightStickX*Math.abs(rightStickX);
// max speed is 165 rpm according to TetrixMotor.java. velocity is in rpm
double leftVelocity = Range.scale(Range.clip(drive + turn, -1, 1), -1, 1, -MAX_VELOCITY_TPS, MAX_VELOCITY_TPS);
double rightVelocity = Range.scale(Range.clip(drive - turn, -1, 1), -1, 1, -MAX_VELOCITY_TPS, MAX_VELOCITY_TPS);
// Calculate fast mode
leftVelocity *= fastMode;
rightVelocity *= fastMode;
// Send power to motors
rearLeftDrive.setVelocity(leftVelocity);
frontLeftDrive.setVelocity(leftVelocity);
rearRightDrive.setVelocity(rightVelocity);
frontRightDrive.setVelocity(rightVelocity);
// Show the elapsed game time and wheel power.
telemetry.addData("Status", "Run Time: " + runtime);
telemetry.addData("RL desired", RL_VELOCITY);
telemetry.addData("RR desired", RR_VELOCITY);
telemetry.addData("FL desired", FL_VELOCITY);
telemetry.addData("FR desired", FR_VELOCITY);
// telemetry.addData("L desired", leftVelocity);
// telemetry.addData("R desired", rightVelocity);
telemetry.addData("RL", rearLeftDrive.getVelocity());
telemetry.addData("RR", rearRightDrive.getVelocity());
telemetry.addData("FL", frontLeftDrive.getVelocity());
telemetry.addData("FR", frontRightDrive.getVelocity());
telemetry.addData("arm pos", armEncoder);
// both triggers = duck mode
if (gamepad1.right_trigger > 0.00 && gamepad1.left_trigger > 0.00) {
clawLeft.setPower(gamepad1.left_trigger);
clawRight.setPower(-gamepad1.right_trigger);
armPosition = Manipulator.ArmPosition.DUCK; // handle later
} else if (gamepad1.right_trigger > 0.0){ // right is out, positive is out
clawLeft.setPower(gamepad1.right_trigger);
clawRight.setPower(gamepad1.right_trigger);
} else if (gamepad1.left_trigger > 0.0) { // left is in, negative is in
clawLeft.setPower(-gamepad1.left_trigger);
clawRight.setPower(-gamepad1.left_trigger);
} else { // halt motors if no buttons are pressed
clawLeft.setPower(0);
clawRight.setPower(0);
}
char button = getGamepadButtons(gamepad1);
telemetry.addData("button", button);
switch (button) {
case 'a':
armPosition = Manipulator.ArmPosition.GROUND;
armTargetRaw = armPosition.encoderTicks;
break;
case 'b':
armPosition = Manipulator.ArmPosition.BOTTOM;
armTargetRaw = armPosition.encoderTicks;
break;
case 'y':
armPosition = Manipulator.ArmPosition.MIDDLE_TELEOP;
armTargetRaw = armPosition.encoderTicks;
break;
case 'x':
armPosition = Manipulator.ArmPosition.TOP;
armTargetRaw = armPosition.encoderTicks;
break;
case 'd':
armTargetRaw += 10;
break;
case 'u':
armTargetRaw -= 10;
break;
}
if (armPosition == Manipulator.ArmPosition.DUCK && lastArmPosition != Manipulator.ArmPosition.DUCK) { // transition to duck mode
armTargetRaw = armPosition.encoderTicks;
}
// profile and move motor
double armTargetSmooth = trap.smooth(armTargetRaw, dt.time()/1000.0);
telemetry.addData("armTargetRaw", (int) armTargetRaw);
telemetry.addData("armTargetSmooth", armTargetSmooth);
telemetry.addData("arm state", armPosition);
telemetry.addData("dt", (int) dt.time());
// don't send commands to motor if we are resetting encoder
if (armLimitState && !lastArmLimitState) { // when switch not on and last state is on
armLift.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
} else {
armLift.setTargetPosition((int) armTargetSmooth);
armLift.setPower(1);
armLift.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
lastArmPosition = armPosition;
lastArmLimitState = armLimitState;
dashboard.updateConfig();
telemetry.update();
dt.reset();
}
private char getGamepadButtons(Gamepad gamepad) {
if (gamepad.a) {
return 'a';
} else if (gamepad.b) {
return 'b';
} else if (gamepad.x) {
return 'x';
} else if (gamepad.y) {
return 'y';
} else if (gamepad.dpad_down) {
return 'd';
} else if (gamepad.dpad_up) {
return 'u';
} else
return '\u0000';
}
}