-
Notifications
You must be signed in to change notification settings - Fork 78
/
JSTEncoder.java
76 lines (64 loc) · 1.87 KB
/
JSTEncoder.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
package com.arcrobotics.ftclib.hardware;
import com.qualcomm.robotcore.hardware.DcMotor;
import com.qualcomm.robotcore.hardware.DcMotorEx;
import com.qualcomm.robotcore.hardware.HardwareMap;
public class JSTEncoder extends ExternalEncoder {
private DcMotorEx encoder;
/**
* counts: Current encoder counts
* offset: Offset encoder to "sync"
* dpp: Distance per pulse
*/
int counts, offset, multiplier;
double dpp;
public JSTEncoder(HardwareMap hw, String encoderName) {
encoder = (DcMotorEx) hw.get(DcMotor.class, encoderName);
encoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
encoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
counts = 0;
offset = 0;
multiplier = 1;
dpp = 0;
}
/**
* How much distance is covered in one tick of an encoder.
* For Drive: dpp = (WHEEL_DIAMETER*Math.PI) / cpr
* @param dpp
*/
public void setDistancePerPulse(double dpp) {
this.dpp = dpp;
}
public double getDistance() {
return dpp * getCounts();
}
public void setInverted(boolean isInverted) {
if(isInverted)
multiplier = -1;
else
multiplier = 1;
}
public boolean getInverted() {
if(multiplier == -1)
return true;
else
return false;
}
@Override
public long getCounts() {
counts = (encoder.getCurrentPosition() + offset) * multiplier;
return counts;
}
@Override
public void syncEncoder() {
offset = (int) -getCounts();
}
@Override
public void resetEncoder() {
encoder.setMode(DcMotor.RunMode.STOP_AND_RESET_ENCODER);
offset = 0;
encoder.setMode(DcMotor.RunMode.RUN_WITHOUT_ENCODER);
}
public double getRate() {
return encoder.getVelocity() * dpp * multiplier;
}
}