b1aze.py 7.38 KB
Newer Older
Malte Schmitz's avatar
Malte Schmitz committed
1 2 3 4 5
#!/usr/bin/env python
import freenect
import cv2
import frame_convert2

Malte Schmitz's avatar
Malte Schmitz committed
6 7 8 9
import RPi.GPIO as GPIO
from time import sleep
from threading import Lock, Thread

Malte Schmitz's avatar
Malte Schmitz committed
10 11 12 13 14 15
from imutils.video import VideoStream
import numpy as np
import argparse
import imutils
import time

Malte Schmitz's avatar
Malte Schmitz committed
16 17 18 19 20 21
from luma.led_matrix.device import max7219
from luma.core.interface.serial import spi, noop
from luma.core.render import canvas
from PIL import Image, ImageDraw
import random

Malte Schmitz's avatar
Malte Schmitz committed
22

Malte Schmitz's avatar
Malte Schmitz committed
23 24
SHOW_IMAGE = True

Malte Schmitz's avatar
Malte Schmitz committed
25 26 27 28 29 30 31 32 33 34 35 36
GPIO.setwarnings(False)
GPIO.setmode(GPIO.BCM)

PIN_STEPPER_DIR = 14
PIN_STEPPER_STEP = 15
PIN_STEPPER_DISABLE = 18

GPIO.setup(PIN_STEPPER_DIR, GPIO.OUT)
GPIO.setup(PIN_STEPPER_STEP, GPIO.OUT)
GPIO.setup(PIN_STEPPER_DISABLE, GPIO.OUT)

GPIO.output(PIN_STEPPER_DIR, False)
Malte Schmitz's avatar
Malte Schmitz committed
37
GPIO.output(PIN_STEPPER_DISABLE, True)
Malte Schmitz's avatar
Malte Schmitz committed
38 39 40 41
GPIO.output(PIN_STEPPER_STEP, False)

position = 0
moving = Lock()
Malte Schmitz's avatar
Malte Schmitz committed
42 43 44 45
MOVE_DELAY = 0.0005
SYNC_EVERY = 60
last_sync = 0

Malte Schmitz's avatar
Malte Schmitz committed
46

Malte Schmitz's avatar
Malte Schmitz committed
47 48 49
PIN_BUTTON = 24 
GPIO.setup(PIN_BUTTON, GPIO.IN, pull_up_down=GPIO.PUD_UP)

Malte Schmitz's avatar
Malte Schmitz committed
50

Malte Schmitz's avatar
Malte Schmitz committed
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

# initialize eye matrix device
serial = spi(port=0, device=0, gpio=noop())
dev = max7219(serial, cascaded=2, rotate=1)

# transform a coordnate from the first eye to the second eye
def eye_transform((x, y)):
    xx = 7 - x
    yy = 7 - y
    return (xx, yy + 8)
def eye_transform_list(list):
    return map(eye_transform, list)

# load circle image
circle_image = Image.open("circle.png")
circle_pixels = list(circle_image.getdata())
circle_coords = []
for i, (a,r,g,b) in enumerate(circle_pixels):
    if r + g + b > 0:
        circle_coords.append((i % 8, i / 8))
circle_coords2 = eye_transform_list(circle_coords)

# -2 <= eye_look <= 2
# 0 <= eye_blink <= 6
eye_look = 0
eye_blink = 0
def draw_eye():
    with canvas(dev) as draw:
        # first eye
        draw.point(circle_coords, fill="white")
        pupille = [(eye_look+3,3), (eye_look+3,4), (eye_look+4,3), (eye_look+4,4)]
        draw.point(pupille, fill="black")
        for y in range(0, eye_blink):
            draw.line([(0,y), (7,y)], fill="black")
        for y in range(8-eye_blink/2, 8):
            draw.line([(0,y), (7,y)], fill="black")
        # second eye
        draw.point(circle_coords2, fill="white")
        draw.point(eye_transform_list(pupille), fill="black")
        for y in range(0, eye_blink):
            draw.line(eye_transform_list([(0,y), (7,y)]), fill="black")
        for y in range(8-eye_blink/2, 8):
            draw.line(eye_transform_list([(0,y), (7,y)]), fill="black")

Malte Schmitz's avatar
Malte Schmitz committed
95
def eyes_direction(dir):
Malte Schmitz's avatar
Malte Schmitz committed
96 97 98 99 100 101 102
    global eye_look
    eye_look = dir
    draw_eye()

def do_eye_blink():
    global eye_blink
    while True:
Malte Schmitz's avatar
Malte Schmitz committed
103
        time.sleep(random.randint(3,8))
Malte Schmitz's avatar
Malte Schmitz committed
104 105 106 107 108 109 110 111 112 113 114 115 116
        for i in range(0, 7):
            eye_blink = i
            draw_eye()
            time.sleep(0.02)
        for i in range(6, -1, -1):
            eye_blink = i
            draw_eye()
            time.sleep(0.02)
t = Thread(target=do_eye_blink)
t.daemon = True
t.start()


Malte Schmitz's avatar
Malte Schmitz committed
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
# setup mini servos for eyes
PIN_SERVO_DATA_RIGHT = 2
PIN_SERVO_DATA_LEFT = 9
GPIO.setup(PIN_SERVO_DATA_RIGHT, GPIO.OUT)
GPIO.setup(PIN_SERVO_DATA_LEFT, GPIO.OUT)
pr = GPIO.PWM(PIN_SERVO_DATA_RIGHT, 50) # 50 Hz
pl = GPIO.PWM(PIN_SERVO_DATA_LEFT, 50) # 50 Hz
eyes_started = False
eyes_position = 0

def eyes_goto(degrees):
    global eyes_started, eyes_position
    degrees = int(round(degrees / 10.0)) * 10
    right_degrees = degrees + degrees / 5
    right_cycles = 12.5 - right_degrees / 18.0
    left_cycles = 2.5 + degrees / 18.0
    if eyes_started:
        if degrees != eyes_position:
            pr.ChangeDutyCycle(right_cycles)
            pl.ChangeDutyCycle(left_cycles)
            eyes_position = degrees
    else:
        pr.start(right_cycles)
        pl.start(left_cycles)
        eyes_started = True



Malte Schmitz's avatar
Malte Schmitz committed
145

Malte Schmitz's avatar
Malte Schmitz committed
146 147 148 149 150 151
# define the lower and upper boundaries of the "green"
# ball in the HSV color space, then initialize the
# list of tracked points
greenLower = (29, 86, 6)
greenUpper = (64, 255, 255)

Malte Schmitz's avatar
Malte Schmitz committed
152 153


Malte Schmitz's avatar
Malte Schmitz committed
154
cv2.namedWindow('Depth')
Malte Schmitz's avatar
Malte Schmitz committed
155 156
print('Press ESC in window to stop')

Malte Schmitz's avatar
Malte Schmitz committed
157 158 159 160 161 162 163
def sync():
    global last_sync, position
    do_move(-5000)
    sleep(0.5)
    do_move(3800/2)
    last_sync = time.time()
    position = 0
Malte Schmitz's avatar
Malte Schmitz committed
164

Malte Schmitz's avatar
Malte Schmitz committed
165
def goto(new_position):
Malte Schmitz's avatar
Malte Schmitz committed
166 167 168
    global position
    if time.time() - last_sync > SYNC_EVERY:
        sync()
Malte Schmitz's avatar
Malte Schmitz committed
169 170
    if moving.locked():
        return False
Malte Schmitz's avatar
Malte Schmitz committed
171
    delta = new_position - position
Malte Schmitz's avatar
Malte Schmitz committed
172 173
    if abs(delta) > 100:
        move(delta)
Malte Schmitz's avatar
Malte Schmitz committed
174 175
        d = min(2, max(-2, int(round(delta / -750.0))))
        eyes_direction(d)
Malte Schmitz's avatar
Malte Schmitz committed
176 177
    position = new_position

Malte Schmitz's avatar
Malte Schmitz committed
178 179
def move(steps):
    t = Thread(target=do_move, args=[steps])
Malte Schmitz's avatar
Malte Schmitz committed
180 181
    t.start()

Malte Schmitz's avatar
Malte Schmitz committed
182
def do_move(steps):
Malte Schmitz's avatar
Malte Schmitz committed
183
    moving.acquire()
Malte Schmitz's avatar
Malte Schmitz committed
184 185 186
    dir_left = steps < 0
    GPIO.output(PIN_STEPPER_DISABLE, False)
    GPIO.output(PIN_STEPPER_DIR, dir_left)
Malte Schmitz's avatar
Malte Schmitz committed
187 188 189 190 191
    steps = abs(steps)
    GPIO.output(PIN_STEPPER_STEP, False)
    for x in range(1, steps):
        GPIO.output(PIN_STEPPER_STEP, True)
        GPIO.output(PIN_STEPPER_STEP, False)
Malte Schmitz's avatar
Malte Schmitz committed
192
        sleep(MOVE_DELAY)
Malte Schmitz's avatar
Malte Schmitz committed
193 194 195
        if (dir_left and not GPIO.input(PIN_BUTTON)):
            break
    GPIO.output(PIN_STEPPER_DISABLE, True)
Malte Schmitz's avatar
Malte Schmitz committed
196 197 198
    moving.release()


Malte Schmitz's avatar
Malte Schmitz committed
199 200
def get_depth():
   return frame_convert2.pretty_depth_cv(freenect.sync_get_depth()[0])
Malte Schmitz's avatar
Malte Schmitz committed
201 202 203 204

def get_video():
    return frame_convert2.video_cv(freenect.sync_get_video()[0])

Malte Schmitz's avatar
Malte Schmitz committed
205 206 207 208
def trackDepth():
    frame = imutils.resize(get_depth(), width=600)
    (minVal, maxVal, minLoc, maxLoc) = cv2.minMaxLoc(frame)
    center = minLoc
Malte Schmitz's avatar
Malte Schmitz committed
209

Malte Schmitz's avatar
Malte Schmitz committed
210 211 212 213
    if center != None:
        target_x = (300 - center[0]) * -5
        goto(target_x)

Malte Schmitz's avatar
Malte Schmitz committed
214
        target_y = (450 - center[1]) / 15
Malte Schmitz's avatar
Malte Schmitz committed
215 216
        eyes_goto(target_y)

Malte Schmitz's avatar
Malte Schmitz committed
217 218 219 220 221 222 223 224 225 226
    if SHOW_IMAGE:
        rgb = cv2.cvtColor(frame, cv2.COLOR_GRAY2RGB)
        cv2.circle(rgb, center, 10, (255, 0, 0), 2)

        video = imutils.resize(get_video(), width=600)
        cv2.circle(video, center, 10, (255, 0, 0), 2)

        return rgb  #video or rgb(for depth)
    
    return None
Malte Schmitz's avatar
Malte Schmitz committed
227

Malte Schmitz's avatar
Malte Schmitz committed
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
def track():
    # resize the frame, blur it, and convert it to the HSV
    # color space
    frame = imutils.resize(get_video(), width=600)
    blurred = cv2.GaussianBlur(frame, (11, 11), 0)
    hsv = cv2.cvtColor(blurred, cv2.COLOR_BGR2HSV)

    # construct a mask for the color "green", then perform
    # a series of dilations and erosions to remove any small
    # blobs left in the mask
    mask = cv2.inRange(hsv, greenLower, greenUpper)
    mask = cv2.erode(mask, None, iterations=2)
    mask = cv2.dilate(mask, None, iterations=2)

    # find contours in the mask and initialize the current
    # (x, y) center of the ball
    cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL,
            cv2.CHAIN_APPROX_SIMPLE)
    cnts = cnts[0] if imutils.is_cv2() else cnts[1]
    center = None

    # only proceed if at least one contour was found
    if len(cnts) > 0:
            # find the largest contour in the mask, then use
            # it to compute the minimum enclosing circle and
            # centroid
            c = max(cnts, key=cv2.contourArea)
            ((x, y), radius) = cv2.minEnclosingCircle(c)
            M = cv2.moments(c)
            center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))

            # only proceed if the radius meets a minimum size
            if radius > 10:
                    # draw the circle and centroid on the frame,
Malte Schmitz's avatar
Malte Schmitz committed
262
                    cv2.circle(frame, (int(x), int(y)), int(radius), (0, 255, 255), 2)
Malte Schmitz's avatar
Malte Schmitz committed
263

Malte Schmitz's avatar
Malte Schmitz committed
264
    if center != None:
Malte Schmitz's avatar
Malte Schmitz committed
265 266
        target_x = (300 - center[0]) * -5
        goto(target_x)
Malte Schmitz's avatar
Malte Schmitz committed
267 268 269 270

    return frame


Malte Schmitz's avatar
Malte Schmitz committed
271 272 273 274 275 276 277 278
if SHOW_IMAGE:
    while True:
        cv2.imshow('Depth', trackDepth())
        if cv2.waitKey(10) == 27:
            break
else:
    while True:
        trackDepth()