-
Notifications
You must be signed in to change notification settings - Fork 1
/
AutonomousManager.java
93 lines (82 loc) · 2.2 KB
/
AutonomousManager.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
package autonomous;
import java.util.ArrayList;
import org.usfirst.frc.team5199.robot.Robot;
import arm.Arm;
import climber.Climber;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import elevator.Elevator;
import gripper.Gripper;
import interfaces.AutFunction;
import util.ClockRegulator;
import drive.DriveBase;
public class AutonomousManager {
private final ArrayList<ArrayList<AutFunction>> functions;
private final ClockRegulator clockRegulator;
private final ArrayList<Boolean> functionsDone;
private int step;
private boolean done;
private boolean moveOn;
public AutonomousManager(ClockRegulator clockRegulator) {
this.clockRegulator = clockRegulator;
functionsDone = new ArrayList<Boolean>();
functions = new ArrayList<ArrayList<AutFunction>>();
step = 0;
}
public void init() {
for (int i = 0; i < functions.size(); i++) {
functionsDone.add(false);
if (functions.get(i).get(0) != null) {
functions.get(i).get(0).init();
}
}
done = false;
moveOn = true;
}
public void update() {
if (step < functions.get(0).size()) {
for (int i = 0; i < functions.size(); i++) {
if (step < functions.get(i).size()) {
if (functions.get(i).get(step) != null) {
functions.get(i).get(step).update(clockRegulator.getMsPerUpdate());
if (functions.get(i).get(step).isDone()) {
functions.get(i).get(step).cleanUp();
Robot.nBroadcaster.println(functions.get(step).getClass().getName() + " end\t");
functionsDone.set(i, true);
}
} else {
functionsDone.set(i, true);
}
}
}
moveOn = true;
for (int i = 0; i < functionsDone.size(); i++) {
if (!functionsDone.get(i)) {
moveOn = false;
}
}
if (moveOn) {
step++;
for (int i = 0; i < functions.size(); i++) {
functionsDone.add(false);
if (functions.get(i).get(step) != null) {
if(step < functions.get(0).size()) {
functions.get(i).get(step).init();
}
}
}
}
} else {
if (!done) {
Robot.nBroadcaster.println("AutonomousManager finished");
done = true;
}
}
clockRegulator.sync();
}
public void add(ArrayList<AutFunction> f) {
functions.add(f);
}
public boolean isDone() {
return done;
}
}