第十七届智能车竞赛比赛系统软件修改-多车组时间延迟 §01 比赛计时系统

89 阅读 0 评论 59 点赞

我是靠谱客的博主花痴哈密瓜，这篇文章主要介绍第十七届智能车竞赛比赛系统软件修改-多车组时间延迟 §01 比赛计时系统，现在分享给大家，希望可以做个参考。

本文记载了用于第十七届全国大学生智能车竞赛竞赛的比赛系统软件修改过程。

§01 比赛计时系统

一、背景介绍

在十七届智能车竞速组 包含了多车组的比赛，这个组别在计时方面的要求比较特殊，需要对 原来基于ESP32比赛裁判系统 的上位机软件进行修改，使其能够适应多车组的比赛要求。

▲ 图1.1.1 计时器模块以及线圈

▲ 图1.1.1 计时器模块以及线圈

1、计时器硬件

计时器硬件包括有基于 ESP32 硬件电路模块，触发线圈等。使用该模块需要安装 CH340 USB 驱动软件 。

安装之后，将硬件模块通过 Type-C USB 接线接入计算机 USB 接口。如果是 Windows7 操作系统，可以在计算机惯例-设备管理-端口 中看到 USB-SERIAL CH340 (COM n) 。其中 COMn 是计算机分配给 USB 窗口号。建议将 COM 号修改到 1 ~ 8 之间。
▲ 图1.1.2 修改USB串口号

2、计时器软件

下面是存储 2021年7月8日的 ESP32 Python 软件。

D:zhuoqingDesignCenterSmartCar2021LongQiuESP32-Judge System

下面是定义的程序下载接口。利用Thonny de Firware Installer 更新软件，需要把除了VCC之外，其它都与下载器的五根线相连。

▲ 图1.1.3 调试接口定义

▲ 图1.1.3 调试接口定义

▲ 图1.1.4 更新ESP32 Fireware

▲ 图1.1.4 更新ESP32 Fireware

二、程序升级

复制代码

from machine                import UART,Pin,Timer,ADC,PWM
import time
import machine, math

machine.freq(240000000)

adc1 = ADC(Pin(36))
adc2 = ADC(Pin(39))
adc3 = ADC(Pin(34))
adc4 = ADC(Pin(35))

adc1.atten(ADC.ATTN_6DB)
adc2.atten(ADC.ATTN_6DB)
adc3.atten(ADC.ATTN_6DB)
adc4.atten(ADC.ATTN_6DB)

button = Pin(32, Pin.IN, Pin.PULL_UP)
sw4 = Pin(15,   Pin.IN, Pin.PULL_UP)
sw3 = Pin(2,    Pin.IN, Pin.PULL_UP)
sw2 = Pin(19,   Pin.IN, Pin.PULL_UP)
sw1 = Pin(4,    Pin.IN, Pin.PULL_UP)
led1 = Pin(5, Pin.OUT)
led2 = Pin(18, Pin.OUT)

led1.off()
led2.off()
bz1 = Pin(21, Pin.OUT)
bz1.off()

gled = Pin(25, Pin.OUT)
rled = Pin(33, Pin.OUT)
speaker = Pin(26, Pin.OUT)

gled.off()
rled.off()
speaker.off()

def sw1234Read():
    workmode = 0
    if sw2.value() == 0: workmode |= 0x1
    if sw3.value() == 0: workmode |= 0x2
    if sw4.value() == 0: workmode |= 0x4

return sw1.value(), workmode

WORKMODE_TIMER      =   0           # Check two line
WORKMODE_NULL1      =   1           #
WORKMODE_NULL2      =   2           #
WORKMODE_NULL3      =   3           #
WORKMODE_FRUIT      =   4           # Target is fruit,  check laser
WORKMODE_ANIMAL     =   5           # Target is Animal, check laser
WORKMODE_DIGIT1     =   6           # Target is digit,  check first pass
WORKMODE_DIGIT2     =   7           # Target is digit,  check first pass

SAMPLE_NUM = const(500)
ad1dim = [0] * SAMPLE_NUM
ad2dim = [0] * SAMPLE_NUM

LASER_THRESHOLD = 50000             # Laser light check threshold

SAMPLE_AVERAGE_LENGTH   = 40
ad3average = [0] * SAMPLE_AVERAGE_LENGTH
ad4average = [0] * SAMPLE_AVERAGE_LENGTH
ad34point = 0
ad3sigma = 0
ad4sigma = 0

AD34_BASE_ALPHA         = 0.0005
ad3baseline = 0
ad4baseline = 0

AD34_CHECK_THRESHOLD_LOW    = 250
AD34_CHECK_THRESHOLD_HIGH   = 500
AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_HIGH
ad3checktime = 0
ad4checktime = 0

sample_mode = 0                     # 0 : sample adc3, adc4

sample_point = 0
stop_flag = 0
total_count = 0

uart1 = UART(2, baudrate=115200, rx=16, tx=17, timeout=10)

DSCMD_NONE          = 0xff
DSCMD_HELLO         = 0x00
DSCMD_INIT          = 0x01
DSCMD_STARTSEND     = 0x10
DSCMD_STOPSEND      = 0x11
DSCMD_BEEP          = 0x12
DSCMD_SENDSNAPSHOT  = 0x13
DSCMD_TURNOFFLIGHT  = 0x14
DSCMD_SETCONTINUECHECK = 0x15
DSCMD_SETBEACONSEQUENCY = 0x16
DSCMD_BEACONSTART   = 0x17
DSCMD_BEACONSTOP    = 0x18
DSCMD_GETBEACONSTATE = 0x19
DSCMD_BEACONINIT    = 0x1a

DSRET_NCHECK        = 0x80
DSRET_CHECK         = 0x81
DSRET_LAST          = 0x82
DSRET_HELLO         = 0x90
DSRET_INIT          = 0x91

SENDFLAG_LAST       = 0x82
SENDFLAG_CHECK      = 0x81
SENDFLAG_NOCHECK    = 0x80
SENDFLAG_STOP       = 0xff
SENDFLAG_SNAPSHOT   = 0x83

def receCmd():
    if uart1.any() == 0: return DSCMD_NONE,0

framebyte = uart1.read(4)
    if len(framebyte) != 4: return DSCMD_NONE, 0

framelist = list(framebyte)
    time = int.from_bytes(framebyte[1:3], 'big')
    sumnum = sum(framelist[0:3]) & 0xff ^ 0xff

if sumnum != framelist[3]: return DSCMD_NONE, 0
    return framelist[0], time

def sendCmd(cmd, time):
    senddim = [0x55,cmd]
    senddim.extend(list(time.to_bytes(4, 'big')))
    sumnum = sum(senddim)&0xff^0xff
    senddim.extend([sumnum])

sendbytes = bytes(senddim)
    uart1.write(sendbytes)

def procCmd(cmd, time):
    global count32,sendflag,delay3s,count32,sendenableflag
    global initflag,lastcount32,snapshot32,speakercount,senddelay,sendcount

if cmd == DSCMD_NONE: return

if cmd == DSCMD_HELLO:
        sendCmd(DSRET_HELLO, 0x0)
        return

if cmd == DSCMD_INIT:
        sendCmd(DSRET_INIT, 0x0)
        count32 = 0x0
        delay3s = time
        count3s = time

initflag = 1
        lastcount32 = 0
        snapshot32 = 0

sendcount = 0
        sendenableflag = 1
        sendflag = SENDFLAG_STOP

return

if cmd == DSCMD_STARTSEND:
        sendCmd(SENDFLAG_LAST, lastcount32)
        count3s = delay3s
        sendflag = 0x0
        sendenableflag = 0x1
        senddelay = time
        sendcount = 0

return

if cmd == DSCMD_STOPSEND:
        sendCmd(SENDFLAG_CHECK, count32)
        sendenableflag = 0
        sendflag = 0
        sendcount = 0
        return

if cmd == DSCMD_BEEP:
        speakercount = 500
        return

if cmd in (DSCMD_SETBEACONSEQUENCY,
               DSCMD_BEACONSTART,
               DSCMD_BEACONSTOP,
               DSCMD_GETBEACONSTATE,
               DSCMD_TURNOFFLIGHT,
               DSCMD_BEACONINIT):
        return

print(cmd, time)

speakercount = 0
flash50mscount = 0
flash50inc = 0
resultflag = 0                      # 1 :OK; 2:ERROR; 0:NULL
buzzcount = 00

def resultOK():
    global resultflag, speakercount, flash50mscount, flash50inc

resultflag = 1
    speakercount = 4500
    flash50mscount = 0
    flash50inc = 0

def resultERROR():
    global resultflag, speakercount, flash50mscount, flash50inc

resultflag = 2
    speakercount = 4500
    flash50mscount = 0
    flash50inc = 0

def speaker1ms():
    global resultflag, speakercount, flash50mscount, flash50inc
    global detectflag1, detectflag2, detectcount, buzzcount

if detectflag1 != detectflag2:
        detectcount += 1

if buzzcount > 0:
        buzzcount -= 1

if speakercount > 0:
        speakercount -= 1

if speakercount == 0:
            if resultflag > 0:
                resultflag = 0
                rled.off()
                gled.off()

flash50mscount += 1
        if flash50mscount >= 50:
            flash50mscount = 0
            flash50inc += 1

if resultflag == 0:
            speaker.on()
        elif resultflag == 1:
            gled.on()
            speaker.on()
        elif resultflag == 2:
            if flash50inc & 0x1 == 0:
                rled.on()
                speaker.on()
            else:
                rled.off()
                speaker.off()

def sendTime():
    global count32,snapshot32,lastcount32,sendflag

if sendflag == SENDFLAG_STOP:
        return

if initflag == 0:               return

if keepWorkMode > 0:
        sendCmd(sendflag, snapshot32)
        sendflag = SENDFLAG_STOP
        return

timesend = count32
    if sendflag == SENDFLAG_LAST:
        timesend = lastcount32
    elif sendflag == SENDFLAG_CHECK or sendflag == SENDFLAG_NOCHECK:
        timesend = count32
    elif sendflag == SENDFLAG_SNAPSHOT:
        timesend = snapshot32
        sendCmd(SENDFLAG_SNAPSHOT, snapshot32)
        sendflag = SENDFLAG_NOCHECK
    elif keepWorkMode > 0:
        timesend = count32
        sendCmd(SENDFLAG_SNAPSHOT, snapshot32)
        sendflag = SENDFLAG_NOCHECK

sendCmd(sendflag, timesend)
    sendflag = SENDFLAG_STOP

count32 = 0                         # Global 1ms counter
sendflag = SENDFLAG_STOP            # send flag
sendenableflag = 0                  # Enable send
senddelay = 100                     # Everay send period
sendcount = 0                       # count for send delay
delay3s = 3000                      # No check delay
count3s = 3000                      # count for no check delay
initflag = 0
lastcount32 = 0
snapshot32 = 0

detectflag1 = 0                     # Stand for line1 check
detectflag2 = 0                     # Stand for line2 check
detectcount = 0                     # Count the detectflag1 detectflag2 no equal

def ADC4Sample(_):
    global count32,sendenableflag,sendflag,senddelay,sendcount
    global delay3s,count3s,initflag,lastcount32,snapshot32
    global speakercount, keepWorkMode
    global detectflag1, detectflag2, buzzcount

count32 += 1
    speaker1ms()

if keepWorkMode == 0:
        if count32 & 0x100:
            led1.on()
        else: led1.off()
    else:
        if count32 & 0x80:
            led1.on()
        else: led1.off()

if sendenableflag != 0:
        sendcount += 1
        if sendcount >= senddelay:
            sendcount = 0
            if sendflag == SENDFLAG_STOP:
                if count32 < delay3s:
                    sendflag = SENDFLAG_NOCHECK
                else: sendflag = SENDFLAG_CHECK
    else:
        sendcount += 1
        if sendcount >= senddelay:
            sendcount = 0
            if sendflag == SENDFLAG_STOP:
                sendflag = SENDFLAG_CHECK

global ad1dim,ad2dim
    global sample_point
    global adc1,adc2,adc3, adc4
    global ad3average, ad4average, ad34point, ad3sigma, ad4sigma

global ad3checktime, ad4checktime,total_count
    global ad3baseline, ad4baseline

total_count += 1

detectflag = 0

if sample_mode == 1:
        ad1dim[sample_point] = adc1.read()
        ad2dim[sample_point] = adc2.read()

if sample_mode == 0 or sample_mode == 1:
        adc = adc3.read()
        ad3sigma += adc
        ad3sigma -= ad3average[ad34point]
        ad3average[ad34point] = adc

adc = adc4.read()
        ad4sigma += adc
        ad4sigma -= ad4average[ad34point]
        ad4average[ad34point] = adc
        ad34point += 1
        if ad34point >= SAMPLE_AVERAGE_LENGTH:
            ad34point = 0

value = ad3sigma / SAMPLE_AVERAGE_LENGTH
        if ad3baseline == 0:
            if ad34point == SAMPLE_AVERAGE_LENGTH - 1:
                ad3baseline = value
        else: ad3baseline = ad3baseline * (1 - AD34_BASE_ALPHA) +AD34_BASE_ALPHA * value

if abs(value - ad3baseline) > AD34_CHECK_THRESHOLD:
            if ad3checktime == 0:
                ad3checktime = total_count
            detectflag = 1
            detectflag1 = 1
            if keepWorkMode == 6 or keepWorkMode == 7:
                gled.on()

value = ad4sigma / SAMPLE_AVERAGE_LENGTH
        if ad4baseline == 0:
            if ad34point == SAMPLE_AVERAGE_LENGTH - 1:
                ad4baseline = value
        else: ad4baseline = ad4baseline * (1 - AD34_BASE_ALPHA) + AD34_BASE_ALPHA * value

if abs(value - ad4baseline) > AD34_CHECK_THRESHOLD:
            if ad4checktime == 0:
                ad4checktime = total_count
            detectflag = 1
            detectflag2 = 1

if keepWorkMode == 6 or keepWorkMode == 7:
                rled.on()

sample_point += 1
    if sample_point >= SAMPLE_NUM:
        sample_point = 0

if detectflag > 0:
        buzzcount = 100
        bz1.on()

if count3s >= delay3s:
        if sendenableflag > 0:
            if detectflag > 0:
                count3s = 1
                lastcount32 = count32
                snapshot32 = coutn32
                if keepWorkMode == 0:
                    count32 = 0
                sendflag = SENDFLAG_LAST
        else:
            if detectflag > 0:
                snapshot32 = count32
            count3s = 1

else:
        if detectflag > 0:
            snapshot32 = count32
        count3s += 1

time0 = Timer(0)
time0.init(period=1, mode=Timer.PERIODIC, callback=ADC4Sample)

showcount = 0

keepThreshold,keepWorkMode = sw1234Read()
def initDevice():
    global sample_mode, detectflag1, detectflag2, detectcount
    global speakercount, flash50mscount, flash50inc, resultflag, buzzcount

speakercount = 0
    buzzcount = 50
    flash50mscount = 0
    flash50inc = 0
    resultflag = 0                      # 1 :OK; 2:ERROR; 0:NULL

detectflag1 = 0
    detectflag2 = 0
    detectcount = 0

print("Initialize device:%d,%d"%(keepThreshold, keepWorkMode))

if keepThreshold == 1:
        AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_LOW
    else:
        AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_HIGH

led1.off()
    led2.off()
    bz1.off()

gled.off()
    rled.off()
    speaker.off()

if keepWorkMode in (4, 5):
        sample_mode = 1             # Sample adc1,2,3,4
    else: sample_mode = 0           # Sample adc3,4

SAMPLE_PERIOD = 1
FREQUENCY_MOD           = 125

def angle1(n):
    return n*2*3.1415926*SAMPLE_PERIOD / 1000.0 * FREQUENCY_MOD

def wval(w):
    if w < SAMPLE_NUM / 2:
        return w * 2 / SAMPLE_NUM
    else: return (SAMPLE_NUM - w) * 2 / SAMPLE_NUM

cosdim = [int(math.sin(angle1(a)) * wval(a) * 0x7ff) for a in range(SAMPLE_NUM)]
sindim = [int(math.cos(angle1(a)) * wval(a) * 0x7ff) for a in range(SAMPLE_NUM)]

def sample_amp(s):
    global cosdim,sindim,sample_point

cos_sam = 0
    sin_sam = 0

scopy = s.copy()

if sample_point > 0:
        scopy = scopy[sample_point:] + scopy[:sample_point]

cos_sum = sum([s*w for s,w in zip(scopy,cosdim)]) / SAMPLE_NUM
    sin_sum = sum([s*w for s,w in zip(scopy,sindim)]) / SAMPLE_NUM

return math.sqrt(cos_sum**2 + sin_sum**2)

initDevice()

while True:
    threshold, workmode = sw1234Read()
    if threshold != keepThreshold or workmode != keepWorkMode:
        keepThreshold = threshold
        keepWorkMode = workmode
        initDevice()

if button.value() == 0:
        time.sleep(0.01)
        if button.value() == 0:
            initDevice()
            while button.value() == 0:
                speakercount = 10

time.sleep(.01)

if keepWorkMode == 4:           # Fruit mode
        check1 = sample_amp(ad1dim)
        check2 = sample_amp(ad2dim)

print((check1, check2))

if check1 > LASER_THRESHOLD:
            if resultflag == 0:
                resultOK()

detectflag1 = 0
            detectflag2 = 0

if check2 > LASER_THRESHOLD:
            if resultflag == 0:
                resultERROR()
            detectflag1 = 0
            detectflag2 = 0

if detectcount > 10000:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0
            bz1.on()
            buzzcount = 200

if detectflag1 > 0 and detectflag2 > 0:
            if resultflag == 0:
                resultERROR()

detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

if keepWorkMode == 5:           # Animal mode
        check1 = sample_amp(ad1dim)
        check2 = sample_amp(ad2dim)

if check1 > LASER_THRESHOLD:
            resultERROR()

if check2 > LASER_THRESHOLD:
            resultERROR()

if detectcount > 6000:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0
            bz1.on()
            buzzcount = 200
        elif detectcount > 3000:
            bz1.on()
            buzzcount = 50

if detectflag1 > 0 and detectflag2 > 0:
            if detectcount < 2500:
                resultERROR()
            else: resultOK()

detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

if resultflag != 0:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

if speakercount == 0:
        speaker.off()

if buzzcount == 0:
        bz1.off()

showcount += 1
    if showcount > 50:
        showcount = 0

cmd,tm = receCmd()
    if cmd != DSCMD_NONE:
        led2.on()
        procCmd(cmd, tm)
        led2.off()

sendTime()

time.sleep_ms(1)

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
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
from machine                import UART,Pin,Timer,ADC,PWM
import time
import machine, math

machine.freq(240000000)

adc1 = ADC(Pin(36))
adc2 = ADC(Pin(39))
adc3 = ADC(Pin(34))
adc4 = ADC(Pin(35))

adc1.atten(ADC.ATTN_6DB)
adc2.atten(ADC.ATTN_6DB)
adc3.atten(ADC.ATTN_6DB)
adc4.atten(ADC.ATTN_6DB)

button = Pin(32, Pin.IN, Pin.PULL_UP)
sw4 = Pin(15,   Pin.IN, Pin.PULL_UP)
sw3 = Pin(2,    Pin.IN, Pin.PULL_UP)
sw2 = Pin(19,   Pin.IN, Pin.PULL_UP)
sw1 = Pin(4,    Pin.IN, Pin.PULL_UP)
led1 = Pin(5, Pin.OUT)
led2 = Pin(18, Pin.OUT)

led1.off()
led2.off()
bz1 = Pin(21, Pin.OUT)
bz1.off()

gled = Pin(25, Pin.OUT)
rled = Pin(33, Pin.OUT)
speaker = Pin(26, Pin.OUT)

gled.off()
rled.off()
speaker.off()

def sw1234Read():
    workmode = 0
    if sw2.value() == 0: workmode |= 0x1
    if sw3.value() == 0: workmode |= 0x2
    if sw4.value() == 0: workmode |= 0x4

    return sw1.value(), workmode

WORKMODE_TIMER      =   0           # Check two line
WORKMODE_NULL1      =   1           #
WORKMODE_NULL2      =   2           #
WORKMODE_NULL3      =   3           #
WORKMODE_FRUIT      =   4           # Target is fruit,  check laser
WORKMODE_ANIMAL     =   5           # Target is Animal, check laser
WORKMODE_DIGIT1     =   6           # Target is digit,  check first pass
WORKMODE_DIGIT2     =   7           # Target is digit,  check first pass

SAMPLE_NUM = const(500)
ad1dim = [0] * SAMPLE_NUM
ad2dim = [0] * SAMPLE_NUM

LASER_THRESHOLD = 50000             # Laser light check threshold

SAMPLE_AVERAGE_LENGTH   = 40
ad3average = [0] * SAMPLE_AVERAGE_LENGTH
ad4average = [0] * SAMPLE_AVERAGE_LENGTH
ad34point = 0
ad3sigma = 0
ad4sigma = 0

AD34_BASE_ALPHA         = 0.0005
ad3baseline = 0
ad4baseline = 0

AD34_CHECK_THRESHOLD_LOW    = 250
AD34_CHECK_THRESHOLD_HIGH   = 500
AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_HIGH
ad3checktime = 0
ad4checktime = 0

sample_mode = 0                     # 0 : sample adc3, adc4

sample_point = 0
stop_flag = 0
total_count = 0

uart1 = UART(2, baudrate=115200, rx=16, tx=17, timeout=10)

DSCMD_NONE          = 0xff
DSCMD_HELLO         = 0x00
DSCMD_INIT          = 0x01
DSCMD_STARTSEND     = 0x10
DSCMD_STOPSEND      = 0x11
DSCMD_BEEP          = 0x12
DSCMD_SENDSNAPSHOT  = 0x13
DSCMD_TURNOFFLIGHT  = 0x14
DSCMD_SETCONTINUECHECK = 0x15
DSCMD_SETBEACONSEQUENCY = 0x16
DSCMD_BEACONSTART   = 0x17
DSCMD_BEACONSTOP    = 0x18
DSCMD_GETBEACONSTATE = 0x19
DSCMD_BEACONINIT    = 0x1a

DSRET_NCHECK        = 0x80
DSRET_CHECK         = 0x81
DSRET_LAST          = 0x82
DSRET_HELLO         = 0x90
DSRET_INIT          = 0x91

SENDFLAG_LAST       = 0x82
SENDFLAG_CHECK      = 0x81
SENDFLAG_NOCHECK    = 0x80
SENDFLAG_STOP       = 0xff
SENDFLAG_SNAPSHOT   = 0x83

def receCmd():
    if uart1.any() == 0: return DSCMD_NONE,0

    framebyte = uart1.read(4)
    if len(framebyte) != 4: return DSCMD_NONE, 0

    framelist = list(framebyte)
    time = int.from_bytes(framebyte[1:3], 'big')
    sumnum = sum(framelist[0:3]) & 0xff ^ 0xff

    if sumnum != framelist[3]: return DSCMD_NONE, 0
    return framelist[0], time

def sendCmd(cmd, time):
    senddim = [0x55,cmd]
    senddim.extend(list(time.to_bytes(4, 'big')))
    sumnum = sum(senddim)&0xff^0xff
    senddim.extend([sumnum])

    sendbytes = bytes(senddim)
    uart1.write(sendbytes)

def procCmd(cmd, time):
    global count32,sendflag,delay3s,count32,sendenableflag
    global initflag,lastcount32,snapshot32,speakercount,senddelay,sendcount

    if cmd == DSCMD_NONE: return

    if cmd == DSCMD_HELLO:
        sendCmd(DSRET_HELLO, 0x0)
        return

    if cmd == DSCMD_INIT:
        sendCmd(DSRET_INIT, 0x0)
        count32 = 0x0
        delay3s = time
        count3s = time

        initflag = 1
        lastcount32 = 0
        snapshot32 = 0

        sendcount = 0
        sendenableflag = 1
        sendflag = SENDFLAG_STOP

        return

    if cmd == DSCMD_STARTSEND:
        sendCmd(SENDFLAG_LAST, lastcount32)
        count3s = delay3s
        sendflag = 0x0
        sendenableflag = 0x1
        senddelay = time
        sendcount = 0

        return

    if cmd == DSCMD_STOPSEND:
        sendCmd(SENDFLAG_CHECK, count32)
        sendenableflag = 0
        sendflag = 0
        sendcount = 0
        return

    if cmd == DSCMD_BEEP:
        speakercount = 500
        return

    if cmd in (DSCMD_SETBEACONSEQUENCY,
               DSCMD_BEACONSTART,
               DSCMD_BEACONSTOP,
               DSCMD_GETBEACONSTATE,
               DSCMD_TURNOFFLIGHT,
               DSCMD_BEACONINIT):
        return

    print(cmd, time)

speakercount = 0
flash50mscount = 0
flash50inc = 0
resultflag = 0                      # 1 :OK; 2:ERROR; 0:NULL
buzzcount = 00

def resultOK():
    global resultflag, speakercount, flash50mscount, flash50inc

    resultflag = 1
    speakercount = 4500
    flash50mscount = 0
    flash50inc = 0

def resultERROR():
    global resultflag, speakercount, flash50mscount, flash50inc

    resultflag = 2
    speakercount = 4500
    flash50mscount = 0
    flash50inc = 0

def speaker1ms():
    global resultflag, speakercount, flash50mscount, flash50inc
    global detectflag1, detectflag2, detectcount, buzzcount

    if detectflag1 != detectflag2:
        detectcount += 1

    if buzzcount > 0:
        buzzcount -= 1

    if speakercount > 0:
        speakercount -= 1

        if speakercount == 0:
            if resultflag > 0:
                resultflag = 0
                rled.off()
                gled.off()

        flash50mscount += 1
        if flash50mscount >= 50:
            flash50mscount = 0
            flash50inc += 1

        if resultflag == 0:
            speaker.on()
        elif resultflag == 1:
            gled.on()
            speaker.on()
        elif resultflag == 2:
            if flash50inc & 0x1 == 0:
                rled.on()
                speaker.on()
            else:
                rled.off()
                speaker.off()

def sendTime():
    global count32,snapshot32,lastcount32,sendflag

    if sendflag == SENDFLAG_STOP:
        return

    if initflag == 0:               return

    if keepWorkMode > 0:
        sendCmd(sendflag, snapshot32)
        sendflag = SENDFLAG_STOP
        return

    timesend = count32
    if sendflag == SENDFLAG_LAST:
        timesend = lastcount32
    elif sendflag == SENDFLAG_CHECK or sendflag == SENDFLAG_NOCHECK:
        timesend = count32
    elif sendflag == SENDFLAG_SNAPSHOT:
        timesend = snapshot32
        sendCmd(SENDFLAG_SNAPSHOT, snapshot32)
        sendflag = SENDFLAG_NOCHECK
    elif keepWorkMode > 0:
        timesend = count32
        sendCmd(SENDFLAG_SNAPSHOT, snapshot32)
        sendflag = SENDFLAG_NOCHECK

    sendCmd(sendflag, timesend)
    sendflag = SENDFLAG_STOP

count32 = 0                         # Global 1ms counter
sendflag = SENDFLAG_STOP            # send flag
sendenableflag = 0                  # Enable send
senddelay = 100                     # Everay send period
sendcount = 0                       # count for send delay
delay3s = 3000                      # No check delay
count3s = 3000                      # count for no check delay
initflag = 0
lastcount32 = 0
snapshot32 = 0

detectflag1 = 0                     # Stand for line1 check
detectflag2 = 0                     # Stand for line2 check
detectcount = 0                     # Count the detectflag1 detectflag2 no equal

def ADC4Sample(_):
    global count32,sendenableflag,sendflag,senddelay,sendcount
    global delay3s,count3s,initflag,lastcount32,snapshot32
    global speakercount, keepWorkMode
    global detectflag1, detectflag2, buzzcount

    count32 += 1
    speaker1ms()

    if keepWorkMode == 0:
        if count32 & 0x100:
            led1.on()
        else: led1.off()
    else:
        if count32 & 0x80:
            led1.on()
        else: led1.off()

    if sendenableflag != 0:
        sendcount += 1
        if sendcount >= senddelay:
            sendcount = 0
            if sendflag == SENDFLAG_STOP:
                if count32 < delay3s:
                    sendflag = SENDFLAG_NOCHECK
                else: sendflag = SENDFLAG_CHECK
    else:
        sendcount += 1
        if sendcount >= senddelay:
            sendcount = 0
            if sendflag == SENDFLAG_STOP:
                sendflag = SENDFLAG_CHECK

    global ad1dim,ad2dim
    global sample_point
    global adc1,adc2,adc3, adc4
    global ad3average, ad4average, ad34point, ad3sigma, ad4sigma

    global ad3checktime, ad4checktime,total_count
    global ad3baseline, ad4baseline

    total_count += 1

    detectflag = 0

    if sample_mode == 1:
        ad1dim[sample_point] = adc1.read()
        ad2dim[sample_point] = adc2.read()

    if sample_mode == 0 or sample_mode == 1:
        adc = adc3.read()
        ad3sigma += adc
        ad3sigma -= ad3average[ad34point]
        ad3average[ad34point] = adc

        adc = adc4.read()
        ad4sigma += adc
        ad4sigma -= ad4average[ad34point]
        ad4average[ad34point] = adc
        ad34point += 1
        if ad34point >= SAMPLE_AVERAGE_LENGTH:
            ad34point = 0

        value = ad3sigma / SAMPLE_AVERAGE_LENGTH
        if ad3baseline == 0:
            if ad34point == SAMPLE_AVERAGE_LENGTH - 1:
                ad3baseline = value
        else: ad3baseline = ad3baseline * (1 - AD34_BASE_ALPHA) +AD34_BASE_ALPHA * value

        if abs(value - ad3baseline) > AD34_CHECK_THRESHOLD:
            if ad3checktime == 0:
                ad3checktime = total_count
            detectflag = 1
            detectflag1 = 1
            if keepWorkMode == 6 or keepWorkMode == 7:
                gled.on()

        value = ad4sigma / SAMPLE_AVERAGE_LENGTH
        if ad4baseline == 0:
            if ad34point == SAMPLE_AVERAGE_LENGTH - 1:
                ad4baseline = value
        else: ad4baseline = ad4baseline * (1 - AD34_BASE_ALPHA) + AD34_BASE_ALPHA * value

        if abs(value - ad4baseline) > AD34_CHECK_THRESHOLD:
            if ad4checktime == 0:
                ad4checktime = total_count
            detectflag = 1
            detectflag2 = 1

            if keepWorkMode == 6 or keepWorkMode == 7:
                rled.on()

    sample_point += 1
    if sample_point >= SAMPLE_NUM:
        sample_point = 0

    if detectflag > 0:
        buzzcount = 100
        bz1.on()

    if count3s >= delay3s:
        if sendenableflag > 0:
            if detectflag > 0:
                count3s = 1
                lastcount32 = count32
                snapshot32 = coutn32
                if keepWorkMode == 0:
                    count32 = 0
                sendflag = SENDFLAG_LAST
        else:
            if detectflag > 0:
                snapshot32 = count32
            count3s = 1

    else:
        if detectflag > 0:
            snapshot32 = count32
        count3s += 1

time0 = Timer(0)
time0.init(period=1, mode=Timer.PERIODIC, callback=ADC4Sample)

showcount = 0

keepThreshold,keepWorkMode = sw1234Read()
def initDevice():
    global sample_mode, detectflag1, detectflag2, detectcount
    global speakercount, flash50mscount, flash50inc, resultflag, buzzcount

    speakercount = 0
    buzzcount = 50
    flash50mscount = 0
    flash50inc = 0
    resultflag = 0                      # 1 :OK; 2:ERROR; 0:NULL

    detectflag1 = 0
    detectflag2 = 0
    detectcount = 0

    print("Initialize device:%d,%d"%(keepThreshold, keepWorkMode))

    if keepThreshold == 1:
        AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_LOW
    else:
        AD34_CHECK_THRESHOLD        = AD34_CHECK_THRESHOLD_HIGH

    led1.off()
    led2.off()
    bz1.off()

    gled.off()
    rled.off()
    speaker.off()

    if keepWorkMode in (4, 5):
        sample_mode = 1             # Sample adc1,2,3,4
    else: sample_mode = 0           # Sample adc3,4

SAMPLE_PERIOD = 1
FREQUENCY_MOD           = 125

def angle1(n):
    return n*2*3.1415926*SAMPLE_PERIOD / 1000.0 * FREQUENCY_MOD

def wval(w):
    if w < SAMPLE_NUM / 2:
        return w * 2 / SAMPLE_NUM
    else: return (SAMPLE_NUM - w) * 2 / SAMPLE_NUM

cosdim = [int(math.sin(angle1(a)) * wval(a) * 0x7ff) for a in range(SAMPLE_NUM)]
sindim = [int(math.cos(angle1(a)) * wval(a) * 0x7ff) for a in range(SAMPLE_NUM)]

def sample_amp(s):
    global cosdim,sindim,sample_point

    cos_sam = 0
    sin_sam = 0

    scopy = s.copy()

    if sample_point > 0:
        scopy = scopy[sample_point:] + scopy[:sample_point]

    cos_sum = sum([s*w for s,w in zip(scopy,cosdim)]) / SAMPLE_NUM
    sin_sum = sum([s*w for s,w in zip(scopy,sindim)]) / SAMPLE_NUM

    return math.sqrt(cos_sum**2 + sin_sum**2)

initDevice()

while True:
    threshold, workmode = sw1234Read()
    if threshold != keepThreshold or workmode != keepWorkMode:
        keepThreshold = threshold
        keepWorkMode = workmode
        initDevice()

    if button.value() == 0:
        time.sleep(0.01)
        if button.value() == 0:
            initDevice()
            while button.value() == 0:
                speakercount = 10

            time.sleep(.01)

    if keepWorkMode == 4:           # Fruit mode
        check1 = sample_amp(ad1dim)
        check2 = sample_amp(ad2dim)

        print((check1, check2))

        if check1 > LASER_THRESHOLD:
            if resultflag == 0:
                resultOK()

            detectflag1 = 0
            detectflag2 = 0

        if check2 > LASER_THRESHOLD:
            if resultflag == 0:
                resultERROR()
            detectflag1 = 0
            detectflag2 = 0

        if detectcount > 10000:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0
            bz1.on()
            buzzcount = 200

        if detectflag1 > 0 and detectflag2 > 0:
            if resultflag == 0:
                resultERROR()

            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

    if keepWorkMode == 5:           # Animal mode
        check1 = sample_amp(ad1dim)
        check2 = sample_amp(ad2dim)

        if check1 > LASER_THRESHOLD:
            resultERROR()

        if check2 > LASER_THRESHOLD:
            resultERROR()

        if detectcount > 6000:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0
            bz1.on()
            buzzcount = 200
        elif detectcount > 3000:
            bz1.on()
            buzzcount = 50

        if detectflag1 > 0 and detectflag2 > 0:
            if detectcount < 2500:
                resultERROR()
            else: resultOK()

            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

        if resultflag != 0:
            detectflag1 = 0
            detectflag2 = 0
            detectcount = 0

    if speakercount == 0:
        speaker.off()

    if buzzcount == 0:
        bz1.off()

    showcount += 1
    if showcount > 50:
        showcount = 0

    cmd,tm = receCmd()
    if cmd != DSCMD_NONE:
        led2.on()
        procCmd(cmd, tm)
        led2.off()

    sendTime()

    time.sleep_ms(1)