def __init__(self, racerBot, controller):
# pixy parameters
self.pixyMinX = 0
self.pixyMaxX = 316
self.pixyMinY = 0
self.pixyMaxY = 208
self.pixyCenterX = (self.pixyMaxX - self.pixyMinX) / 2
self.pixyCenterY = (self.pixyMaxY - self.pixyMinY) / 2
self.targetSignature = 1
self.calibrationSignature = 2
# servo parameters
self.servoMinPos = -90
self.servoMaxPos = 90
self.servoPos = 0
# variables for pixy
self.oldBlocks = pixy.BlockArray(100)
self.newBlocks = pixy.BlockArray(100)
self.oldCount = pixy.ccc_get_blocks(100, self.oldBlocks)
self.newCount = pixy.ccc_get_blocks(100, self.newBlocks)
# controller needs to have an update function must have an
# update function that takes in error (in X) and returns
# change in angle to send to the servo
self.gimbal = controller
self.calibrationController = PID_controller(0.06, 0, 0.06)
# pixyRacer to control
self.racerBot = racerBot
# tuning curve parameters
self.minAngle = 0
self.maxAngle = 0
self.targetMinAngle = -35
self.targetMaxAngle = 35
def f_getBlocks():
global run
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)
while run == 1:
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
arr = []
arr.append(blocks[0].m_signature)
arr.append(blocks[0].m_width)
arr.append(blocks[0].m_height)
arr.append(blocks[0].m_x)
arr.append(blocks[0].m_y)
arr_pub = Int32MultiArray(data=arr)
print(str(arr[0]) + " erkannt")
f_publish(arr_pub)
rate.sleep()
else:
arr = []
arr.append(0)
arr_pub = Int32MultiArray(data=arr)
f_publish(arr_pub)
rate.sleep()
print("Nichts erkannt")
print("ende")
def get(Signatur):
blocks = BlockArray(100)
frame = 0
signal = 0, 0
signal1 = 0, 0
signal2 = 0, 0
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
frame = frame + 1
for index in range(0, count):
logging.debug('[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))
signal = (blocks[index].m_signature, blocks[index].m_x,
blocks[index].m_y, blocks[index].m_width,
blocks[index].m_height)
if signal[0] == 1 or signal[0] == 0:
signal1 = signal
if signal[0] == 2 or signal[0] == 0:
signal2 = signal
if Signatur == 1:
return (signal1)
elif Signatur == 2:
return (signal2)
def get_pixy():
count = pixy.ccc_get_blocks (100, blocks)
if count > 0:
# print('frame %3d:' % (frame))
# frame = frame + 1
for index in range (0, count):
#Anpassung des Pixy-Koordinatensystems an das Roboterkoordinatensystem
'''
(0|0) ___________________(315|0) (157.5|-103.5) _______(157.5|103.5)
| | | |
| | | |
| | --> | (0|0) |
| | | |
| | | |
(0|207)__________________(315|207) (-157.5|-103.5)_______(-157.5|103.5)
^
___|___
|PIXY|
'''
X_ball = blocks[index].m_y - 103.5 #Y_pixy_max / 2
Y_ball = (blocks[index].m_x - 157.5)*(-1) #X_pixy_max / 2
print('(X|Y)= ',X_ball,'|',Y_ball)
return X_ball,Y_ball
def track():
global taskInput
global Locked_On_Block
global Locked_Block_Index
global Blocks
global Count
global Pan
# while taskInput != "S":
Count = pixy.ccc_get_blocks (100, Blocks)
if Count > 0:
# Block acquisition logic #
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range (0, Count):
Pan_Offset = (pixy.get_frame_width () / 2) - Blocks[Index].m_x;
Tilt_Offset = Blocks[Index].m_y - (pixy.get_frame_height () / 2)
Pan_PID_Controller.Update (Pan_Offset)
Tilt_PID_Controller.Update (Tilt_Offset)
pixy.set_servos (Pan_PID_Controller.Command, Tilt_PID_Controller.Command)
Pan = Pan_PID_Controller.Command;
else:
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK:
Locked_Block_Index = Blocks[0].m_index;
Locked_On_Block = True
else:
Reset ()
def trackSignature(self, _signature):
print("Starting to track signature")
pixy.change_prog("color_connected_components")
print("Changed program to CCC")
data = pixy.ccc_get_blocks(100, self.blocks)
print("Data gathered")
#Find the target with largest area to return
return dataFormat.format()
def getBlocks(self):
blocks = BlockArray(100)
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
for index in range(0, count):
centerx = blocks[index].m_x + blocks[index].m_width / 2
centery = blocks[index].m_y - blocks[index].m_height / 2
return 1, centerx, centery, blocks[index].m_width, blocks[
index].m_height
return None, None, None, None, None
def get_block_arrays(block_array, colours=None, max_blocks=100):
"""Return a list of blocks for each coloru in for *colours*.
They are returned as a len(colours) array of arrays in
the order specified. If colours is None, all signatures
are returned.
If no blocks are found, None is returned.
Each item in each array takes the form:
[x, y, width, height, index, age]
Parameters:
- block_array
This is the BlockArray() needed to collect the blocks.
- colours
A list of all colour signatures needed.
- max_blocks
The maximum number of blocks fetched.
WARNING: Not yet tested.
"""
# Build signature mask.
if colours is None:
sig_mask = [True] * 10
else:
sig_mask = [False] * 10
for colour in colours:
sig_mask[colour] = True
# Now proceed to collect data.
no_blocks = pixy.ccc_get_blocks(max_blocks, block_array)
if no_blocks == 0:
return None
# Set up lists.
block_lists = [[]] * 10
some_of_interest = False
for i in range(no_blocks):
sig = block_array[i].m_signature
if sig_mask[sig]:
some_of_interest = True
block_lists[sig].append([
block_array[i].m_x,
block_array[i].m_y,
block_array[i].m_width,
block_array[i].m_height,
block_array[i].m_index,
block_array[i].m_age
])
if not some_of_interest:
# Nothing of interest found.
return None
to_return = []
for colour in colours:
to_return.append(block_lists[colour])
return to_return
def get_pixy():
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
# print('frame %3d:' % (frame))
# 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))
X_ball = blocks[index].m_x
Y_ball = blocks[index].m_y
print('(X|Y)= ', X_ball, '|', Y_ball)
def read_raw(self):
global Ball_Status
if self.sim_mode == True:
X_ball, Y_ball = test_data
else:
count = pixy.ccc_get_blocks(100, self.blocks)
if count > 0:
for index in range(0, count):
X_ball = self.blocks[index].m_x
Y_ball = self.blocks[index].m_y
#if X_ball is not None:
#print('(X|Y)= ', X_ball, '|', Y_ball)
return X_ball, Y_ball
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 getBlocks(self):
"""Get various information about the most prominent circle in the image
Return 1: centrex [int]; the x centre of the nearest circle
Return 2: centrey [int]; the y centre of the nearest circle
Return 3: blockWidth [int];
Return 4: blockHeight [int];
"""
blocks = BlockArray(100)
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
for index in range (0, count):
centerx = blocks[index].m_x + blocks[index].m_width/2
centery = blocks[index].m_y - blocks[index].m_height/2
return 1,centerx, centery, blocks[index].m_width, blocks[index].m_height
return None,None,None,None,None
def get_block_object():
blocks = BlockArray(2)
count = pixy.ccc_get_blocks(2, blocks)
if count > 1:
result = ('[BLOCK: SIG=%d X=%2d Y=%3d WIDTH=%3d HEIGHT=%3d]' %
(blocks[1].m_signature, blocks[1].m_x, blocks[1].m_y,
blocks[1].m_width, blocks[1].m_height))
output = []
output.append(blocks[1].m_x)
output.append(blocks[1].m_y)
print(result)
print(output)
return output
else:
print("Nothing found")
def detect_color(frame):
#print ('colors')
global BoxFoundTimeStamp
global LeftLane
global SafeToShift
global TimeToSafety
global ShiftLaneDelay
if frame == FrameThreashold:
pixy.change_prog ("color_connected_components");
count = pixy.ccc_get_blocks (100, blocks)
''' if count > 0:
#print('frame %3d:' % (frame))
#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))'''
FoundBox = False
if count > 0:
for i in range(count):
if(blocks[i].m_signature == GreenBox):
FoundBox = True
if(SafeToShift):
backward()
time.sleep(0.5)
BoxFoundTimeStamp = time.time()
if(LeftLane):
shift_right(180 - LeftShiftAngle)
time.sleep(ShiftLaneDelay)
LeftLane = False
SafeToShift = False
forward(100)
time.sleep(0.5)
shift_left(LeftShiftAngle)
else:
shift_left(LeftShiftAngle)
time.sleep(ShiftLaneDelay)
LeftLane = True
SafeToShift = False
forward(100)
time.sleep(0.5)
shift_right(180 - LeftShiftAngle)
if( (not FoundBox) and ( (time.time() - BoxFoundTimeStamp) > TimeToSafety) ):
SafeToShift = True
def getSig():
global pixyCam
#jsonify({'some':'data'})
if pixyCam == -1:
return "Can't connect to PixyCamera"
count = pixy.ccc_get_blocks (100, blocks)
ret = {"C":count}
for i in range(count):
ret ["S"+str(i)] = blocks[i].m_signature
ret ["X"+str(i)] = blocks[i].m_x
ret ["Y"+str(i)] = blocks[i].m_y
ret ["W"+str(i)] = blocks[i].m_width
ret ["H"+str(i)] = blocks[i].m_height
return ret
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 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 getGoodSig():
global pixyCam
if pixyCam == -1:
return "Can't connect to PixyCamera"
count = pixy.ccc_get_blocks (100, blocks)
ret = {}
for i in range(count):
blk = {}
blk ["S"] = blocks[i].m_signature
blk ["X"] = blocks[i].m_x
blk ["Y"] = blocks[i].m_y
blk ["W"] = blocks[i].m_width
blk ["H"] = blocks[i].m_height
ret[i] = blk
return ret
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 get_base_position():
objects_seen = pixy.ccc_get_blocks(100, blocks)
#TODO: add procedure for if there is no base detected
if objects_seen > 0:
lowest_distance_to_center = 1000
target_block = None
# choose the most centered block in the frame
for i in range(0, objects_seen):
if blocks[i].m_signature == 1:
# print('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[i].m_signature, blocks[i].m_x, blocks[i].m_y, blocks[i].m_width, blocks[i].m_height))
distance_to_center = math.sqrt((blocks[i].m_x - x_center)**2 +
(blocks[i].m_y - y_center)**2)
if distance_to_center < lowest_distance_to_center:
lowest_distance_to_center = distance_to_center
target_block = blocks[i]
if target_block:
position_vector = (target_block.m_x - base_center_target[0],
base_center_target[1] - target_block.m_y)
return position_vector
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 forward(eCount):
global distAlongY
global yLength
global distAlongY
blocks = BlockArray(100)
a_star.motors(50, 50)
leftCount = 0 # use left count to determine when to stop
rightCount = 0
leftAcc = 0
rightAcc = 0
countAcc = 0
encoders1 = a_star.read_encoders()
time2 = time.perf_counter()
while (leftCount < eCount): # left encoder count is less than goal count
if ((time.perf_counter() - time2) >
0.05): # look at encoder values every 0.05 second
encoders2 = a_star.read_encoders()
righten = encoders2[1] - encoders1[1]
leften = encoders2[0] - encoders1[0]
# only update if difference is positive (not wraparound case)
if (righten > 0):
rightError = 35 - righten
rightCount += righten
if (leften > 0):
leftError = 35 - leften
leftCount += leften
# Update distAlongY
distAlongY = int(leftCount / countToCM)
if not goingDown:
distAlongY = int(yLength) - int(distAlongY)
# only apply integral control on fifth iteration
if (countAcc > 5):
leftAcc += leftError
rightAcc += rightError
# get motor parameters from PI method
left = piControl(leftError, leftAcc, 0)
right = piControl(rightError, rightAcc, 1)
# angle correction and motor control
if (leftCount - rightCount > 10):
a_star.motors(left, right + 40)
print("adjusting right")
if (rightCount - leftCount > 10):
a_star.motors(left + 40, right)
print("adjusting left")
else:
a_star.motors(left, right)
# look at the camera input
count = pixy.ccc_get_blocks(100, blocks)
if (count > 0):
sig = int(blocks[0].m_signature)
if (sig == 2):
print(sig)
# make sure block is large enough to be relevant and signature is red (1)
else:
if (blocks[0].m_width > 50 and blocks[0].m_height > 50):
a_star.motors(0, 0) # stops if a block is detected
encInitL, encInitR = a_star.read_encoders()
print("detected an obstacle")
print(blocks[0].m_signature)
avoidObstacle()
encFinL, encFinR = a_star.read_encoders()
# adjust right and left counts to account for extra distance
leftCount -= (encFinL - encInitL)
rightCount -= (encFinR - encInitR)
leftCount += int(35 * countToCM)
rightCount += int(35 * countToCM)
# reset time 2 /encoders
time2 = time.perf_counter()
encoders1 = encoders2
countAcc += 1
stop(1)
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))
frame = frame + 1
#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))
pixyTable.putNumber('sig', blocks[0].m_signature)
pixyTable.putNumber('x', blocks[0].m_x)
pixyTable.putNumber('y', blocks[0].m_y)
pixyTable.putNumber('width', blocks[0].m_width)
pixyTable.putNumber('height', blocks[0].m_height)
pixyTable.putNumber('angle', blocks[0].m_angle)
pixyTable.putNumber('age', blocks[0].m_age)
pixyTable.putBoolean('visible', True)
else:
pixyTable.putBoolean('visible', False)
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 forwardForDetection():
blocks = BlockArray(100)
#start pi control
a_star.motors(70, 70)
leftCount = 0 # use leftCount as goal encoder count
rightCount = 0
leftAcc = 0
rightAcc = 0
countAcc = 0
startTime = time.perf_counter()
encoders1 = a_star.read_encoders()
time2 = time.perf_counter()
while 1:
if ((time.perf_counter() - time2) >
0.05): # look at encoder values every 0.05 second
encoders2 = a_star.read_encoders()
righten = encoders2[1] - encoders1[1]
leften = encoders2[0] - encoders1[0]
if (righten > 0):
rightError = 35 - righten
rightCount += righten
if (leften > 0):
leftError = 35 - leften
leftCount += leften
if (countAcc > 5):
leftAcc += leftError
rightAcc += rightError
# calculate new motor parameters and apply to motor
left = piControl(leftError, leftAcc, 0)
right = piControl(rightError, rightAcc, 1)
# angle correction and motor control
if (leftCount - rightCount > 10):
a_star.motors(left, right + 40)
if (rightCount - leftCount > 10):
a_star.motors(left + 40, right)
else:
a_star.motors(left, right)
# reset time 2 /encoders
time2 = time.perf_counter()
encoders1 = encoders2
countAcc += 1
# look at the camera input
count = pixy.ccc_get_blocks(100, blocks)
if (count > 0):
sig = int(blocks[0].m_signature)
# make sure block is large enough to be relevant and signature is green
if (sig == 1):
print(sig)
else:
if (blocks[0].m_width > 50 and blocks[0].m_height > 50):
a_star.motors(0, 0) # stops if a block is detected
print("detected a corner")
print(blocks[0].m_signature)
break
stop(2)
return leftCount
print("Pixy2 Python SWIG Example -- Pan/Tilt Tracking Demo")
# Initialize pan/tilt controllers #
Pan_PID_Controller = PID_Controller(PAN_GAIN, 0, PAN_GAIN, True)
Tilt_PID_Controller = PID_Controller(TILT_GAIN, 0, TILT_GAIN, True)
pixy.init()
pixy.change_prog("color_connected_components")
Reset()
Blocks = BlockArray(1)
Frame = 0
while 1:
Count = pixy.ccc_get_blocks(1, Blocks)
if Count > 0:
Frame = Frame + 1
# Block acquisition logic #
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range(0, Count):
if Blocks[Index].m_index == Locked_Block_Index:
print('Frame %3d: Locked' % (Frame))
Display_Block(Index, Blocks[Index])
Pan_Offset = (pixy.get_frame_width() /
2) - Blocks[Index].m_x
Tilt_Offset = Blocks[Index].m_y - (
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
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 detect_color(frame):
#print ('colors')
if frame == 20:
pixy.change_prog("color_connected_components")
count = pixy.ccc_get_blocks(5, blocks)
def get_all_bot_positions(self):
pixy.change_prog("color_connected_components")
count = pixy.ccc_get_blocks(10, self.bots)
return (self.bots, count)
