AI4R hardware challenge python script

pixycam_bottle_chaser.py

from __future__ import print_function
import pixy
from ctypes import *
from pixy import *

import picar
import time
from picar import front_wheels
from picar import back_wheels

# picar parameters
motor_speed = 90
step = 40
db = 'config'
FRAMEX_LEFT, FRAMEX_CENTER, FRAMEX_RIGHT = 60, 150, 300
FRAME_THRESHOLD 100
blocks = BlockArray(100)
frame = 0

class Blocks (Structure):
  _fields_ = [ ("m_signature", c_uint),
    ("m_x", c_uint),
    ("m_y", c_uint),
    ("m_width", c_uint),
    ("m_height", c_uint),
    ("m_angle", c_uint),
    ("m_index", c_uint),
    ("m_age", c_uint) ]

def picar_init(motor_speed, step, db_config='config'):
    global motor_speed
    global step
    global db
    global fw
    global bw
    motor_speed = motor_speed
    step = step
    print("setting up picar hardware")
    picar.setup()
    fw = front_wheels.Front_Wheels(db=db_config)
    bw = back_wheels.Back_Wheels(db=db_config)
    print("done init")

def picar_steer(frame_x):
    def _resume(sleep_time=0.5):
        time.sleep(sleep_time)
        fw.turn(90)
        bw.stop()
    if 0 <= frame_x < FRAMEX_LEFT:
        print("chase left")
        fw.turn(90 - step)
        bw.forward()
        bw.speed = motor_speed
        _resume()
    elif FRAMEX_LEFT <= frame_x < FRAMEX_CENTER:
        print("chase straight")
        fw.turn(90)
        bw.forward()
        bw.speed = motor_speed
        _resume()
    elif FRAMEX_CENTER <= frame_x < FRAMEX_RIGHT:
        print("chase right")
        fw.turn(90 + step)
        bw.forward()
        bw.speed = motor_speed
        print("right turn")
        _resume()
    else:
        print("N/A")
        fw.turn(90)
        bw.stop()

def pixycam_init():
    print("pixy2 hw init")
    pixy.init()
    pixy.change_prog("color_connected_components");


## init
pixycam_init()
picar_init()

while 1:

  count = pixy.ccc_get_blocks(100, blocks)

  if count > 0:
    print('frame %3d:' % (frame))
    frame += 1
    for index in range (0, count):
        print('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature,
            blocks[index].m_x,
            blocks[index].m_y,
            blocks[index].m_width,
            blocks[index].m_height))
        if frame >= FRAME_THRESHOLD:
            frame = 0
            picar_steer(blocks[index].m_x)