-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathplane.pde
148 lines (138 loc) · 3.34 KB
/
plane.pde
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
136
137
138
139
140
141
142
143
144
145
146
147
148
void inline setCurPlane()
{
/*
while(g_robotState.currPlane < PLANE_COUNT && g_robotState.planes[g_robotState.currPlane].len <= 0)
{
g_robotState.currPlane++;
}
*/
if (g_robotState.planes[g_robotState.currPlane].type == CONDITION)
{
switch(g_robotState.planes[g_robotState.currPlane].len)
{
case COND_GO_TO:
g_robotState.currPlane = g_robotState.planes[g_robotState.currPlane].speed;
break;
default:
g_robotState.currPlane++;
}
}
else
g_robotState.currPlane++;
if (g_robotState.currPlane < PLANE_COUNT)
{
g_robotState.currLen = 0;
// stopEngines();
setPlaneOutput();
}
else
{
shutDown();
// SerialUSB.println("END!!!!");
}
toggleLED();
g_robotState.leftEncoder = g_robotState.rightEncoder = 0;
}
bool checkComplete()
{
if (g_robotState.currPlane >= PLANE_COUNT)
return true;
if (g_robotState.currLen >= g_robotState.planes[g_robotState.currPlane].len)
{
// stopEngines();
setCurPlane();
return true;
}
return false;
}
void updatePlane()
{
if (g_robotState.planes[g_robotState.currPlane].type != BACK_COLLISION)
{
g_robotState.leftBackPressed = g_robotState.rightBackPressed = 0;
}
switch(g_robotState.planes[g_robotState.currPlane].type)
{
case MOVE_FORWARD:
case MOVE_BACKWARD:
if (g_robotState.leftEncoder > g_robotState.rightEncoder)
g_robotState.currLen += g_robotState.leftEncoder;
else
g_robotState.currLen += g_robotState.rightEncoder;
break;
case TURN_LEFT:
g_robotState.currLen += g_robotState.rightEncoder;
break;
case TURN_RIGHT:
g_robotState.currLen += g_robotState.leftEncoder;
break;
case WAIT:
g_robotState.currLen = millis() - g_robotState.startPlaneTime;
break;
case OPEN_LEFT_DOOR:
case OPEN_RIGHT_DOOR:
case CLOSE_LEFT_DOOR:
case CLOSE_RIGHT_DOOR:
case HALF_LEFT_DOOR:
case HALF_RIGHT_DOOR:
case CONDITION:
g_robotState.currLen ++;
break;
case BACK_COLLISION:
if (g_robotState.rightBackPressed && g_robotState.leftBackPressed)
{
g_robotState.rightBackPressed = g_robotState.leftBackPressed = 0;
g_robotState.currLen ++;
}
break;
// default:
// g_robotState.currLen++;
}
}
void setPlaneOutput()
{
g_robotState.startPlaneTime = millis();
switch(g_robotState.planes[g_robotState.currPlane].type)
{
case WAIT:
// g_robotState.startPlaneTime = millis();
break;
case MOVE_FORWARD:
leftEngineForward();
rightEngineForward();
break;
case MOVE_BACKWARD:
case BACK_COLLISION:
leftEngineBackward();
rightEngineBackward();
break;
case TURN_LEFT:
// leftEngineStop();
leftEngineBackward();
rightEngineForward();
break;
case TURN_RIGHT:
// rightEngineStop();
rightEngineBackward();
leftEngineForward();
break;
case OPEN_LEFT_DOOR:
openLeftDoor();
break;
case OPEN_RIGHT_DOOR:
openRightDoor();
break;
case CLOSE_LEFT_DOOR:
closeLeftDoor();
break;
case CLOSE_RIGHT_DOOR:
closeRightDoor();
break;
case HALF_LEFT_DOOR:
halfOpenLeftDoor();
break;
case HALF_RIGHT_DOOR:
halfOpenRightDoor();
break;
}
}