def main():
# Initialize the pixycam and select the line following program.
pixy.init()
pixy.change_prog("line")
# NetworkTables requires logging module
logging.basicConfig(level=logging.DEBUG)
# Connect to network tables server on the IP address of the Rio.
NetworkTables.initialize(server='10.11.65.2')
# Connect to SmartDashboard
smart_dash = NetworkTables.getTable('SmartDashboard')
# Initialize a Vector Object with a (very generous) 10 slots
# (We ever only want to pay attention to the first vector anyway.)
vectors = VectorArray(10)
# loop forever. Not very pythonic, but it's what we need.
while True:
# get all of the current features (what the pixycam sees)
line_get_all_features()
# fetch the top 10 vectors from pixy
vector_count = line_get_vectors(10, vectors)
# If there are any vectors, v_count will be greater than zero.
if vector_count > 0:
smart_dash.putNumber('Pixy X0', vectors[0].m_x0)
smart_dash.putNumber('Pixy Y0', vectors[0].m_y0)
smart_dash.putNumber('Pixy X1', vectors[0].m_x1)
smart_dash.putNumber('Pixy Y1', vectors[0].m_y1)
time.sleep(1)
def main():
"""Run the test."""
pixy.init()
pixy.change_prog("color_connected_components")
requested_signature = int(
input("What colour signature do you want to use?: "))
blocks = pixy.BlockArray(100)
while True:
print_largest(requested_signature, blocks)
def __init__(self):
for i in range(13, 20):
GPIO.setup(i, GPIO.OUT, initial=1)
GPIO.setup(8, GPIO.OUT, initial=1)
GPIO.setup(26, GPIO.OUT, initial=1)
GPIO.setup(11, GPIO.OUT, initial=1)
GPIO.setup(27, GPIO.OUT, initial=1)
pixy.init()
pixy.change_prog("color_connected_components")
self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
self.tof.open() # Initialise the i2c bus and configure the sensor
self.tof.start_ranging(1)
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 setup(self):
GPIO.setup(24, GPIO.IN, pull_up_down=GPIO.PUD_UP)
self.DIABLO = Diablo.Diablo()
self.DIABLO.i2cAddress = 0x25
self.DIABLO.Init()
self.DIABLO.ResetEpo()
self.DIABLO.SetEnabled(True)
self.ESCs = ESCD3in.PairESCController()
self.defaultFlywheelDuty = "1060"
pixy.init()
pixy.change_prog("color_connected_components")
self.tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
self.tof.open()
self.tof.start_ranging(
3
) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
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 startup():
global pixyCam
pixyCam = pixy.init ()
if pixyCam == -1:
print "Can't connect to pixycamera!!!!!"
else:
pixy.change_prog ("color_connected_components")
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 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)
def ColorSensor():
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
i = 0
check = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
ret = check
while i < 20:
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
#print 'frame %3d:' % (frame)
frame = frame + 1
for index in range(0, count):
check[blocks[index].m_signature - 1] += 1
#print blocks[index].m_signature
#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)
for j in range(0, count):
if (check[j] - 1 == 1 or check[j] == 2 or check[j] == 3):
ret[check[j] - 1] += 1
print check
#print ret
i += 1
time.sleep(.5)
return ret
#ColorSensor()
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()
def pixy_start():
print("Pixy2 Python SWIG Example -- Get Blocks")
pixy.init()
pixy.change_prog("color_connected_components")
def __init__(self):
if pixy.init() != 0:
print("Error initializing pixy2")
sys.exit(0)
self.bots = BlockArray(10)
self.angles = []
cond.notify()
NetworkTables.initialize(server='10.46.69.2')
NetworkTables.addConnectionListener(connectionListener, immediateNotify=True)
with cond:
print("Waiting")
if not notified[0]:
cond.wait()
print("Connected!")
# Insert your processing code here
table = NetworkTables.getTable('DataTable')
if pixy.init()!=0:
print("Pixy 2 not connected")
exit()
if(pixy.getUID()==frontUID):
isFrontLine = true
if(pixy.getUID()==rearUID):
isFrontLine = false
pixy.change_prog ("line")
class Vector (Structure):
_fields_ = [
("m_x0", c_uint),
("m_y0", c_uint),
("m_x1", c_uint),
def __init__(self):
pixy.init()
self.frame = 0
def stop(t):
pixy.init()
pixy.set_lamp(0, 0)
a_star.motors(0, 0)
time.sleep(t)
def __init__(self):
pixy.init()
def __init__(self):
#print("testClassifier")
pixy.init()
def f_init():
print("Blocks started")
pixy.init()
pixy.change_prog("color_connected_components")
def coordinates(numCords, mode, lamp, lTime): #numCords is size of coordinate array, mode =1 for laser, =2 for pump
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) ]
if (mode == 1): #LASER MODE
Xmin = 80
Xmax = 245
Ymin = 0
Ymax = 143
elif (mode == 2): #PUMP MODE
Xmin = 65
Xmax = 245
Ymin = 0
Ymax = 143
blocks = BlockArray(100)
frame = 0
sz=numCords #size of coordinate array
if (lamp==1):
set_lamp(1,0)
count = pixy.ccc_get_blocks (100, blocks)
time.sleep(lTime)
set_lamp(0,0)
elif(lamp==2):
set_lamp(1,1)
count = pixy.ccc_get_blocks (100, blocks)
time.sleep(lTime)
set_lamp(0,0)
elif(lamp==0):
count = pixy.ccc_get_blocks (100, blocks)
n = 0
temp = numpy.zeros((sz,2))
for k in range (1, count):
X = blocks[k].m_x #retreive x and y coordinates of block centerpoint
Y = blocks[k].m_y
if X>Xmin and X<Xmax and Y>Ymin and Y<Ymax: #check for ROI
temp[n] = (Y,X-Xmin) #arrange cords in (Y,X) for sorting by Y
n+=1
order = numpy.argsort(temp,0) #create sort order array
index = numpy.zeros((sz,2))
cords = numpy.zeros((sz+1,2)) #array to hold sorted coordinates
cords[0,0] = n #first item in coordinate array is number of coordinates
#arranges coordinates acording to values in order[]
for k in range (0,sz):
cords[sz-k,:] = temp[order[k,0],:]
cords[1:, [0,1]] = cords[1:,[1,0]] #switch cords from (Y,X) to (X,Y)
cords = cords.astype(numpy.int) #change cords[] to integer array
#print('count', count)
print('number', n)
#print('\ntemp\n', temp)
#print('order\n', order)
print('cords\n',cords)
return cords
from __future__ import print_function
import pixy
from networktables import NetworkTables
from ctypes import *
from pixy import *
NetworkTables.initialize(server='roborio-4646-frc.local')
# Pixy2 Python SWIG get blocks example #
print("Pixy2 Python SWIG Example -- Get Blocks")
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
pixyTable = NetworkTables.getTable('Pixy')
while 1:
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
print('frame %3d:' % (frame))
def initialize():
#initiliaze the pixy in the module
pixy.init()
#ensure it is switched to detect the tape on the ground
pixy.change_prog("line")
