-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathtaskSequences.cpp
135 lines (99 loc) · 2.73 KB
/
taskSequences.cpp
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
#include <main.h>
//Constructor: initialize control variables + set up subsystem dependencies
TaskSequences::TaskSequences(Drivetrain drivetrain, Arm arm):
drivetrain(drivetrain),
arm(arm),
substep(0),
subTimeStamp(0){}
//Subsequence for passport task
int TaskSequences::stampPassport(){
//Execute command based on robot substate,
//Advance substate whenever a command is complete
switch(substep){
//Position arm and move in for stamp
case 0:
substep += arm.setPos(120);
break;
case 1:
substep += drivetrain.driveDistance(4);
break;
//Stamp passport
case 2:
substep += arm.reset();
break;
//Move out from passport + reset arm
case 3:
substep += arm.setPos(60);
break;
case 4:
substep += drivetrain.driveBack(12);
break;
case 5:
substep += arm.reset();
break;
//End subsequence
case 6:
//Stop all motors + reset control vars
drivetrain.stop();
arm.stop();
substep = 0;
return 1; //Advance main state
break;
}
return 0; //Maintain main state
}
//Subsequence for fuel lever task
int TaskSequences::fuelLever(int leverNo){
//Execute command based on robot substate,
//Advance substate whenever a command is complete
switch(substep){
//Flip lever down
case 0:
substep += arm.reset();
break;
case 1:
substep += arm.setPos(140);
break;
//Drive back + position for flip up
case 2:
substep += arm.reset();
break;
case 3:
substep += drivetrain.driveDistance(3);
break;
case 4:
substep += arm.setPos(110);
break;
//Wait for bonus pts
case 5:
Sleep(5.0);
substep++;
break;
//Flip lever up
case 6:
substep += drivetrain.driveBack(3.5);
break;
case 7:
substep += arm.setPos(0);
break;
case 8:
substep += arm.setPos(30);
break;
//Align with wall near levers + reset arm position
case 9:
substep += drivetrain.align();
break;
case 10:
substep += arm.reset();
break;
//End subsequence
case 11:
//Stop all motors + reset control vars
drivetrain.stop();
arm.stop();
substep = 0;
return 1; //Advance main state
break;
}
return 0; //Maintain main state
}