AutoDrive/control/keyControl.py at dev · laserprec/AutoDrive · 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
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
import pygame
from pygame.locals import *
from motor import Motor
from servo import Servo

SCREEN_WIDTH, SCREEN_HEIGHT = 230, 180
FONT, FONT_SIZE             = 'Comic Sans MS', 30
BLACK, WHITE                = (0, 0, 0), (250, 250, 250)

LEFT_KEY, UP_KEY, RIGHT_KEY, DOWN_KEY = 276, 273, 275, 274
MARGIN                      = 20
PADDING                     = 5
KEY_REC_LEN                 = 60
KEY_REC_BORDER              = 1

MOTOR_PIN = 13
SERVO_PIN = 19

# Initial Setup
pygame.init()
text = pygame.font.SysFont(FONT, FONT_SIZE)
screen = pygame.display.set_mode((SCREEN_WIDTH, SCREEN_HEIGHT), 0)

def setup():
    screen.fill(BLACK)
    pygame.display.update()
    return Motor(MOTOR_PIN), Servo(SERVO_PIN)

def printPressedKey(key):
    textSurface = text.render("Pressed {}".format(key), True, WHITE)
    screen.fill(BLACK)
    screen.blit(textSurface, (100, 100))
    pygame.display.flip()

def screenRefresh():
    screen.fill(BLACK)
    pygame.display.flip()

def drawKeyBoard(left, top):
    # left key
    leftKeyRec = (left + MARGIN, top + MARGIN + KEY_REC_LEN + PADDING, KEY_REC_LEN, KEY_REC_LEN)
    pygame.draw.rect(screen, WHITE, leftKeyRec, KEY_REC_BORDER)
    # down key
    downKeyRec = (left + MARGIN + KEY_REC_LEN + PADDING,top + MARGIN + KEY_REC_LEN + PADDING, KEY_REC_LEN, KEY_REC_LEN)
    pygame.draw.rect(screen, WHITE, downKeyRec, KEY_REC_BORDER)
    # up key
    upKeyRec = (left + MARGIN + KEY_REC_LEN + PADDING,top + MARGIN, KEY_REC_LEN, KEY_REC_LEN)
    pygame.draw.rect(screen, WHITE, upKeyRec, KEY_REC_BORDER)
    # right key
    rightKeyRec = (left + MARGIN + 2 * (KEY_REC_LEN + PADDING),top + MARGIN + KEY_REC_LEN + PADDING, KEY_REC_LEN, KEY_REC_LEN)
    pygame.draw.rect(screen, WHITE, rightKeyRec, KEY_REC_BORDER)

    pygame.display.update()
    return (leftKeyRec, rightKeyRec, upKeyRec, downKeyRec)

def lightKeyUp(rec):
    pygame.draw.rect(screen, WHITE, rec)
    pygame.display.update()

def main():
    motor, servo = setup()
    forward = False
    backward = False
    turnLeft = False
    turnRight = False
    while 1:
        (leftKeyRec, rightKeyRec, upKeyRec, downKeyRec) = drawKeyBoard(0,0)
        if forward:
            lightKeyUp(upKeyRec)
            motor.moveForward()
        if backward:
            lightKeyUp(downKeyRec)
        if turnLeft:
            servo.left()
        elif turnRight:
            servo.right()
        else:
            servo.neutral()

        for event in pygame.event.get():
            screenRefresh()

            if event.type == pygame.QUIT:
                pygame.quit()
                quit()
            elif event.type == pygame.KEYDOWN:
                print(event.key, 'pressed')
                # printPressedKey(event.key)
                if event.key == LEFT_KEY:
                    turnLeft = True
                    lightKeyUp(leftKeyRec)
                elif event.key == RIGHT_KEY:
                    turnRight = True
                    lightKeyUp(rightKeyRec)
                elif event.key == UP_KEY:
                    forward = True
                    lightKeyUp(upKeyRec)
                elif event.key == DOWN_KEY:
                    backward = True
                    lightKeyUp(downKeyRec)
                else:
                    pass
            elif event.type == pygame.KEYUP:
                print(event.key, 'released')
                if event.key == UP_KEY:
                    motor.stop()
                    forward = False
                    screenRefresh()
                    pygame.draw.rect(screen, WHITE, upKeyRec, KEY_REC_BORDER)

                elif event.key == DOWN_KEY:
                    backward = False
                    screenRefresh()
                    pygame.draw.rect(screen, WHITE, downKeyRec, KEY_REC_BORDER)

                elif event.key == LEFT_KEY:
                    turnLeft = False
                elif event.key == RIGHT_KEY:
                    turnRight = False

                else:
                    pass
            else:
                pass

if __name__ == '__main__':
    main()