forked from pinetum/Sensor-JY901
-
Notifications
You must be signed in to change notification settings - Fork 0
/
connecter.py
180 lines (153 loc) · 6.07 KB
/
connecter.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
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
# 5550
# 1978
# 5551
# 1979
# 5552
# 1980
# 5553
# 1981
# 5554
# 1982
# 5557
# 1983
# 5558
# 1984
# 555a
# 1985
import os
import sys
import math
import serial
import pygame
from operator import itemgetter
class Point3D:
def __init__(self, x = 0, y = 0, z = 0):
self.x, self.y, self.z = float(x), float(y), float(z)
def rotateX(self, angle):
""" Rotates the point around the X axis by the given angle in degrees. """
rad = angle * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
y = self.y * cosa - self.z * sina
z = self.y * sina + self.z * cosa
return Point3D(self.x, y, z)
def rotateY(self, angle):
""" Rotates the point around the Y axis by the given angle in degrees. """
rad = angle * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
z = self.z * cosa - self.x * sina
x = self.z * sina + self.x * cosa
return Point3D(x, self.y, z)
def rotateZ(self, angle):
""" Rotates the point around the Z axis by the given angle in degrees. """
rad = angle * math.pi / 180
cosa = math.cos(rad)
sina = math.sin(rad)
x = self.x * cosa - self.y * sina
y = self.x * sina + self.y * cosa
return Point3D(x, y, self.z)
def project(self, win_width, win_height, fov, viewer_distance):
""" Transforms this 3D point to 2D using a perspective projection. """
factor = fov / (viewer_distance + self.z)
x = self.x * factor + win_width / 2
y = -self.y * factor + win_height / 2
return Point3D(x, y, self.z)
class RealTimeCube:
def __init__(self, win_width = 640, win_height = 480):
pygame.init()
self.screen = pygame.display.set_mode((win_width, win_height))
pygame.display.set_caption("JY901-realtime Cube by qtlintw")
self.clock = pygame.time.Clock()
self.vertices = [
Point3D(-1,1,-1),
Point3D(1,1,-1),
Point3D(1,-1,-1),
Point3D(-1,-1,-1),
Point3D(-1,1,1),
Point3D(1,1,1),
Point3D(1,-1,1),
Point3D(-1,-1,1)
]
# Define the vertices that compose each of the 6 faces. These numbers are
# indices to the vertices list defined above.
self.faces = [(0,1,2,3),(1,5,6,2),(5,4,7,6),(4,0,3,7),(0,4,5,1),(3,2,6,7)]
# Define colors for each face
self.colors = [(255,0,255),(255,0,0),(0,255,0),(0,0,255),(0,255,255),(255,255,0)]
self.angle_x = 0
self.angle_y = 0
self.angle_z = 0
def run(self):
#inital serial port
jy_sensor = serial.Serial(port=sys.argv[1], baudrate="9600", timeout=2)
print(jy_sensor.name)
try:
while True:
data = jy_sensor.read(size=1)
if data == b'\x55':
jy_sensor.read(size=10)
print("success!")
break;
print("trying", data)
#main loop
while 1:
for event in pygame.event.get():
if event.type == pygame.QUIT:
pygame.quit()
sys.exit()
data = jy_sensor.read(size=11)
if not len(data) == 11:
print("error byte lenth:", len(data), "data=", data)
continue
if data[0] != b'\x55':
print("error occurr, nor 55 start:", data[0])
break
if data[1] == 83:
bytes_x = data[2:4]
bytes_y = data[4:6]
bytes_z = data[6:8]
bytes_x.reverse()
bytes_y.reverse()
bytes_z.reverse()
self.angle_x = int.from_bytes(bytes_x, byteorder='little')/32768*180
self.angle_y = int.from_bytes(bytes_y, byteorder='little')/32768*180
self.angle_z = int.from_bytes(bytes_z.reverse(), byteorder='little')/32768*180
print("Angle output:{}, {}, {}".format(self.angle_x, self.angle_y, self.angle_z))
else:
print(data)
continue
#print("-----", data[0], data[1])
# It will hold transformed vertices.
#self.clock.tick(50)
self.screen.fill((0,32,0))
t = []
for v in self.vertices:
# Rotate the point around X axis, then around Y axis, and finally around Z axis.
r = v.rotateX(self.angle_x).rotateY(self.angle_y).rotateZ(self.angle_z)
# Transform the point from 3D to 2D
p = r.project(self.screen.get_width(), self.screen.get_height(), 256, 4)
# Put the point in the list of transformed vertices
t.append(p)
# Calculate the average Z values of each face.
avg_z = []
i = 0
for f in self.faces:
z = (t[f[0]].z + t[f[1]].z + t[f[2]].z + t[f[3]].z) / 4.0
avg_z.append([i,z])
i = i + 1
# Draw the faces using the Painter's algorithm:
# Distant faces are drawn before the closer ones.
for tmp in sorted(avg_z,key=itemgetter(1),reverse=True):
face_index = tmp[0]
f = self.faces[face_index]
pointlist = [(t[f[0]].x, t[f[0]].y), (t[f[1]].x, t[f[1]].y),
(t[f[1]].x, t[f[1]].y), (t[f[2]].x, t[f[2]].y),
(t[f[2]].x, t[f[2]].y), (t[f[3]].x, t[f[3]].y),
(t[f[3]].x, t[f[3]].y), (t[f[0]].x, t[f[0]].y)]
pygame.draw.polygon(self.screen,self.colors[face_index],pointlist)
pygame.display.flip()
except KeyboardInterrupt:
jy_sensor.close()
print("close port")
if __name__ == "__main__":
RealTimeCube().run()