-
Notifications
You must be signed in to change notification settings - Fork 0
Expand file tree
/
Copy pathEncoderTest.java
More file actions
102 lines (83 loc) · 3.12 KB
/
EncoderTest.java
File metadata and controls
102 lines (83 loc) · 3.12 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
package org.firstinspires.ftc.teamcode;
import com.qualcomm.robotcore.eventloop.opmode.Disabled;
import com.qualcomm.robotcore.eventloop.opmode.OpMode;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.eventloop.opmode.TeleOp;
import com.qualcomm.robotcore.hardware.DcMotorEx;
@TeleOp(name = "Encoder Test", group = "SkeletonCode") //Establishes name and group
//@Disabled //Stops this from appearing, just comment it out to include
public class EncoderTest extends OpMode{
DcMotor M1;
DcMotor M2;
int pos1, pos2;
int change = 50;
double yStartTime = 0;
double aStartTime = 0;
double upStartTime = 0;
double downStartTime = 0;
//Values for button cooldowns
public void init() {
M1 = hardwareMap.dcMotor.get("Motor 1");
M2 = hardwareMap.dcMotor.get("Motor 2");
M1.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
M2.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
}
public void loop() {
if(gamepad1.y && yCooldown()) {
pos1 = M1.getCurrentPosition();
pos1 = (int) (pos1 + change);
M1.setTargetPosition(pos1);
M1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if(gamepad1.a && aCooldown()) {
pos1 = M1.getCurrentPosition();
pos1 = (int) (pos1 - change);
M1.setTargetPosition(pos1);
M1.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if(gamepad1.dpad_up && upCooldown()) {
pos2 = M2.getCurrentPosition();
pos2 = (int) (pos2 + change);
M2.setTargetPosition(pos2);
M2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
if(gamepad1.dpad_down && downCooldown()) {
pos2 = M2.getCurrentPosition();
pos2 = (int) (pos2 - change);
M2.setTargetPosition(pos2);
M2.setMode(DcMotor.RunMode.RUN_TO_POSITION);
}
telemetry.addData("Motor 1:", pos1);
telemetry.addData("Motor 2:", pos2);
telemetry.update();
}
//Input Cooldowns
public boolean yCooldown() {
if(getRuntime() - yStartTime > .25) { //Must wait 250 milliseconds before input can be used again
yStartTime = getRuntime();
return true;
}
return false;
}
public boolean aCooldown() {
if(getRuntime() - aStartTime > .25) { //Must wait 250 milliseconds before input can be used again
aStartTime = getRuntime();
return true;
}
return false;
}
public boolean upCooldown() {
if(getRuntime() - upStartTime > .25) { //Must wait 250 milliseconds before input can be used again
upStartTime = getRuntime();
return true;
}
return false;
}
public boolean downCooldown() {
if(getRuntime() - downStartTime > .25) { //Must wait 250 milliseconds before input can be used again
downStartTime = getRuntime();
return true;
}
return false;
}
}