ADping/data.py at main · hellojoa1202/ADping · 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
import mycamera
import cv2
import time
from gpiozero import DigitalOutputDevice
from gpiozero import PWMOutputDevice

PWMA = PWMOutputDevice(18)
AIN1 = DigitalOutputDevice(22)
AIN2 = DigitalOutputDevice(27)

PWMB = PWMOutputDevice(23)
BIN1 = DigitalOutputDevice(25)
BIN2 = DigitalOutputDevice(24)

def motor_go(speed):
    AIN1.value = 0
    AIN2.value = 1
    PWMA.value = speed
    BIN1.value = 0
    BIN2.value = 1
    PWMB.value = speed

def motor_back(speed):
    AIN1.value = 1
    AIN2.value = 0
    PWMA.value = speed
    BIN1.value = 1
    BIN2.value = 0
    PWMB.value = speed

def motor_left(speed):
    AIN1.value = 0
    AIN2.value = 1
    PWMA.value = speed
    BIN1.value = 1
    BIN2.value = 0
    PWMB.value = 0.0

def motor_right(speed):
    AIN1.value = 0
    AIN2.value = 1
    PWMA.value = speed
    BIN1.value = 0
    BIN2.value = 1
    PWMB.value = 0.0

def motor_stop():
    AIN1.value = 0
    AIN2.value = 0
    PWMA.value = 0.0
    BIN1.value = 0
    BIN2.value = 0
    PWMB.value = 0.0

speedSet = 0.5

def main():
    camera = mycamera.MyPiCamera(640, 480)
    filepath = "/home/pi/AI_CAR/video/train"
    i = 0
    carState = "stop"
    while camera.isOpened():
        keyValue = cv2.waitKey(10)
        if keyValue == ord('q'):
            break
        elif keyValue == 82:  # Up arrow
            print("go")
            carState = "go"
            motor_go(speedSet)
        elif keyValue == 81:  # Left arrow
            print("left")
            carState = "left"
            motor_left(speedSet)
        elif keyValue == 83:  # Right arrow
            print("right")
            carState = "right"
            motor_right(speedSet)
        elif keyValue == 84:  # Down arrow
            print("stop")
            carState = "stop"
            motor_stop()

        _, image = camera.read()
        image = cv2.flip(image, -1)
        cv2.imshow("Original", image)

        height, _, _ = image.shape
        save_image = image[int(height / 2):, :, :]
        save_image = cv2.cvtColor(save_image, cv2.COLOR_BGR2YUV)
        save_image = cv2.GaussianBlur(save_image, (3, 3), 0)
        save_image = cv2.resize(save_image, (200, 66))
        cv2.imshow("Save", save_image)

        if carState == "left":
            cv2.imwrite("%s_%05d_%03d.png" % (filepath, i, 45), save_image)
            i += 1
        elif carState == "right":
            cv2.imwrite("%s_%05d_%03d.png" % (filepath, i, 135), save_image)
            i += 1
        elif carState == "go":
            cv2.imwrite("%s_%05d_%03d.png" % (filepath, i, 90), save_image)
            i += 1

    cv2.destroyAllWindows()

if __name__ == "__main__":
    main()
    PWMA.value = 0.0
    PWMB.value = 0.0