-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathsudocode.py
165 lines (126 loc) · 5.5 KB
/
sudocode.py
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
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
leftSensor = Sensor()
rightSensor = Sensor()
leftSensorPrevColor = leftSensor.getColor()
rightSensorPrevColor = leftSensor.getColor()
lastTransition = 0
lastTransitionSensor = None
# max time in ms since last transition to consider sensors on same height, so that vehicle is driving straight to center
transitionThreshold = 100
loopDelay = 50 # time in ms to wait before race logic loop continues
# marks a turn around action, when driving away from center was detected
changesDirection = False
def main():
while True:
delay(50)
# when vehicle is turning around at the moment, do not trigger other events
if changesDirection:
continue
# right sensor detection
# vehicle potentially driving to central axis
if (((rightSensor.getColor() == "black" and rightSensorPrevColor == "white")
or (rightSensor.getColor() == "grey" and rightSensorPrevColor == "black")
or (rightSensor.getColor == "white" and rightSensorPrevColor == "grey"))
and leftSensor.getColor() == rightSensorPrevColor):
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != rightSensor):
lastTransitionSensor = rightSensor
lastTransition = time.time()
continue
lastTransition = time.time()
turnRight()
if ((rightSensor.getColor() == "black" and rightSensorPrevColor == "white")
or (rightSensor.getColor() == "grey" and rightSensorPrevColor == "black")
or (rightSensor.getColor == "white" and rightSensorPrevColor == "grey"))
and leftSensor.getColor() == rightSensor.getColor():
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != rightSensor):
lastTransitionSensor = rightSensor
lastTransition = time.time()
continue
lastTransition = time.time()
turnLeft()
# left sensor detection
# vehicle potentially driving to central axis
if ((leftSensor.getColor() == "black" and leftSensorPrevColor == "white")
or (leftSensor.getColor() == "grey" and leftSensorPrevColor == "black")
or (leftSensor.getColor == "white" and leftSensorPrevColor == "grey"))
and rightSensor.getColor() == leftSensorPrevColor:
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != leftSensor):
lastTransitionSensor = leftSensor
lastTransition = time.time()
continue
lastTransition = time.time()
trunLeft()
if ((leftSensor.getColor() == "black" and leftSensorPrevColor == "white")
or (leftSensor.getColor() == "grey" and leftSensorPrevColor == "black")
or (leftSensor.getColor == "white" and leftSensorPrevColor == "grey"))
and rightSensor.getColor() == leftSensor.getColor():
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != leftSensor):
lastTransitionSensor = leftSensor
lastTransition = time.time()
continue
lastTransition = time.time()
turnRight()
# right sensor detection
# vehicle potentially driving away from center
if ((rightSensor.getColor() == "grey" and rightSensorPrevColor == "white")
or (rightSensor.getColor() == "black" and rightSensorPrevColor == "grey")
or (rightSensor.getColor == "white" and rightSensorPrevColor == "black"))
and leftSensor.getColor() == rightSensorPrevColor:
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != rightSensor):
lastTransitionSensor = rightSensor
lastTransition = time.time()
turn180()
changesDirection = True
continue
changesDirection = True
lastTransition = time.time()
turnStrongerLeft()
if ((rightSensor.getColor() == "grey" and rightSensorPrevColor == "white")
or (rightSensor.getColor() == "black" and rightSensorPrevColor == "grey")
or (rightSensor.getColor == "white" and rightSensorPrevColor == "black"))
and leftSensor.getColor() == rightSensor.getColor():
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != rightSensor):
lastTransitionSensor = rightSensor
lastTransition = time.time()
turn180()
changesDirection = True
continue
changesDirection = True
lastTransition = time.time()
turnStrongerRight()
# left sensor detection
# vehicle potentially driving away from center
if ((leftSensor.getColor() == "grey" and leftSensorPrevColor == "white")
or (leftSensor.getColor() == "black" and leftSensorPrevColor == "grey")
or (leftSensor.getColor == "white" and leftSensorPrevColor == "black"))
and rightSensor.getColor() == leftSensorPrevColor:
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != leftSensor):
lastTransitionSensor = leftSensor
lastTransition = time.time()
turn180()
changesDirection = True
continue
changesDirection = True
lastTransition = time.time()
turnStrongerRight()
if ((leftSensor.getColor() == "grey" and rightSensorPrevColor == "white")
or (leftSensor.getColor() == "black" and rightSensorPrevColor == "grey")
or (leftSensor.getColor == "white" and rightSensorPrevColor == "black"))
and rightSensor.getColor() == leftSensor.getColor():
if(time.time() - lastTransition < transitionThreshold and lastTransitionSensor != rightSensor):
lastTransitionSensor = rightSensor
lastTransition = time.time()
turn180()
changesDirection = True
continue
changesDirection = True
lastTransition = time.time()
turnStrongerLeft()
def turnStrongerLeft():
doStuffForTurningStrongerLeft
changesDirection = False
def turnStrongerRight():
doStuffForTurningStrongerRight
changesDirection = False
def turn180():
doStuffForTurning180
changesDirection = False