def join(self, timeout=None):
""" Stop the thread. """
self._stopevent.set()
pixy.set_lamp(0, 0)
drive = clsDrive.Drive()
drive.stop()
threading.Thread.join(self, timeout)
def run(self):
drive = clsDrive.Drive()
control = clsDrive.Control()
point = 0
pixy.init()
pixy.change_prog("color_connected_components")
pixy.set_lamp(0, 0)
pixy.set_servos(500, 650)
blocks = BlockArray(100)
while not self._stopevent.isSet():
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
x = 0
y = 0
width = 0
for index in range(0, count):
curBlock = blocks[index]
if curBlock.m_signature == 1 and curBlock.m_width > width:
width = curBlock.m_width
x = curBlock.m_x
y = curBlock.m_y
if width > 0:
control.maze(x, y, width)
if control.stop == True:
if control.progress == 0 or control.progress == 1 or control.progress == 4 or control.progress == 5 or control.progress == 6:
drive.turnLeft()
elif control.progress == 2 or control.progress == 3 or control.progress == 7:
drive.turnRight()
control.progress += 1
elif control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 0.7
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
def get_pixy_data(self):
pixy.set_lamp(2, 0)
blocks = BlockArray(40)
last_counts = [
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0
]
iter = 0
#sample = 0;
while (1):
save = True
piece_lst = []
# red = 0;
# purple = 0;
# blue = 0;
# green = 0;
# pink = 0;
color = ''
# start = time.time();
count = pixy.ccc_get_blocks(40, blocks)
if count > 0:
for index in range(count):
if blocks[index].m_signature == 1:
# red += 1;
color = 'red'
elif blocks[index].m_signature == 2:
# blue += 1;
color = 'blue'
elif blocks[index].m_signature == 3:
# purple += 1;
color = 'purple'
elif blocks[index].m_signature == 4:
# green += 1;
color = 'green'
elif blocks[index].m_signature == 5:
# pink += 1;
color = 'pink'
#if not (blocks[index].m_x in range(40, 252)) or not (blocks[index].m_y in range(0, 207)):
piece_lst.append(
'SIG=%s X=%d Y=%d' %
(color, blocks[index].m_x, blocks[index].m_y))
# print('[SIG = %s X = %d Y = %d AGE = %3d]'%(color, blocks[index].m_x, blocks[index].m_y, blocks[index].m_age));
last_counts[iter] = count
iter = (iter + 1) % 25
if (all(x == last_counts[0] for x in last_counts)):
return piece_lst
def runPath(x, y, w): # arguments passed are encoder counts
global currentLineCount
global yLength
global goingDown
global distAlongY
distAlongY = 0
yLength = y
goingDown = True
obstacles = []
countPlant = int(int(y) * countToCM)
maxCount = int(x) / int(w)
widthCount = int(int(w) * countToCM)
if maxCount % 10 == 0:
maxCount += 1
leftOrRight = False # False: initially turn left, True: initially turn right
currentLineCount = 0
count = 0
#initialize camera
pixy.init()
pixy.set_lamp(1, 1)
pixy.change_prog("color_connected_components")
while count <= maxCount:
forward(countPlant)
currentLineCount += 1
goingDown = not goingDown
# width is length of romi (15cm*countToCM) + a count num
if leftOrRight:
turnRight()
forwardOnX(widthCount - int(15 * countToCM))
turnRight()
leftOrRight = False
else:
turnLeft()
forwardOnX(widthCount - int(15 * countToCM))
turnLeft()
leftOrRight = True
count += 1
pixy.set_lamp(0, 0)
currentLineCount = -1
def detectObject(
self
): # Method for detection of the object inside a pallet. The method returns the first detected object
blocks = BlockArray(1)
objectFound = False
pixy.set_lamp(1, 0) # Turning the pixi light on
while objectFound == False:
count = pixy.ccc_get_blocks(1, blocks)
if count > 0:
objectFound = True
print('Object Detected: Signature %d' % blocks[0].m_signature)
time.sleep(2)
pixy.set_lamp(0, 0) # Turning the pixi light off
return blocks[0].m_signature
def run(self):
drive = clsDrive.Drive()
control = clsDrive.Control()
pixy.init()
pixy.change_prog("line")
pixy.set_servos(500, 800)
pixy.set_lamp(1, 0)
vectors = VectorArray(100)
intersections = IntersectionLineArray(100)
while not self._stopevent.isSet():
line_get_all_features()
i_count = line_get_intersections(100, intersections)
v_count = line_get_vectors(100, vectors)
print("Scanning")
if i_count > 0 or v_count > 0:
# print('frame %3d:' % (frame))
for index in range(0, v_count):
control.lineFollow(vectors[index].m_x0,
vectors[index].m_y0,
vectors[index].m_x1,
vectors[index].m_y1)
print(str(control.frame))
if control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 1
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
drive.stop()
def get_pixy_data(self):
_fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint)]
pixy.set_lamp(2, 0)
blocks = BlockArray(100)
num_iter = 25
last_counts = []
for i in range(num_iter):
last_counts.append(0)
iter = 0
while (1):
save = True
piece_lst = []
color = ''
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
# print('count' + str(count))
for index in range(count):
if blocks[index].m_signature == 1:
color = 'red'
elif blocks[index].m_signature == 2:
color = 'blue'
elif (blocks[index].m_signature
== 3) or (blocks[index].m_signature == 5):
color = 'pink'
elif blocks[index].m_signature == 4:
color = 'green'
# if not (blocks[index].m_x in range(40, 252)) or not (blocks[index].m_y in range(0, 207)):
# save = False;
# else:
piece_lst.append(
'SIG=%s X=%d Y=%d' %
(color, blocks[index].m_x, blocks[index].m_y))
# print('[SIG = %s X = %d Y = %d]'%(color, blocks[index].m_x, blocks[index].m_y));
last_counts[iter] = count
iter = (iter + 1) % num_iter
if save and (all(x == last_counts[0] for x in last_counts)):
return piece_lst
def detectField():
#initialize camera
pixy.init()
pixy.set_lamp(1, 1)
pixy.change_prog("color_connected_components")
# field detection instructions
x1_count = forwardForDetection()
turnLeft()
y1_count = forwardForDetection()
turnLeft()
forwardObstacle(x1_count)
turnLeft()
forwardObstacle(y1_count)
turnLeft()
pixy.set_lamp(0, 0)
y = int(x1_count / countToCM) # length in cm
x = int(y1_count / countToCM) # width in cm
return str(x) + " " + str(y)
class Lights:
pixy.change_prog ("video")
turnOn = pixy.set_lamp (1, 0)
turnOff = pixy.set_lamp (0, 0)
class Blocks(Structure):
pixy.set_lamp(0, 0)
exit(0)
import pixy
from ctypes import *
from pixy import *
# Pixy2 Python SWIG Set Lamp Example #
print ("Pixy2 Python SWIG Example -- Set Lamp")
pixy.init ()
pixy.change_prog ("video");
pixy.set_lamp (1, 0);
from Movement import *
from Vision2 import *
import pixy
m = Movement()
v = Vision()
v.setmode(1)
tspeed = int(input('Turning Speed: '))
pixy.set_lamp(1, 0)
area_limit = 1000
for i in range(2):
done_colour = False
while not done_colour:
blocks = v.get_color(i)
if blocks == None:
m.turn('r')
else:
block = blocks[0]
x = int(block.m_x)
y = int(block.m_y)
a = int(block.m_width * block.m_height)
print(block, x, y, a)
if a <= area_limit or True:
if abs(x - 160) < 20:
m.forward(100, 0, 0)
elif x > 160:
m.turn('r', 0, tspeed)
else:
m.turn('l', 0, tspeed)
elif a > area_limit:
done_colour = True
def run(self):
'''pixy.init()
pixy.change_prog("color_connected_components");
pixy.set_lamp(1, 0)
drive = clsDrive.Drive()
control = clsDrive.Control()
blocks = BlockArray(100)
while not self._stopevent.isSet( ):
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
for index in range(0, count):
curBlock = blocks[index]
print("x " + str(curBlock.m_x) + "- y " + str(curBlock.m_y))
#control.neb(curBlock.m_x, curBlock.m_y)
#control.speed = 0.5
#drive.HarDrive(control)
else:
#drive.joltLeft()
print("Jolt")
'''
tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof.open() # Initialise the i2c bus and configure the sensor
tof.start_ranging(
1
) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
drive = clsDrive.Drive()
control = clsDrive.Control()
control.progress = 2
pixy.init()
pixy.change_prog("color_connected_components")
pixy.set_lamp(0, 0)
pixy.set_servos(500, 650)
blocks = BlockArray(100)
while not self._stopevent.isSet():
count = pixy.ccc_get_blocks(100, blocks)
if count > 0 and control.progress < 6:
x = 0
y = 0
width = 0
for index in range(0, count):
curBlock = blocks[index]
if curBlock.m_signature == control.progress and curBlock.m_width > width:
width = curBlock.m_width
x = curBlock.m_x
y = curBlock.m_y
if width > 0:
control.distance = tof.get_distance()
#print(str(control.distance))
if control.distance < 100:
if control.progress == 5:
drive.stop()
else:
control.angle = 180
control.speed = 0.5
control.stop = False
drive.HarDrive(control)
sleep(1)
drive.stop()
control.progress += 1
else:
#print("control.neb")
print(str(width))
control.neb(x, y, width)
#print(str(control.frame))
if control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 0.5
control.stop = False
drive.HarDrive(control)
else:
drive.joltLeft()
elif control.progress == 6:
drive.stop()
break
else:
drive.joltLeft()
tof.close()
print("Test PixyCam")
pixy.init()
pixy.change_prog("color_connected_components")
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)]
blocks = BlockArray(100)
frame = 0
pixy.set_lamp(0, 0)
a_star.motors(70, 70) # move straight forward, need to use pi control!!!
print("begin obstacle search")
while 1:
count = pixy.ccc_get_blocks(100, blocks) # 100 blocks probably overkill...
if (count > 0):
# make sure block is large enough to be relevant
if (blocks[0].m_width > 50 and blocks[0].m_height > 50):
a_star.motors(0, 0) # stops if a block is detected
pixy.set_lamp(1, 1) # turns on lamp if a block is detected
time.sleep(2)
pixy.set_lamp(0, 0)
print("obstacle detected, motor stopped")
def stop(t):
pixy.init()
pixy.set_lamp(0, 0)
a_star.motors(0, 0)
time.sleep(t)
def shutDown(self): # Method for preparation for shutDown
pixy.set_lamp(0, 0) # Turning the pixi light off
from __future__ import print_function
import pixy
from ctypes import *
from pixy import *
# Pixy2 Python SWIG Set Lamp Example #
print("Pixy2 Python SWIG Example -- Set Lamp")
pixy.init()
pixy.change_prog("video")
pixy.set_lamp(0, 0)
import pixy
import time
from ctypes import *
from pixy import *
# Pixy2 Python SWIG Set Lamp Example #
print("Pixy2 Python SWIG Example -- Set Lamp")
pixy.init()
pixy.change_prog("video")
pixy.set_lamp(1, 0)
time.sleep(5)
pixy.set_lamp(0, 1)