color.py 3.48 KB
Newer Older
Phyks's avatar
Phyks committed
1
2
3
4
5
#!/bin/env python

import serial
import pygame

Phyks's avatar
Phyks committed
6

7
def compute_value(line, minimum, maximum):
Phyks's avatar
Phyks committed
8
9
10
    if len(line) < 3:
        return False

Phyks's avatar
Phyks committed
11
    value = [0, 0, 0]
Phyks's avatar
Phyks committed
12
    for i in range(3):
Phyks's avatar
Phyks committed
13
14
15
16
17
        if maximum[i]-minimum[i] != 0:
            value[i] = int(255*(line[i]-minimum[i])/(maximum[i]-minimum[i]))
        else:
            value[i] = 0

Phyks's avatar
Phyks committed
18
19
20
21
22
        if value[i] > 255:
            value[i] = 255
        elif value[i] < 0:
            value[i] = 0

Phyks's avatar
Phyks committed
23
    return value
Phyks's avatar
Phyks committed
24
25


Phyks's avatar
Phyks committed
26
27
28
ser = serial.Serial("/dev/ttyACM0", 115200)
pygame.init()

29
30
31
32
# Keep old value to determine a mean value
value = [[0, 0, 0], [0, 0, 0]]
font = pygame.font.Font(None, 36)

Phyks's avatar
Phyks committed
33
34
size = width, height = 640, 480
screen = pygame.display.set_mode(size)
Phyks's avatar
Phyks committed
35
pygame.display.set_caption("Touchless 3D tracking")
Phyks's avatar
Phyks committed
36
screen.fill((0, 0, 0))
Phyks's avatar
Phyks committed
37
38
39

try:
    ser.open()
Phyks's avatar
Phyks committed
40
41
except Exception as e:
    print("Error while opening serial port : "+str(e))
Phyks's avatar
Phyks committed
42
43
44
45
46
47

if ser.isOpen():
    try:
        ser.flushInput()
        ser.flushOutput()

Phyks's avatar
Phyks committed
48
49
        maximum = [-1, -1, -1]
        minimum = [-1, -1, -1]
Phyks's avatar
Phyks committed
50
51

        print("Calibration :")
Phyks's avatar
Phyks committed
52
        print("Press any key to launch the program when calibration is " +
Phyks's avatar
Phyks committed
53
54
55
56
57
58
59
60
              "finished.")
        # Display info in window
        label = font.render("Calibration...", 1, (255, 255, 255))
        label_pos = label.get_rect()
        label_pos.centerx = screen.get_rect().centerx
        label_pos.centery = 20
        screen.blit(label, label_pos)

Phyks's avatar
Phyks committed
61
62
        pygame.display.flip()

Phyks's avatar
Phyks committed
63
64
65
        running = True
        while running:
            for event in pygame.event.get():
Phyks's avatar
Phyks committed
66
                if event.type == pygame.KEYDOWN:
Phyks's avatar
Phyks committed
67
                    running = False
Phyks's avatar
Phyks committed
68
                    continue
Phyks's avatar
Phyks committed
69

Phyks's avatar
Phyks committed
70
71
72
73
            line = ser.readline()
            line = line.decode().strip("\r\n")
            line = line.split(" ")
            line = [int(j or 0) for j in line]
Phyks's avatar
Phyks committed
74

Phyks's avatar
Phyks committed
75
76
77
78
79
80
81
            for i in range(3):
                if line[i] < minimum[i] or minimum[i] < 0:
                    minimum[i] = line[i]
                if line[i] > maximum[i]:
                    maximum[i] = line[i]

            print(line)
Phyks's avatar
Phyks committed
82

Phyks's avatar
Phyks committed
83
        print("Running...")
Phyks's avatar
Phyks committed
84
85
86
87
        running = True
        while running:
            # Quit if window is closed
            for event in pygame.event.get():
Phyks's avatar
Phyks committed
88
89
                if event.type == pygame.QUIT or event.type == pygame.KEYDOWN:
                    pygame.quit()
Phyks's avatar
Phyks committed
90
                    running = False
Phyks's avatar
Phyks committed
91
                    continue
Phyks's avatar
Phyks committed
92
93

            # Read line from serial
Phyks's avatar
Phyks committed
94
95
96
97
98
            line = ser.readline()
            line = line.decode().strip("\r\n")
            line = line.split(" ")
            line = [int(j or 0) for j in line]

Phyks's avatar
Phyks committed
99
            # Compute the value for red
100
101
            value[1] = value[0]
            value[0] = compute_value(line, minimum, maximum)
Phyks's avatar
Phyks committed
102
            value_bg = [(value[0][i] + value[1][i])/2 for i in range(3)]
103
104
            value_text = [255 - i for i in compute_value(line, minimum,
                                                         maximum)]
Phyks's avatar
Phyks committed
105

Phyks's avatar
Phyks committed
106
            if value is not False:
107
                screen.fill((value_bg[0], value_bg[1], value_bg[2]))
Phyks's avatar
Phyks committed
108
109

                # Display info in window
110
111
                label = font.render("Running...", 1, (value_text[0],
                                    value_text[1], value_text[2]))
Phyks's avatar
Phyks committed
112
113
114
115
116
                label_pos = label.get_rect()
                label_pos.centerx = screen.get_rect().centerx
                label_pos.centery = 20
                screen.blit(label, label_pos)

Phyks's avatar
Phyks committed
117
                pygame.display.flip()
Phyks's avatar
Phyks committed
118
119

        ser.close()
Phyks's avatar
Phyks committed
120
121
    except Exception as e:
        print("Error while fetching data from serial : "+str(e))