1# labplus mPython library
2# MIT license; Copyright (c) 2018 labplus
3# V1.0 Zhang KaiHua(apple_eat@126.com)
4
5# mpython buildin periphers drivers
6
7# history:
8# V1.1 add oled draw function,add buzz.freq(). by tangliufeng
9# V1.2 add servo/ui class,by tangliufeng
10# labplus_Ledong_v2 202411
11# v2.0 modified by zhaohuijiang for mpython V3 20250301
12
13from machine import I2C, PWM, Pin, ADC
14import esp, math, time, network
15import ustruct
16from neopixel import NeoPixel
17import time
18from micropython import schedule,const
19from esp32 import NVS
20from _ntptime import *
21from ltr308 import *
22import framebuf
23import lcd
24
25i2c = I2C(0, scl=Pin(43), sda=Pin(44), freq=200000)
26
27if '_print' not in dir(): _print = print
28
29def print(_t, *args, sep=' ', end='\n'):
30 _s = str(_t)[0:1]
31 if u'\u4e00' <= _s <= u'\u9fff':
32 _print(' ' + str(_t), *args, sep=sep, end=end)
33 else:
34 _print(_t, *args, sep=sep, end=end)
35
36def numberMap(inputNum, bMin, bMax, cMin, cMax):
37 outputNum = 0
38 outputNum = ((cMax - cMin) / (bMax - bMin)) * (inputNum - bMin) + cMin
39 return outputNum
40
41class TFT_displayer(framebuf.FrameBuffer):
42 def __init__(self):
43 self.lcd_width = 320
44 self.lcd_height = 172
45 self.buffer = bytearray(110080) # self.lcd_width*self.lcd_height*2
46 super().__init__(self.buffer, self.lcd_width, self.lcd_height, framebuf.RGB565)
47
48 def _mono2rgb65(self, ch_bitmap:bytes, ch_w:int, ch_h:int, char_col=0xffff)->list[int]:
49 """
50 黑白像素字符缓存点转化为RGB565缓存
51 param bitmap: 黑白像素点字符缓存
52 param ch_w ch_w: 缓存宽高
53 param char_col: 目标字体颜色
54 return: RGB565字符缓存
55 """
56 char_buf = bytearray(ch_w * ch_h * 2)
57 char_fb = framebuf.FrameBuffer(char_buf, ch_w, ch_h, framebuf.RGB565)
58
59 # 填充字符FB：将1bit位图映射为RGB565像素
60 for row in range(ch_h):
61 for col in range(ch_w):
62 # 计算当前像素在1bit位图中的字节/位位置
63 byte_idx = row * ((ch_w + 7) // 8) + (col // 8)
64 bit_idx = 7 - (col % 8) # 适配font-to-py的位序（bit7对应第一个像素）
65
66 # 读取1bit值（0=背景/透明，1=前景）
67 bit_val = (ch_bitmap[byte_idx] >> bit_idx) & 0x01 if byte_idx < len(ch_bitmap) else 0
68
69 # 映射为RGB565颜色并填充到字符FB
70 bg_color = super().background_color()
71 color = char_col if bit_val else bg_color
72 char_fb.pixel(col, row, color)
73
74 return char_fb
75
76 def DispChar(self, str, x, y, color=lcd.WHITE, auto_wrap = False):
77 """
78 显示内置思源24位黑体字符
79 :param str: 待显示字符
80 :param x y: 位置
81 :param color:字体颜色
82 :param auto_wrap:是否换行
83 """
84 self.dispChar(str, x, y, color, auto_wrap)
85
86 def DispChar_font(self, font, str, x, y, color=lcd.WHITE, auto_wrap = False):
87 """
88 显示用户自定义字体，参考： https://github.com/peterhinch/micropython-font-to-py
89 参数
90 :param font: 使用 font_to_py.py 从`ttf` or `otf`转换出的字体py文件，内置的相关字体位于font文件夹下.
91 :param str: 待显示字体，内置了数码管字体和相关ASCII字符字体，当然用户可以自已制作中文字体。
92 :param x y: 显示位置坐标
93 :param color: 字体颜色
94 :param auto_wrap: 是否自动换行，每行高度：char_height + 5，擦除一行时会用到此参数
95 """
96 is_x_change = False
97 for char in str:
98 char_buf, char_height, char_width = font.get_ch(char)
99
100 # 在数码管字体中，调整‘1’字符位置，在时钟显示中更美观
101 if font.fon_type() == 'DIGIFACE':
102 if char == '1':
103 x = x + char_width // 2
104 is_x_change = True
105 elif is_x_change:
106 x = x - char_width // 2
107 is_x_change = False
108
109 if x > self.lcd_width - char_width:
110 if auto_wrap:
111 x = 0
112 y += char_height + 5
113 else: # x超出屏幕宽度则不显示
114 return
115 if y > 240 - char_height: # y超出屏幕高度则不显示
116 return
117
118 fb = self._mono2rgb65(char_buf, char_width, char_height, color)
119 super().blit(fb, x, y)
120 x += char_width #+ 5
121
122 def clear(self, color = None):
123 if color is not None:
124 super().background_color(color)
125 super().fill(super().background_color())
126
127 def show(self):
128 lcd.show(self.buffer)
129
130display = TFT_displayer()
131
132# my_wifi = wifi()
133#多次尝试连接wifi
134def try_connect_wifi(_wifi, _ssid, _pass, _times):
135 if _times < 1: return False
136 try:
137 print("Try Connect WiFi ... {} Times".format(_times) )
138 _wifi.connectWiFi(_ssid, _pass)
139 if _wifi.sta.isconnected(): return True
140 else:
141 time.sleep(5)
142 return try_connect_wifi(_wifi, _ssid, _pass, _times-1)
143 except:
144 time.sleep(5)
145 return try_connect_wifi(_wifi, _ssid, _pass, _times-1)
146
147class wifi:
148 def __init__(self):
149 self.sta = network.WLAN(network.STA_IF)
150 self.ap = network.WLAN(network.AP_IF)
151
152 def connectWiFi(self, ssid, passwd, timeout=10000):
153 if self.sta.isconnected():
154 self.sta.disconnect()
155 self.sta.active(True)
156 list = self.sta.scan()
157 for i, wifi_info in enumerate(list):
158 try:
159 if wifi_info[0].decode() == ssid:
160 self.sta.connect(ssid, passwd)
161 wifi_dbm = wifi_info[3]
162 break
163 except UnicodeError:
164 self.sta.connect(ssid, passwd)
165 wifi_dbm = '?'
166 break
167 if i == len(list) - 1:
168 raise OSError("SSID invalid / failed to scan this wifi")
169 start = time.time()
170 print("Connection WiFi", end="")
171 while (self.sta.ifconfig()[0] == '0.0.0.0'):
172 if time.ticks_diff(time.time(), start) > timeout:
173 print("")
174 raise OSError("Timeout!,check your wifi password and keep your network unblocked")
175 print(".", end="")
176 time.sleep_ms(500)
177 print("")
178 print('WiFi(%s,%sdBm) Connection Successful, Config:%s' % (ssid, str(wifi_dbm), str(self.sta.ifconfig())))
179
180 def disconnectWiFi(self):
181 if self.sta.isconnected():
182 self.sta.disconnect()
183 self.sta.active(False)
184 print('disconnect WiFi...')
185
186 def enable_APWiFi(self, essid, password=b'',channel=10):
187 self.ap.active(True)
188 if password:
189 authmode=4
190 else:
191 authmode=0
192 self.ap.config(essid=essid,password=password,authmode=authmode, channel=channel)
193
194 def disable_APWiFi(self):
195 self.ap.active(False)
196 print('disable AP WiFi...')
197
198class MOTION(object):
199 def __init__(self):
200 self.i2c = i2c
201 addr = self.i2c.scan()
202 if 107 in addr:
203 MOTION.IIC_ADDR = 107
204 else:
205 raise OSError("MOTION init error")
206
207 MOTION._writeReg(0x60, 0x01) # soft reset regist value.
208 time.sleep_ms(20)
209 MOTION._writeReg(0x02, 0x60) # Enabe reg address auto increment auto
210 MOTION._writeReg(0x08, 0x03) # Enable accel and gyro
211 MOTION._writeReg(0x03, 0x1c) # accel range:4g ODR 128HZ
212 MOTION._writeReg(0x04, 0x40) # gyro ODR 8000HZ, FS 256dps
213 MOTION._writeReg(0x06, 0x55) # Enable accel and gyro Low-Pass Filter
214 # print('Motion init finished!')
215
216 # @staticmethod
217 def _readReg(reg, nbytes=1):
218 return i2c.readfrom_mem(MOTION.IIC_ADDR, reg, nbytes)
219
220 # @staticmethod
221 def _writeReg(reg, value):
222 i2c.writeto_mem(MOTION.IIC_ADDR, reg, value.to_bytes(1, 'little'))
223
224 def get_fw_version(self):
225 MOTION._writeReg(0x0a, 0x10) # send ctrl9R read FW cmd
226 while True:
227 if (MOTION._readReg(0x2F, 1)[0] & 0X01) == 0X01:
228 break
229 buf = MOTION._readReg(0X49, 3)
230
231 class Accelerometer():
232 """QMI8658C"""
233 # Range and resolustion
234 RANGE_2G = const(0)
235 RANGE_4G = const(1)
236 RANGE_8G = const(2)
237 RANGE_16G = const(3)
238 RES_14_BIT = const(0)
239 RES_12_BIT = const(1)
240 RES_10_BIT = const(2)
241
242 def __init__(self):
243 # 设置偏移值
244 self.x_offset = 0
245 self.y_offset = 0
246 self.z_offset = 0
247 self.get_nvs_offset()
248 try:
249 id = MOTION._readReg(0x0, 2)
250 except:
251 pass
252 self.set_range(MOTION.Accelerometer.RANGE_2G) #设置默认分辨率+-2g
253 self.int = Pin(45, Pin.IN)
254 self.int.irq(trigger=Pin.IRQ_FALLING, handler=self.irq)
255 # event handler
256 self.wom = None
257
258
259 def irq(self, arg):
260 flag = MOTION._readReg(0x2F, 1)[0]
261 if (flag & 0x04) == 0x04:
262 print('wom int trigged.')
263
264 def wom_config(self):
265 MOTION._writeReg(0x60, 0x01) # soft reset regist value.
266 time.sleep_ms(20)
267 MOTION._writeReg(0x08, 0x0) # disable all sensor
268 MOTION._writeReg(0x03, 0x1c) # accel range:4g ODR 128HZ
269 MOTION._writeReg(0x0B, 0xfF) # CAL_L WoM Threshold(1mg/LSB resolution)
270 MOTION._writeReg(0x0C, 0x8F) # CAL_H WoM (INT1 blank time 0x1f)
271 MOTION._writeReg(0x0A, 0x08)
272 while True:
273 if (MOTION._readReg(0x2F, 1)[0] & 0X01) == 0X01:
274 break
275 MOTION._writeReg(0x08, 0x01) # enable accel
276
277 def set_resolution(self, resolution):# set data output rate
278 self.odr = resolution
279 format = MOTION._readReg(0x03, 1)
280 format = format[0] & 0xf0
281 format |= (resolution & 0x0f)
282 MOTION._writeReg(0x03, format)
283
284 def set_range(self, range):
285 if(range==3):
286 range = 64 #0x40
287 else:
288 range = range << 4
289 self.FS = 2*(2**(range >> 4))
290 format = MOTION._readReg(0x03, 1)
291 format = format[0] & 0x8F
292 format |= range
293 MOTION._writeReg(0x03, format)
294
295 def set_offset(self, x=None, y=None, z=None):
296 for i in (x, y, z):
297 if i is not None:
298 if i < -16 or i > 16:
299 raise ValueError("超出调整范围!!!")
300 if x is not None:
301 self.x_offset = x
302 self.set_nvs_offset("x", x)
303 if y is not None:
304 self.y_offset = y
305 self.set_nvs_offset("y", y)
306 if z is not None:
307 self.z_offset = z
308 self.set_nvs_offset("z", z)
309
310 def get_x(self):
311 buf = MOTION._readReg(0x35, 2)
312 x = ustruct.unpack('<h', buf)[0]
313 return (x * self.FS) / 32768 + self.x_offset
314
315 def get_y(self):
316 buf = MOTION._readReg(0x37, 2)
317 y = ustruct.unpack('<h', buf)[0]
318 return (y * self.FS) / 32768 + self.y_offset
319
320 def get_z(self):
321 buf = MOTION._readReg(0x39, 2)
322 z = ustruct.unpack('<h', buf)[0]
323 return (z * self.FS) / 32768 + self.z_offset
324 # return -(z * self.FS) / 32768
325
326 def roll_pitch_angle(self):
327 x, y, z = self.get_x(), self.get_y(), -self.get_z()
328 # vector normalize
329 mag = math.sqrt(x ** 2 + y ** 2+z ** 2)
330 x /= mag
331 y /= mag
332 z /= mag
333 roll = math.degrees(-math.asin(y))
334 pitch = math.degrees(math.atan2(x, z))
335
336 return roll, pitch
337
338 def get_nvs_offset(self):
339 try:
340 tmp = NVS("offset_a")
341 self.x_offset = round(tmp.get_i32("x")/1e5, 5)
342 self.y_offset = round(tmp.get_i32("y")/1e5, 5)
343 self.z_offset = round(tmp.get_i32("z")/1e5, 5)
344 except OSError as e:
345 # print('Accelerometer get_nvs_offset:',e)
346 # self.x_offset = 0
347 # self.y_offset = 0
348 # self.z_offset = 0
349 self.set_offset(0,0,0)
350
351 def set_nvs_offset(self, key, value):
352 try:
353 nvs = NVS("offset_a")
354 nvs.set_i32(key, int(value*1e5))
355 nvs.commit()
356 except OSError as e:
357 print('Gyroscope set_nvs_offset error:',e)
358
359 class Gyroscope():
360 # gyro full scale
361 RANGE_16_DPS = const(0x00)
362 RANGE_32_DPS = const(0x10)
363 RANGE_64_DPS = const(0x20)
364 RANGE_128_DPS = const(0x30)
365 RANGE_256_DPS = const(0x40)
366 RANGE_512_DPS = const(0x50)
367 RANGE_1024_DPS = const(0x60)
368 RANGE_2048_DPS = const(0x70)
369
370 def __init__(self):
371 # 设置偏移值
372 self.x_offset = 0
373 self.y_offset = 0
374 self.z_offset = 0
375 self.get_nvs_offset()
376 self.set_range(MOTION.Gyroscope.RANGE_256_DPS)
377
378 def set_range(self, range):
379 self.FS = 16*(2**(range >> 4))
380 format = MOTION._readReg(0x04, 1)
381 format = format[0] & 0x8F
382 format |= range
383 MOTION._writeReg(0x04, format)
384
385 def set_ODR(self, odr): # set data output rate
386 self.odr = odr
387 format = MOTION._readReg(0x04, 1)
388 format = format[0] & 0xF0
389 format |= odr
390 MOTION._writeReg(0x04, format)
391
392 def get_x(self):
393 buf = MOTION._readReg(0x3b, 2)
394 x = ustruct.unpack('<h', buf)[0]
395 return (x * self.FS) / 32768 + self.x_offset
396
397 def get_y(self):
398 buf = MOTION._readReg(0x3d, 2)
399 y = ustruct.unpack('<h', buf)[0]
400 return (y * self.FS) / 32768 + self.y_offset
401
402 def get_z(self):
403 buf = MOTION._readReg(0x3f, 2)
404 z = ustruct.unpack('<h', buf)[0]
405 return (z * self.FS) / 32768 + self.z_offset
406
407 def set_offset(self, x=None, y=None, z=None):
408 for i in (x, y, z):
409 if i is not None:
410 if i < -4096 or i > 4096:
411 raise ValueError("超出调整范围!!!")
412 if x is not None:
413 self.x_offset = x
414 self.set_nvs_offset("x", x)
415 if y is not None:
416 self.y_offset = y
417 self.set_nvs_offset("y", y)
418 if z is not None:
419 self.z_offset = z
420 self.set_nvs_offset("z", z)
421
422 def get_nvs_offset(self):
423 try:
424 tmp = NVS("offset_g")
425 self.x_offset = round(tmp.get_i32("x")/1e5, 5)
426 self.y_offset = round(tmp.get_i32("y")/1e5, 5)
427 self.z_offset = round(tmp.get_i32("z")/1e5, 5)
428 except OSError as e:
429 # print('Gyroscope get_nvs_offset:',e)
430 self.set_offset(0,0,0)
431 # self.x_offset = 0
432 # self.y_offset = 0
433 # self.z_offset = 0
434
435 def set_nvs_offset(self, key, value):
436 try:
437 nvs = NVS("offset_g")
438 nvs.set_i32(key, int(value*1e5))
439 nvs.commit()
440 except OSError as e:
441 print('Gyroscope set_nvs_offset error:',e)
442
443motion = MOTION()
444accelerometer = motion.Accelerometer()
445gyroscope = motion.Gyroscope()
446
447class Magnetic(object):
448 """ MMC5983MA driver """
449 """ MMC5603NJ driver 20211028替换"""
450 def __init__(self):
451 self.addr = 48
452 self.i2c = i2c
453 self._judge_id()
454 time.sleep_ms(5)
455 if (self.product_ID==48):
456 pass # MMC5983MA
457 elif (self.product_ID==16):
458 pass # MMC5603NJ
459 else:
460 raise OSError("Magnetic init error")
461 """ MMC5983MA driver """
462 # 传量器裸数据，乘0.25后转化为mGS
463 self.raw_x = 0.0
464 self.raw_y = 0.0
465 self.raw_z = 0.0
466 # 校准后的偏移量, 基于裸数据
467 self.cali_offset_x = 0.0
468 self.cali_offset_y = 0.0
469 self.cali_offset_z = 0.0
470 # 去皮偏移量，类似电子秤去皮功能，基于裸数据。
471 self.peeling_x = 0.0
472 self.peeling_y = 0.0
473 self.peeling_z = 0.0
474 self.is_peeling = 0
475 if (self.chip==1):
476 self.i2c.writeto(self.addr, b'\x09\x20\xbd\x00', True)
477 """ MMC5603NJ driver """
478 if (self.chip==2):
479 self._writeReg(0x1C, 0x80)#软件复位
480 time.sleep_ms(100)
481 self._writeReg(0x1A, 255)
482 self._writeReg(0x1B, 0b10100001)
483 # self._writeReg(0x1C, 0b00000011)
484 self._writeReg(0x1C, 0b00000000)
485 self._writeReg(0x1D, 0b10010000)
486 time.sleep_ms(100)
487
488 def _readReg(self, reg, nbytes=1):
489 return i2c.readfrom_mem(self.addr, reg, nbytes)
490
491 def _writeReg(self, reg, value):
492 i2c.writeto_mem(self.addr, reg, value.to_bytes(1, 'little'))
493
494 def _set_offset(self):
495 if(self.chip == 1):
496 self.i2c.writeto(self.addr, b'\x09\x08', True) #set
497 self.i2c.writeto(self.addr, b'\x09\x01', True)
498 while True:
499 self.i2c.writeto(self.addr, b'\x08', False)
500 buf = self.i2c.readfrom(self.addr, 1)
501 status = ustruct.unpack('B', buf)[0]
502 if(status & 0x01):
503 break
504 self.i2c.writeto(self.addr, b'\x00', False)
505 buf = self.i2c.readfrom(self.addr, 6)
506 data = ustruct.unpack('>3H', buf)
507
508 self.i2c.writeto(self.addr, b'\x09\x10', True) #reset
509
510 self.i2c.writeto(self.addr, b'\x09\x01', True)
511 while True:
512 self.i2c.writeto(self.addr, b'\x08', False)
513 buf = self.i2c.readfrom(self.addr, 1)
514 status = ustruct.unpack('B', buf)[0]
515 if(status & 0x01):
516 break
517 self.i2c.writeto(self.addr, b'\x00', False)
518 buf = self.i2c.readfrom(self.addr, 6)
519 data1 = ustruct.unpack('>3H', buf)
520
521 self.x_offset = (data[0] + data1[0])/2
522 self.y_offset = (data[1] + data1[1])/2
523 self.z_offset = (data[2] + data1[2])/2
524 elif(self.chip == 2):
525 pass
526
527 def _get_raw(self):
528 if (self.chip == 1):
529 retry = 0
530 if (retry < 5):
531 try:
532 self.i2c.writeto(self.addr, b'\x09\x08', True) #set
533
534 self.i2c.writeto(self.addr, b'\x09\x01', True)
535 while True:
536 self.i2c.writeto(self.addr, b'\x08', False)
537 buf = self.i2c.readfrom(self.addr, 1)
538 status = ustruct.unpack('B', buf)[0]
539 if(status & 0x01):
540 break
541 self.i2c.writeto(self.addr, b'\x00', False)
542 buf = self.i2c.readfrom(self.addr, 6)
543 data = ustruct.unpack('>3H', buf)
544
545 self.i2c.writeto(self.addr, b'\x09\x10', True) #reset
546
547 self.i2c.writeto(self.addr, b'\x09\x01', True)
548 while True:
549 self.i2c.writeto(self.addr, b'\x08', False)
550 buf = self.i2c.readfrom(self.addr, 1)
551 status = ustruct.unpack('B', buf)[0]
552 if(status & 0x01):
553 break
554 self.i2c.writeto(self.addr, b'\x00', False)
555 buf = self.i2c.readfrom(self.addr, 6)
556 data1 = ustruct.unpack('>3H', buf)
557
558 self.raw_x = -((data[0] - data1[0])/2)
559 self.raw_y = -((data[1] - data1[1])/2)
560 self.raw_z = -((data[2] - data1[2])/2)
561 # print(str(self.raw_x) + " " + str(self.raw_y) + " " + str(self.raw_z))
562 except:
563 retry = retry + 1
564 else:
565 raise Exception("i2c read/write error!")
566 elif(self.chip == 2):
567 retry = 0
568 if (retry < 5):
569 try:
570 _raw_x = 0.0
571 _raw_y = 0.0
572 _raw_z = 0.0
573
574 self.i2c.writeto(self.addr, b'\x1B\x08', True) #set
575 self.i2c.writeto(self.addr, b'\x1B\x01', True)
576
577 while True:
578 time.sleep_ms(25)
579 buf = self._readReg(0x18, 1)
580 status = buf[0]
581 if(status & 0x40):
582 break
583
584 buf = self._readReg(0x00, 9)
585
586 _raw_x = (buf[0] << 12) | (buf[1] << 4) | (buf[6] >> 4)
587 _raw_y = (buf[2] << 12) | (buf[3] << 4) | (buf[7] >> 4)
588 _raw_z = (buf[4] << 12) | (buf[5] << 4) | (buf[8] >> 4)
589
590 self.raw_x = _raw_x
591 self.raw_y = _raw_y
592 self.raw_z = _raw_z
593 except:
594 retry = retry + 1
595 else:
596 raise Exception("i2c read/write error!")
597
598 def peeling(self):
599 '''
600 去除磁场环境
601 '''
602 self._get_raw()
603 self.peeling_x = self.raw_x
604 self.peeling_y = self.raw_y
605 self.peeling_z = self.raw_z
606 self.is_peeling = 1
607
608 def clear_peeling(self):
609 self.peeling_x = 0.0
610 self.peeling_y = 0.0
611 self.peeling_z = 0.0
612 self.is_peeling = 0
613
614 def get_x(self):
615 if (self.chip == 1):
616 self._get_raw()
617 return self.raw_x * 0.25
618 if (self.chip == 2):
619 self._get_raw()
620 if(self.cali_offset_x):
621 return -0.0625 * (self.raw_x - self.cali_offset_x)
622 else:
623 return -0.0625 * (self.raw_x - 524288)
624 # return -(self.raw_x - 524288)/16384
625
626 def get_y(self):
627 if (self.chip == 1):
628 self._get_raw()
629 return self.raw_y * 0.25
630 if (self.chip == 2):
631 self._get_raw()
632 if(self.cali_offset_y):
633 return -0.0625 * (self.raw_y - self.cali_offset_y)
634 else:
635 return -0.0625 * (self.raw_y - 524288)
636 # return -(self.raw_y - 524288)/16384
637
638 def get_z(self):
639 if (self.chip == 1):
640 self._get_raw()
641 return self.raw_z * 0.25
642 if (self.chip == 2):
643 self._get_raw()
644 if(self.cali_offset_z):
645 return 0.0625 * (self.raw_z - self.cali_offset_z)
646 else:
647 return 0.0625 * (self.raw_z - 524288)
648 # return (self.raw_z - 524288)/16384
649
650 def get_field_strength(self):
651 if(self.chip==1):
652 self._get_raw()
653 if self.is_peeling == 1:
654 return (math.sqrt((self.raw_x - self.peeling_x)*(self.raw_x - self.peeling_x) + (self.raw_y - self.peeling_y)*(self.raw_y - self.peeling_y) + (self.raw_z - self.peeling_z)*(self.raw_z - self.peeling_z)))*0.25
655 return (math.sqrt(self.raw_x * self.raw_x + self.raw_y * self.raw_y + self.raw_z * self.raw_z))*0.25
656 elif(self.chip==2):
657 self._get_raw()
658 if self.is_peeling == 1:
659 return (math.sqrt(math.pow(self.raw_x - self.peeling_x, 2) + pow(self.raw_y - self.peeling_y, 2) + pow(self.raw_z - self.peeling_z , 2)))*0.0625
660 return (math.sqrt(math.pow(self.get_x(), 2) + pow(self.get_y(), 2) + pow(self.get_z(), 2)))
661
662 def calibrate(self):
663 display.clear(lcd.WHITE)
664 w, h, buff = display.decode_png_internal(72)
665 fb = framebuf.FrameBuffer(buff, w, h, framebuf.RGB565)
666 display.blit(fb, 75, -17)
667 display.DispChar("步骤1:如图转几周", 50, 130, lcd.RED)
668 display.show()
669
670 self._get_raw()
671 min_x = max_x = self.raw_x
672 min_y = max_y = self.raw_y
673 min_z = max_z = self.raw_z
674 ticks_start = time.ticks_ms()
675 while (time.ticks_diff(time.ticks_ms(), ticks_start) < 15000) :
676 self._get_raw()
677 min_x = min(self.raw_x, min_x)
678 min_y = min(self.raw_y, min_y)
679 max_x = max(self.raw_x, max_x)
680 max_y = max(self.raw_y, max_y)
681 time.sleep_ms(100)
682 self.cali_offset_x = (max_x + min_x) / 2
683 self.cali_offset_y = (max_y + min_y) / 2
684 print('cali_offset_x: ' + str(self.cali_offset_x) + ' cali_offset_y: ' + str(self.cali_offset_y))
685
686 display.clear(lcd.WHITE)
687 w, h, buff = display.decode_png_internal(73)
688 fb = framebuf.FrameBuffer(buff, w, h, framebuf.RGB565)
689 display.blit(fb, 75, -20)
690 display.DispChar("步骤2:如图转几周", 50, 130, lcd.RED)
691 display.show()
692
693 time.sleep(5)
694 ticks_start = time.ticks_ms()
695 while (time.ticks_diff(time.ticks_ms(), ticks_start) < 15000) :
696 self._get_raw()
697 min_z = min(self.raw_z, min_z)
698 max_z = max(self.raw_z, max_z)
699 time.sleep_ms(100)
700 self.cali_offset_z = (max_z + min_z) / 2
701
702 print('cali_offset_z: ' + str(self.cali_offset_z))
703
704 display.clear(lcd.WHITE)
705 display.DispChar("校准完成！", 100, 60, lcd.RED)
706 display.show()
707
708 def get_heading(self):
709 if(self.chip==1):
710 self._get_raw()
711 temp_x = self.raw_x - self.cali_offset_x
712 temp_y = self.raw_y - self.cali_offset_y
713 # temp_z = self.raw_z - self.cali_offset_z
714 heading = math.atan2(temp_y, -temp_x) * (180 / 3.14159265) + 180 + 3
715 return heading
716 else:
717 if(self.cali_offset_x):
718 self._get_raw()
719 temp_x = -(self.raw_x - self.cali_offset_x)
720 temp_y = -(self.raw_y - self.cali_offset_y)
721 heading = math.atan2(temp_y, -temp_x) * (180 / 3.14159265) + 180 + 3
722 else:
723 heading = math.atan2(self.get_y(), -self.get_x()) * (180 / 3.14159265) + 180 + 3
724 return heading
725
726 def _get_temperature(self):
727 if(self.chip==1):
728 retry = 0
729 if (retry < 5):
730 try:
731 self.i2c.writeto(self.addr, b'\x09\x02', True)
732 while True:
733 self.i2c.writeto(self.addr, b'\x08', False)
734 buf = self.i2c.readfrom(self.addr, 1)
735 status = ustruct.unpack('B', buf)[0]
736 if(status & 0x02):
737 break
738 self.i2c.writeto(self.addr, b'\x07', False)
739 buf = self.i2c.readfrom(self.addr, 1)
740 temp = (ustruct.unpack('B', buf)[0])*0.8 -75
741 # print(data)
742 return temp
743 except:
744 retry = retry + 1
745 else:
746 raise Exception("i2c read/write error!")
747 elif(self.chip == 2):
748 pass
749
750 def _get_id(self):
751 if (self.chip==1):
752 retry = 0
753 if (retry < 5):
754 try:
755 self.i2c.writeto(self.addr, bytearray([0x2f]), False)
756 buf = self.i2c.readfrom(self.addr, 1, True)
757 print(buf)
758 id = ustruct.unpack('B', buf)[0]
759 return id
760 except:
761 retry = retry + 1
762 else:
763 raise Exception("i2c read/write error!")
764 elif (self.chip==2):
765 retry = 0
766 if (retry < 5):
767 try:
768 self.i2c.writeto(self.addr, bytearray([0x39]), False)
769 buf = self.i2c.readfrom(self.addr, 1, True)
770 id = ustruct.unpack('B', buf)[0]
771 return id
772 except:
773 retry = retry + 1
774 else:
775 raise Exception("i2c read/write error!")
776
777 def _judge_id(self):
778 """
779 判断product_ID
780 """
781 retry = 0
782 if (retry < 5):
783 try:
784 self.i2c.writeto(self.addr, bytearray([0x39]), False)
785 buf = self.i2c.readfrom(self.addr, 1, True)
786 id = ustruct.unpack('B', buf)[0]
787 if(id == 16):
788 self.chip = 2
789 self.product_ID = 16
790 else:
791 self.chip = 1
792 self.product_ID = 48
793 except:
794 retry = retry + 1
795 else:
796 raise Exception("i2c read/write error!")
797
798# Magnetic
799if 48 in i2c.scan():
800 magnetic = Magnetic()
801else:
802 print("Magnetic sensor not found.")
803
804class PinMode(object):
805 IN = 1
806 OUT = 2
807 PWM = 3
808 ANALOG = 4
809 OUT_DRAIN = 5
810
811# P5: A P11: B
812# P7: RGB LED
813# P10: sound
814# P12: buzzer
815# P0 P1 P2 P3 P4 P5 P6 P7 P8 P9 P10 P11 P12 P13 P14 P15 P16 P19 P20 P21 P23 P24 P25 P26 P27 P28
816# * * * * * * * * * scl sda * * * * * * *
817pins_remap_esp32 = (1, 2, 3, 4, 5, 0, 7, 16, 8, 9, 6, 46, 21, 17, 18, 48, 47, -1, -1, 43, 44, 45, 15, 10, 11, 12, 13, 14)
818
819class MPythonPin():
820 def __init__(self, pin, mode=PinMode.IN, pull=None):
821 if mode not in [PinMode.IN, PinMode.OUT, PinMode.PWM, PinMode.ANALOG, PinMode.OUT_DRAIN]:
822 raise TypeError("mode must be 'IN, OUT, PWM, ANALOG,OUT_DRAIN'")
823 if pin == 10:
824 raise TypeError("P10 is used for internal sound sensor")
825 if pin == 5 or pin == 11:
826 raise TypeError("P5 or P11 is used for internal A B key.")
827 if pin == 7:
828 raise TypeError("P7 is used for internal RGB LED.")
829 if pin == 12:
830 raise TypeError("P12 is used for internal buzzer.")
831 try:
832 self.id = pins_remap_esp32[pin]
833 except IndexError:
834 raise IndexError("Out of Pin range")
835 if mode == PinMode.IN:
836 if pin not in [0, 1, 2, 3, 4, 6, 8, 9]:
837 raise TypeError('IN not supported on P%d' % pin)
838 self.Pin = Pin(self.id, Pin.IN, pull)
839 if mode == PinMode.OUT:
840 if pin not in [0, 1, 2, 3, 4, 6, 8, 9]:
841 raise TypeError('OUT not supported on P%d' % pin)
842 self.Pin = Pin(self.id, Pin.IN, pull)
843 time.sleep_ms(1)
844 self.Pin = Pin(self.id, Pin.OUT, pull)
845 if mode == PinMode.OUT_DRAIN:
846 if pin not in [0, 1, 2, 3, 4, 6, 8, 9]:
847 raise TypeError('OUT_DRAIN not supported on P%d' % pin)
848 self.Pin = Pin(self.id, Pin.IN, pull)
849 time.sleep_ms(1)
850 self.Pin = Pin(self.id, Pin.OPEN_DRAIN, pull)
851 if mode == PinMode.PWM:
852 if pin not in [0, 1, 2, 3, 4, 6, 8, 9]:
853 raise TypeError('PWM not supported on P%d' % pin)
854 self.Pin = Pin(self.id, Pin.IN, pull)
855 time.sleep_ms(1)
856 self.pwm = PWM(Pin(self.id), duty=0)
857 if mode == PinMode.ANALOG:
858 if pin not in [0, 1, 2, 3, 4, 6, 8, 9]:
859 raise TypeError('ANALOG not supported on P%d' % pin)
860 self.adc = ADC(Pin(self.id))
861 self.adc.atten(ADC.ATTN_11DB)
862 self.mode = mode
863
864 def irq(self, handler=None, trigger=Pin.IRQ_RISING):
865 if not self.mode == PinMode.IN:
866 raise TypeError('the pin is not in IN mode')
867 return self.Pin.irq(handler, trigger)
868
869 def read_digital(self):
870 if not self.mode == PinMode.IN:
871 raise TypeError('the pin is not in IN mode')
872 return self.Pin.value()
873
874 def write_digital(self, value):
875 if self.mode not in [PinMode.OUT, PinMode.OUT_DRAIN]:
876 raise TypeError('the pin is not in OUT or OUT_DRAIN mode')
877 self.Pin.value(value)
878
879 def read_analog(self):
880 if not self.mode == PinMode.ANALOG:
881 raise TypeError('the pin is not in ANALOG mode')
882 return self.adc.read()
883
884
885 def write_analog(self, duty, freq=1000):
886 if not self.mode == PinMode.PWM:
887 raise TypeError('the pin is not in PWM mode')
888 self.pwm.freq(freq)
889 self.pwm.duty(duty)
890
891
892
893# 3 rgb leds
894rgb = NeoPixel(Pin(16, Pin.OUT), 3, 3, 1, brightness=0.3)
895rgb.write()
896
897# light sensor LTR-308ALS
898if 83 in i2c.scan():
899 light = LTR_308ALS(i2c)
900else:
901 print("light sensor not found!")
902
903# sound sensor
904sound = ADC(Pin(6))
905sound.atten(sound.ATTN_11DB)
906
907# buttons
908class Button:
909 def __init__(self, pin_num, reverse=False):
910 self.__reverse = reverse
911 (self.__press_level, self.__release_level) = (0, 1) if not self.__reverse else (1, 0)
912 self.__pin = Pin(pin_num, Pin.IN, pull=Pin.PULL_UP)
913 self.__pin.irq(trigger=Pin.IRQ_FALLING | Pin.IRQ_RISING, handler=self.__irq_handler)
914 # self.__user_irq = None
915 self.event_pressed = None
916 self.event_released = None
917 self.__pressed_count = 0
918 self.__was_pressed = False
919 # print("level: pressed is {}, released is {}." .format(self.__press_level, self.__release_level))
920
921
922 def __irq_handler(self, pin):
923 irq_falling = True if pin.value() == self.__press_level else False
924 # debounce
925 time.sleep_ms(10)
926 if self.__pin.value() == (self.__press_level if irq_falling else self.__release_level):
927 # new event handler
928 # pressed event
929 if irq_falling:
930 if self.event_pressed is not None:
931 schedule(self.event_pressed, self.__pin)
932 # key status
933 self.__was_pressed = True
934 if (self.__pressed_count < 100):
935 self.__pressed_count = self.__pressed_count + 1
936 # release event
937 else:
938 if self.event_released is not None:
939 schedule(self.event_released, self.__pin)
940
941
942 def is_pressed(self):
943 if self.__pin.value() == self.__press_level:
944 return True
945 else:
946 return False
947
948 def was_pressed(self):
949 r = self.__was_pressed
950 self.__was_pressed = False
951 return r
952
953 def get_presses(self):
954 r = self.__pressed_count
955 self.__pressed_count = 0
956 return r
957
958 def value(self):
959 return self.__pin.value()
960
961 def irq(self, *args, **kws):
962 self.__pin.irq(*args, **kws)
963
964 def status(self):
965 val = self.__pin.value()
966 if(val==0):
967 return 1
968 elif(val==1):
969 return 0
970
971button_a = Button(0)
972button_b = Button(46)
973
974from touchpad import TouchPad
975
976# touchpad
977touchpad_p = touchPad_P = TouchPad(5)
978touchpad_y = touchPad_Y = TouchPad(4)
979touchpad_t = touchPad_T = TouchPad(3)
980touchpad_h = touchPad_H = TouchPad(2)
981touchpad_o = touchPad_O = TouchPad(1)
982touchpad_n = touchPad_N = TouchPad(0)
983
984# motor controller
985
986class Ledong_shield(object):
987 def __init__(self):
988 self.speed = 0
989 self.i2c = i2c
990 self.i2c_addr = 17
991
992 def set_motor(self, motor_num, speed):
993 self.speed = speed
994 if self.speed > 100:
995 self.speed = 100
996 if self.speed < -100:
997 self.speed = -100
998 self.i2c.writeto(self.i2c_addr, bytearray([motor_num, self.speed]), True)
999
1000 def power_off(self):
1001 self.i2c.writeto(self.i2c_addr, b'\x06\x01', True)
1002
1003 def get_battery_level(self):
1004 self.i2c.writeto(self.i2c_addr, b'\x03', True)
1005 tmp = self.i2c.readfrom(self.i2c_addr, 2)
1006 data = tmp[1] << 8 + tmp[0]
1007 data = max(min(data, 4200), 3300)
1008 return data
1009
1010if 17 in i2c.scan():
1011 ledong_shield = Ledong_shield()
1012else:
1013 print("Ledong shield not found!")
1014
1015from gui import *
1016
1017"""
1018uuid
1019"""
1020def uuid():
1021 import ubinascii
1022 import machine
1023 uuid = ''
1024 try:
1025 uuid = ubinascii.hexlify(machine.unique_id_custom()).decode().upper()
1026 except Exception as e:
1027 uuid = ubinascii.hexlify(machine.unique_id()).decode().upper()
1028
1029 if(uuid=='ffffffffffff'.upper() or uuid=='000000000000'.upper()):
1030 uuid = ubinascii.hexlify(machine.unique_id()).decode().upper()
1031
1032 return uuid