rascar/2nd_assignment_main.py at master · KMUlee/rascar · GitHub

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
#########################################################################
# 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
#########################################################################
import front_wheels
import rear_wheels
from SEN040134.SEN040134_Tracking import SEN040134_Tracking
from car import Car
import time
import threading
from song import Song

def sing():
    s = Song()
    s.song()

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 car_startup(self):
        line = SEN040134_Tracking([16, 18, 22, 40, 32])
        drive = rear_wheels.Rear_Wheels(db='config')
        front = front_wheels.Front_Wheels()
        drive.ready()
        temp = 0
        num = 0
        curve = 0
        test =1
        t = threading.Thread(target=sing, daemon=True)
        while(True):
            rawData = self.car.color_getter.get_raw_data()
            print("R", rawData[0])
            print("G", rawData[1])
            print("B", rawData[2])
            print(test)
            print(line.read_digital())
            distance = self.car.distance_detector.get_distance()
            print(distance)
            if test == 1:
                drive.go_forward(30)
                if line.is_equal_status([1, 0, 0, 0, 0]):
                    temp = 70
                    front.turn(temp)
                elif line.is_equal_status([1,1,0,0,0]):
                    temp = 75
                    front.turn(temp)
                elif line.is_equal_status([0,1,0,0,0]):
                    temp = 80
                    front.turn(temp)
                elif line.is_equal_status([0, 1, 1, 0, 0]):
                    temp = 85
                    front.turn(temp)
                elif line.is_equal_status([0, 0, 1, 0, 0]):
                    temp = 90
                    front.turn(temp)
                elif line.is_equal_status([0, 0, 1, 1, 0]):
                    temp = 95
                    front.turn(temp)
                elif line.is_equal_status([0, 0, 0, 1, 0]):
                    temp = 100
                    front.turn(temp)
                elif line.is_equal_status([0, 0, 0, 1, 1]):
                    temp = 105
                    front.turn(temp)
                elif line.is_equal_status([0, 0, 0, 0, 1]):
                    temp = 110
                    front.turn(temp)
                elif line.is_equal_status([0,0,0,0,0]):
                    front.turn(120)
                    drive.go_backward(25)
                    time.sleep(0.1)
                elif line.is_equal_status([1,1,1,1,1]) or line.is_equal_status([1,1,1,1,0]):
                    drive.stop()
                    break
                elif line.is_equal_status([1,1,1,0,0]) and num == 0:
                    try:
                        t.start()
                    except:
                        pass
                    front.turn(90)
                    time.sleep(1.3)
                    for i in range(2):
                        front.turn(60)
                        drive.go_backward(30)
                        time.sleep(1.5)
                        front.turn(120)
                        drive.go_forward(30)
                        time.sleep(1.3)
                    front.turn(90)
                    drive.go_backward(30)
                    time.sleep(3)
                    drive.stop()
                    time.sleep(5)
                    front.turn(60)
                    drive.go_forward(30)
                    time.sleep(0.7)
                    num+=1
                else:
                    front.turn(temp)
                if 0 < distance < 8:
                    drive.go_backward(30)
                    time.sleep(0.2)
                    if rawData[0] > rawData[1] and rawData[0] > rawData[2]:
                        drive.stop()
                        print("Stop")
                        print("R", rawData[0])
                        print("G", rawData[1])
                        print("B", rawData[2])
                        time.sleep(5)
                    elif rawData[1] > rawData[0] and rawData[1] > rawData[2]:
                        drive.stop()
                        drive.go_backward(20)
                        time.sleep(0.1)
                        drive.stop()
                        print("Go")
                        print("R", rawData[0])
                        print("G", rawData[1])
                        print("B", rawData[2])
                        front.turn(120)
                        drive.go_backward(60)
                        time.sleep(0.3)
                        test = 2

                time.sleep(0.1)
            elif test == 2:
                drive.go_forward(60)
                front.turn(60)
                time.sleep(0.9)
                front.turn(120)
                time.sleep(1.3)
                test = 1
            elif test == 3:
                drive.stop()


if __name__ == "__main__":
    try:
        myCar = myCar("CarName")
        myCar.car_startup()
    except KeyboardInterrupt:
        pass