-
Notifications
You must be signed in to change notification settings - Fork 0
/
BigMotor.java
59 lines (48 loc) · 1.69 KB
/
BigMotor.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
import com.pi4j.component.adafruithat.AdafruitServo;
import com.pi4j.component.adafruithat.AdafruitServoHat;
public class BigMotor implements Runnable {
private final int Address = 0X40;
private String[] pwms;
private String[] dirs;
private final float minPulse = 0.001f;
private final float nPulse = 1.00f;
private final float maxPulse = 1.999f;
private float[] powers;
private MotorSignal signal;
public BigMotor(String[] pwmPorts, String[] dirPorts, MotorSignal signal) {
pwms = new String[pwmPorts.length];
dirs = new String[dirPorts.length];
for(int i = 0; i < pwmPorts.length; i++) {
pwms[i] = pwmPorts[i];
dirs[i] = dirPorts[i];
}
powers = new float[pwmPorts.length];
this.signal = signal;
}
public void run() {
AdafruitServoHat hat = new AdafruitServoHat(Address);
AdafruitServo[] servos = new AdafruitServo[pwms.length];
AdafruitServo[] dirport = new AdafruitServo[dirs.length];
for(int i = 0; i < pwms.length; i++) {
servos[i] = hat.getServo(pwms[i]);
dirport[i] = hat.getServo(dirs[i]);
servos[i].setOperatingLimits(minPulse, nPulse, maxPulse);
dirport[i].setOperatingLimits(minPulse, nPulse, maxPulse);
servos[i].setPositionRange(0.0f, 100.0f);
}
Runtime.getRuntime().addShutdownHook(new Thread() {
public void run() {
System.out.println("Turn off all motors");
hat.stopAll();
}
});
System.out.println("Motors ready!");
while(!Thread.currentThread().isInterrupted()) {
powers = signal.getPower();
for(int i = 0; i < servos.length; i++) {
servos[i].setPosition(Math.abs(powers[i]));
dirport[i].setPosition(powers[i] < 0 ? 0 : 1);
}
}
}
}