forked from sch2307/all_new_rascar
-
Notifications
You must be signed in to change notification settings - Fork 0
/
3rd_assignment_main.py
137 lines (99 loc) · 4.62 KB
/
3rd_assignment_main.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
#########################################################################
# Date: 2018/10/02
# file name: 2nd_assignment_main.py
# Purpose: this code has been generated for the 4 wheel drive body
# moving object to perform the project with line detector
# this code is used for the student only
#########################################################################
from car import Car
import time
class myCar(object):
def __init__(self, car_name):
self.car = Car(car_name)
def drive_parking(self):
self.car.drive_parking()
# =======================================================================
# 2ND_ASSIGNMENT_CODE
# Complete the code to perform Second Assignment
# =======================================================================
def line_tracing(self, lt_status):
if lt_status == [0,0,1,0,0] or lt_status == [0,1,1,1,0]:
return 90
elif lt_status == [0,1,1,0,0] or lt_status == [1,1,1,1,0]:
return 90-15
elif lt_status == [0,0,1,1,0] or lt_status == [0,1,1,1,1]:
return 90+15
elif lt_status == [0,1,0,0,0] or lt_status == [1,1,1,0,0]:
return 90-10
elif lt_status == [0,0,1,1,1] or lt_status == [0,0,0,1,0]:
return 90+10
elif lt_status == [1,1,0,0,0]:
return 90-30
elif lt_status == [0,0,0,1,1]:
return 90+30
elif lt_status == [1,0,0,0,0]:
return 90-35
elif lt_status == [0,0,0,0,1]:
return 90+35
else:
return 90
def car_startup(self):
current_speed = self.car.SLOW
current_direction = 90
current_distance = self.car.distance_detector.get_distance()
lab = 0
while lab < 2:
lt_status = self.car.line_detector.read_digital()
current_direction = self.line_tracing(lt_status)
self.car.accelerator.go_forward(current_speed)
self.car.steering.turn(current_direction)
current_distance = self.car.distance_detector.get_distance()
print(lt_status, current_distance)
if 0 < current_distance < 25:
escape_time = 1.5
print("loop 1 in")
while 1 in lt_status:
self.car.steering.turn(90-35)
lt_status = self.car.line_detector.read_digital()
print(lt_status)
time.sleep(escape_time)
self.car.steering.turn(90)
print("loop 2 in")
while lt_status == [0,0,0,0,0]:
lt_status = self.car.line_detector.read_digital()
print("loop 3 in")
while 1 in lt_status:
self.car.steering.turn(90+35)
lt_status = self.car.line_detector.read_digital()
print(lt_status)
time.sleep(escape_time)
self.car.steering.turn(90)
print("loop 4 in")
while lt_status == [0,0,0,0,0]:
lt_status = self.car.line_detector.read_digital()
print("loop out")
if lt_status == [0,0,0,0,0]:
print("back")
if current_direction < 90:
self.car.steering.turn(90-35)
while not self.car.line_detector.read_digital()[3]:
self.car.accelerator.go_backward(current_speed)
else:
self.car.steering.turn(90+35)
while not self.car.line_detector.read_digital()[1]:
self.car.accelerator.go_backward(current_speed)
elif lt_status == [1,1,1,1,1]:
lab += 1
while lt_status == [1,1,1,1,1]:
lt_status = self.car.line_detector.read_digital()
self.car.accelerator.stop()
self.car.steering.center_alignment()
print("stop")
if __name__ == "__main__":
try:
myCar = myCar("CarName")
myCar.car_startup()
except KeyboardInterrupt:
# when the Ctrl+C key has been pressed,
# the moving object will be stopped
myCar.drive_parking()