YTS-Code/lidarSerial.py at main · pi34/YTS-Code · 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
from dis import dis
import serial
import numpy as np
import matplotlib
matplotlib.use('TkAgg')
import threading
import matplotlib.pyplot as plt
import matplotlib.animation as animation
from rplidar import RPLidar

PORT_NAME = "/dev/cu.usbserial-0001"
lidar = RPLidar(PORT_NAME)

length = 360*2

scanX = [0] * length
scanY = [0] * length

arduino = serial.Serial(port='/dev/cu.usbmodem1101', baudrate=115200, timeout=.1)

decision = '1'

try:
    def fetchData():

        global scanX
        global scanY
        global lidar
        global decision

        for scan in lidar.iter_scans():

            obstruction = 1e99
            ang = 370

            for (_, angle, distance) in scan:

                if (angle <= 45 or angle >= 315):

                    #print("single:", angle, distance, obstruction)

                    if (distance <= obstruction):

                        obstruction = distance
                        ang = angle

                    angle_value = float(angle) * 0.0174533

                    ox = distance * np.sin(angle_value)
                    oy = distance * np.cos(angle_value)

                    scanX.append(ox)
                    scanY.append(oy)

                    scanX = scanX[-length:]
                    scanY = scanY[-length:]

            #print("final:", ang, obstruction)

            if (obstruction <= 500):
                decision = '5'
            elif (obstruction >= 500 and obstruction <= 1000):
                if (ang <= 45):
                    decision = '2'
                else:
                    decision = '3'
            else:
                decision = '1'

            #print("decision:", decision)

    def writeToArd ():
        global decision
        try:
            while True:
                #print("writting to arduino",decision)
                arduino.write(bytes(decision, 'utf-8'))
                data = arduino.readline()
                print(data)
        finally:
            arduino.close()

    firstThread = threading.Thread(target=fetchData)
    firstThread.start()

    if not arduino.isOpen():
        arduino.open()

    dataThread = threading.Thread(target=writeToArd)
    dataThread.start()

    fig = plt.figure()

    radius = 500
    ax = fig.add_subplot(1, 1, 1)


    def animate(i):
        global scanX
        global scanY
        ax.clear()
        ax.plot(scanX, scanY, "bo")
        for i in range (360):
            ax.plot(radius * np.cos(np.radians(i)), radius * np.sin(np.radians(i)), "ro")

    ani = animation.FuncAnimation(fig, animate, fargs = (), interval = 1)
    plt.show()

finally:
    lidar.stop()
    lidar.disconnect()
    firstThread.join()
    dataThread.join()