def get_blocks(self):
self.m_count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, self.m_blocks)
# If negative blocks, something went wrong
if self.m_count < 0:
print 'Error: pixy_get_blocks() [%d] ' % self.m_count
pixy.pixy_error(self.m_count)
sys.exit(1)
if self.m_count == 0:
print "Detected no blocks"
return None
# package per signature
i = 0
blockmap = {}
for block in self.m_blocks:
if ignore(block):
continue
if block.signature not in blockmap:
blockmap[block.signature] = []
blockmap[block.signature].append(block)
if i >= self.m_count:
break
i += 1
return blockmap
def update_state(self):
self.number_of_blocks = pixy.pixy_get_blocks(self.blocks_to_consider,
self._BLOCKS)
if self.number_of_blocks > 0:
# CONSIDER: Use the middle of all big-enough blocks?
self.position = Point(self._BLOCKS[0].x, self._BLOCKS[0].y)
self.area = self._BLOCKS[0].area
else:
# CONSIDER: Estimate position based on previous position and velocity?
# Or just leave it unchanged (as here)?
pass
def findBlocks(self):
# Wait for blocks #
count = pixy_get_blocks(100, self.blocks)
found_blocks = []
if count > 0:
# Blocks found #
print 'frame %3d:' % (self.frame)
frame = self.frame + 1
for index in range(0, count):
block = self.blocks[index]
found_blocks.append(block)
# print '[BLOCK_TYPE=%d SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].type, blocks[index].signature, blocks[index].x, blocks[index].y, blocks[index].width, blocks[index].height)
# print 'sig: {0}, center at ({1}, {2}), height = {3}, width = {4}'.format(block.signature, block.x, block.y, block.height, block.width)
return found_blocks
def main():
global prevTime
# Vector in x-y plane
vector = [0,0]
# Transform matrix
coordinates = transform(vector)
# Initialize servos
pwm.setPWMFreq(60) # Set PWM frequencies to 60Hz
pwm.setPWM(servo1, 0, openLoop1)
pwm.setPWM(servo2, 0, openLoop2)
while True:
#t = Time()
#print "time: ", t - prevTime
count = pixy.pixy_get_blocks(1, blocks)
if count > 0:
# Save coordinates of ball
vector = [blocks.x, blocks.y]
# Use coordinates if valid
if vector[0] > 70 and vector[0] < 265 and vector[1] < 190:
# Transform coordinates
coordinates = transform(vector)
# Print statements for testing
if TEST:
print "transformed: ", coordinates
print "goal: ", GOAL
print "error: ", Error_x(coordinates[0]), Error_y(coordinates[1])
# Execute PID control
if PID:
PID_Control(coordinates[0], coordinates[1])
def loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global blocks, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError, panError_prev, distError_prev
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
# If negative blocks, something went wrong
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
# if more than one block
# Check which the largest block's signature and either do target chasing or
# line following
if count > 0:
lastTime = currentTime
print lastTime
#print block
numBlocks = 0
print "new loop!"
greenBlocks = []
sideLane = []
for idx in range(count):
block_ = blocks[idx]
#print block_.signature
if block_.signature == 3 :
greenBlocks.append(block_)
#print "X: " + str(block_.x) + " Y: " + str(block_.y) + " width: " + str(block_.width) + " height: " + str(block_.height) + " area: " + str(block_.height*block_.width)
elif block_.signature == 1 or block_.signature == 2:
sideLane.append(block_)
max_area = 0
area_idx = 0
if greenBlocks <> []:
# let's first get the biggest first
furthest_idx = 0
furthest = 600
gb_idx = 0
averageX = 0
for gb in greenBlocks:
block_area = gb.height*gb.width
if block_area > max_area:
max_area = block_area
area_idx = gb_idx
if gb.y < furthest :
furthest_idx = gb_idx
furthest = gb.y
averageX = averageX + gb.x
gb_idx = gb_idx + 1
averageX = averageX / gb_idx
#averageX = averageX + greenBlocks[area_idx].x / (gb_idx+1) # average weighted (a bit) w/ closest block
print "green block n# " + str(gb_idx) + ", X pos is " + str(greenBlocks[area_idx].x) + ", averageX is " + str(averageX)
#furthest_block = greenBlocks[furthest_idx]
#closest_block = greenBlocks[area_idx]
elif sideLane <> []:
sl_idx = 0
for sl in sideLane:
sidelane_area = sl.height*sl.width
if sidelane_area > max_area:
max_area = sidelane_area
area_idx = sl_idx
averageX = abs(319-sideLane[area_idx].x)
closest_block = sideLane[area_idx]
print "side lane n# " + str(area_idx) + ", X pos is " + str(sideLane[area_idx].x) + ", averageX is " + str(averageX)
else :
averageX = 50
print "no blocks, averageX is " + str(averageX)
singleObjTrack = 0
#target_block = closest_block
averageX = 50
def get_block(BLOCKS):
count = pixy.pixy_get_blocks(1, BLOCKS)
return BLOCKS[0] if count > 0 else None
def loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global blocks, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError, panError_prev, distError_prev
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
# If negative blocks, something went wrong
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
# if more than one block
# Check which the largest block's signature and either do target chasing or
# line following
if count > 0:
lastTime = currentTime
print lastTime
#print block
numBlocks = 0
print "new loop!"
greenBlocks = []
for idx in range(count):
block_ = blocks[idx]
#print block_.signature
if block_.signature == 3 :
greenBlocks.append(block_)
#print "X: " + str(block_.x) + " Y: " + str(block_.y) + " width: " + str(block_.width) + " height: " + str(block_.height) + " area: " + str(block_.height*block_.width)
# let's first get the biggest first
max_area = 0
area_idx = 0
furthest_idx = 0
furthest = 600
gb_idx = 0
averageX = 0
for gb in greenBlocks:
block_area = gb.height*gb.width
if block_area > max_area:
max_area = block_area
area_idx = gb_idx
if gb.y < furthest :
furthest_idx = gb_idx
furthest = gb.y
averageX = averageX + gb.x
gb_idx = gb_idx + 1
if gb_idx > 0 : # we don't have green blocks
averageX = averageX / gb_idx
#print gb_idx
#print greenBlocks[area_idx].x
furthest_block = greenBlocks[furthest_idx]
closest_block = greenBlocks[area_idx]
singleObjTrack = 0
# we select which object to track
target_block = closest_block
# target_block = furthest_block
if singleObjTrack == 1:
panError = PIXY_X_CENTER - target_block.x
objectDist = refSize1 / (2 * math.tan(math.radians(target_block.width * pix2ang_factor)))
else :
#panError = PIXY_X_CENTER - averageX
LPError_x.calculate(PIXY_X_CENTER - averageX)
panError = LPError_x.currentValue
objectDist = refSize1 / (2 * math.tan(math.radians(target_block.width * pix2ang_factor)))
pass
throttle = 0.3
diffDrive = 1.5* diffGain * abs(float(panError)) / PIXY_X_CENTER
#diffDrive = 0.6
#distError = objectDist - targetDist
#advance = driveGain * float(distError) / refDist
advance = 1
print "panError: " + str(panError) + " objectDist: " + str(objectDist) + " diffDrive: " + str(diffDrive)
#print objectDist
panLoop.update(panError)
# Update pixy's pan position
#pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
# if Pixy sees nothing recognizable, don't move.
time_difference = currentTime - lastTime
if time_difference.total_seconds() >= timeout:
throttle = 0.0
diffDrive = 1
print "throttle is " + str(throttle)
# this is turning to left
print "panLoop.m_pos: " + str(panLoop.m_pos)
if panLoop.m_pos > PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = panLoop.m_pos - PIXY_RCS_CENTER_POS
# <0 is turning left; currently only p-control is implemented
bias = - float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
# this is turning to right
elif panLoop.m_pos < PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
# >0 is turning left; currently only p-control is implemented
bias = float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
elapsedTime = currentTime - startTime
if elapsedTime.secs > waiTime:
drive()
else :
# we only adjust the beginning
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
else: # no green blocks
pass
return run_flag
def loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global blocks, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError_prev, distError_prev
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
# If negative blocks, something went wrong
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
# if more than one block
# Check which the largest block's signature and either do target chasing or
# line following
if count > 0:
lastTime = currentTime
# if the largest block is the object to pursue, then prioritize this behavior
if blocks[0].signature == 1:
panError = PIXY_X_CENTER - blocks[0].x
objectDist = refSize1 / (
2 * math.tan(math.radians(blocks[0].width * pix2ang_factor)))
throttle = 0.5
# amount of steering depends on how much deviation is there
diffDrive = diffGain * abs(float(panError)) / PIXY_X_CENTER
distError = objectDist - targetDist
# this is in float format with sign indicating advancing or retreating
advance = driveGain * float(distError) / refDist
# if Pixy sees a guideline, perform line following algorithm
elif blocks[0].signature == 2:
panError = PIXY_X_CENTER - blocks[0].x
throttle = 1.0
diffDrive = 0.6
# amount of steering depends on how much deviation is there
# diffDrive = diffGain * abs(float(turnError)) / PIXY_X_CENTER
# use full available throttle for charging forward
advance = 1
# if none of the blocks make sense, just pause
# else:
# panError = 0
# throttle = 0.0
# diffDrive = 1
panLoop.update(panError)
# Update pixy's pan position
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
# if Pixy sees nothing recognizable, don't move.
time_difference = currentTime - lastTime
if time_difference.total_seconds() >= timeout:
throttle = 0.0
diffDrive = 1
# this is turning to left
if panLoop.m_pos > PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = panLoop.m_pos - PIXY_RCS_CENTER_POS
# <0 is turning left; currently only p-control is implemented
bias = -float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
# this is turning to right
elif panLoop.m_pos < PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
# >0 is turning left; currently only p-control is implemented
bias = float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
drive()
return run_flag
def run(self):
from pixy import pixy_get_blocks
maxPoints = MAX_NUM_POINTS
pointDataSize = POINT_DATA_SIZE
CMD_PASS = 1001
CMD_NEW_FRAME = 1002
frame = 0
# Note: This is never called directly. It is called by Qt once the
# thread environment has been set up.
while not self.exiting:
numOfPointForFrame = pixy_get_blocks(100, self.blocks)
frame = frame + 1
if numOfPointForFrame > 0:
numOfPointForFrame = numOfPointForFrame if numOfPointForFrame < maxPoints else maxPoints
for index in xrange (0, numOfPointForFrame):
dataIndexOffset = 0
if index != 0:
dataIndexOffset = index * pointDataSize
# command to identify all point for frame have bee sent
cmd = CMD_PASS if index < ( numOfPointForFrame -1 ) else CMD_NEW_FRAME
tag = int(index)#blocks[index].signature
coltag = int(self.blocks[index].signature)
# flip for ui
y = self.blocks[index].x
x = self.blocks[index].y
h = self.blocks[index].width
w = self.blocks[index].height
if self.setClasifydata:
# setup nural pos tages done once to enable consistant traking
# of named points , once done smart clasification is used
'''
self._clasfyRegens.regenClasify(tag,coltag,x,y, w,h,dataIndexOffset)
'''
tag = self._clasfyRegens.regenClasify(tag,coltag,x,y)
if int(tag) != MOCAP_ROGE_DATA:
if self.filterDataFlag:
x,y,w,h = self.applyFiltering(tag, x, y, w, h)
##
# emit point data to update the UI draw
#
self.emit(SIGNAL("subFrame(int, long, long, long, long)"),
tag, x, y,w,h)
self.frameDataBuffer[dataIndexOffset] = tag
self.frameDataBuffer[dataIndexOffset+1] = coltag
self.frameDataBuffer[dataIndexOffset+2] = x
self.frameDataBuffer[dataIndexOffset+3] = y
self.frameDataBuffer[dataIndexOffset+4] = w
self.frameDataBuffer[dataIndexOffset+5] = h
else:
self.frameDataBuffer[dataIndexOffset] = 9999
self.frameDataBuffer[dataIndexOffset+1] = 9999
self.frameDataBuffer[dataIndexOffset+2] = 9999
self.frameDataBuffer[dataIndexOffset+3] = 9999
self.frameDataBuffer[dataIndexOffset+4] = 9999
self.frameDataBuffer[dataIndexOffset+5] = 9999
if cmd == CMD_NEW_FRAME:
break
if self.setClasifydata:
self._clasfyRegens.updateBoxesForNextFrame()
self.emit(SIGNAL("frameEnd()"))
if self.openRecording:
self.openRecording.write(str(self.frameDataBuffer) + "\n")
if self._serverThred:
self.packer_data.pack_into(self.sendDataBuffer, 0, *self.frameDataBuffer)
for key in self._serverThred.data_queues.keys():
self._serverThred.data_queues[key].append(self.sendDataBuffer)
fpsclock.sleep()
def loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global blocks, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError, panError_prev, distError_prev
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
# If negative blocks, something went wrong
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
# if more than one block
# Check which the largest block's signature and either do target chasing or
# line following
if count > 0:
lastTime = currentTime
print lastTime
#print block
numBlocks = 0
print "new loop!"
greenBlocks = []
for idx in range(count):
block_ = blocks[idx]
#print block_.signature
if block_.signature == 3:
greenBlocks.append(block_)
#print "X: " + str(block_.x) + " Y: " + str(block_.y) + " width: " + str(block_.width) + " height: " + str(block_.height) + " area: " + str(block_.height*block_.width)
# let's first get the biggest first
max_area = 0
area_idx = 0
furthest_idx = 0
furthest = 600
gb_idx = 0
averageX = 0
for gb in greenBlocks:
block_area = gb.height * gb.width
if block_area > max_area:
max_area = block_area
area_idx = gb_idx
if gb.y < furthest:
furthest_idx = gb_idx
furthest = gb.y
averageX = averageX + gb.x
gb_idx = gb_idx + 1
if gb_idx > 0: # we don't have green blocks
averageX = averageX / gb_idx
#print gb_idx
#print greenBlocks[area_idx].x
furthest_block = greenBlocks[furthest_idx]
closest_block = greenBlocks[area_idx]
singleObjTrack = 0
# we select which object to track
target_block = closest_block
# target_block = furthest_block
if singleObjTrack == 1:
panError = PIXY_X_CENTER - target_block.x
objectDist = refSize1 / (2 * math.tan(
math.radians(target_block.width * pix2ang_factor)))
else:
panErrorRaw = PIXY_X_CENTER - averageX
LPError_x.calculate(PIXY_X_CENTER - averageX)
panError = LPError_x.currentValue
objectDist = refSize1 / (2 * math.tan(
math.radians(target_block.width * pix2ang_factor)))
Grad_x.calculate(panError)
pass
throttle = 0.3
diffDrive = 1.5 * diffGain * abs(float(panError)) / PIXY_X_CENTER
#diffDrive = 0.6
#distError = objectDist - targetDist
#advance = driveGain * float(distError) / refDist
advance = 1
print "panError: " + str(panError) + " objectDist: " + str(
objectDist) + " diffDrive: " + str(diffDrive)
#print objectDist
panLoop.update(panErrorRaw)
# Update pixy's pan position
#pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
# if Pixy sees nothing recognizable, don't move.
time_difference = currentTime - lastTime
if time_difference.total_seconds() >= timeout:
throttle = 0.0
diffDrive = 1
print "throttle is " + str(throttle)
drive_original = False
elapsedTime = currentTime - startTime
if elapsedTime.seconds > waitTime:
advance = 1
if drive_original == True:
# this is turning to left
# print "panLoop.m_pos: " + str(panLoop.m_pos)
if panLoop.m_pos > PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = panLoop.m_pos - PIXY_RCS_CENTER_POS
# <0 is turning left; currently only p-control is implemented
bias = -float(turnError) / float(
PIXY_RCS_CENTER_POS) * h_pgain
# this is turning to right
elif panLoop.m_pos < PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
# >0 is turning left; currently only p-control is implemented
bias = float(turnError) / float(
PIXY_RCS_CENTER_POS) * h_pgain
drive()
else: # we use our new way
turnError = Grad_x.uOut
h_pgain = 1 # for now we don't do anything
bias = float(turnError) * h_pgain
drive_new()
else:
# we only adjust the beginning
advance = 0
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
drive()
drive_new()
else: # no green blocks
pass
return run_flag
# Connect the socket to the port where the server is listening
server_address = ('HARD CODED TO MY PC NAME', 10000)
print >>sys.stderr, 'connecting to %s port %s' % server_address
sock.connect(server_address)
##
# setup command queue thred
command_queue = Queue.Queue()
input_thread = threading.Thread(target=SeverCommad.wait, args=(command_queue,sock))
input_thread.daemon = True
input_thread.start()
try:
while True:
frameData = buildFrameData(maxPoints,pointDataSize)
numOfPointForFrame = pixy_get_blocks(100, blocks)
frame = frame + 1
if numOfPointForFrame > 0:
numOfPointForFrame = numOfPointForFrame if numOfPointForFrame < maxPoints else maxPoints
for index in xrange (0, numOfPointForFrame):
# command to identify all point for frame have bee sent
cmd = CMD_PASS if index < ( numOfPointForFrame -1 ) else CMD_NEW_FRAME
tag = index#blocks[index].signature
colourID = blocks[index].signature
x = blocks[index].x
y = blocks[index].y
w = blocks[index].width
h = blocks[index].height
dataIndexOffset = 0
if tag != 0:
def loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global blocks, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError_prev, distError_prev
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
# If negative blocks, something went wrong
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
# if more than one block
# Check which the largest block's signature and either do target chasing or
# line following
if count > 0:
lastTime = currentTime
# if the largest block is the object to pursue, then prioritize this behavior
if blocks[0].signature == 1:
panError = PIXY_X_CENTER - blocks[0].x
objectDist = refSize1 / (2 * math.tan(math.radians(blocks[0].width * pix2ang_factor)))
throttle = 0.5
# amount of steering depends on how much deviation is there
diffDrive = diffGain * abs(float(panError)) / PIXY_X_CENTER
distError = objectDist - targetDist
# this is in float format with sign indicating advancing or retreating
advance = driveGain * float(distError) / refDist
# if Pixy sees a guideline, perform line following algorithm
elif blocks[0].signature == 2:
panError = PIXY_X_CENTER-blocks[0].x
throttle = 1.0
diffDrive = 0.6
# amount of steering depends on how much deviation is there
# diffDrive = diffGain * abs(float(turnError)) / PIXY_X_CENTER
# use full available throttle for charging forward
advance = 1
# if none of the blocks make sense, just pause
# else:
# panError = 0
# throttle = 0.0
# diffDrive = 1
panLoop.update(panError)
# Update pixy's pan position
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
# if Pixy sees nothing recognizable, don't move.
time_difference = currentTime - lastTime
if time_difference.total_seconds() >= timeout:
throttle = 0.0
diffDrive = 1
# this is turning to left
if panLoop.m_pos > PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = panLoop.m_pos - PIXY_RCS_CENTER_POS
# <0 is turning left; currently only p-control is implemented
bias = - float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
# this is turning to right
elif panLoop.m_pos < PIXY_RCS_CENTER_POS:
# should be still int32_t
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
# >0 is turning left; currently only p-control is implemented
bias = float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
drive_rr()
return run_flag
from ctypes import *
# ==========================================================================
# Prints coordinates of object
# ==========================================================================
# Initialize Pixy Interpreter thread #
pixy.pixy_init()
# NOT USED #
class Blocks (Structure):
_fields_ = [ ("type", c_uint),
("signature", c_uint),
("x", c_uint),
("y", c_uint),
("width", c_uint),
("height", c_uint),
("angle", c_uint) ]
# Create block object #
blocks = pixy.Block()
# Wait for blocks #
while True:
count = pixy.pixy_get_blocks(1, blocks)
if count > 0:
# Blocks found #
print '[X=%3d Y=%3d]' % (blocks.x, blocks.y)
def hailmary(block_count):
global throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError_prev, distError_prev
if len(block_count) == 0:
print "can't do hailmary with no blocks"
return
print "Attempting to hail Mary with %d block history" % len(block_count)
leftTotal = 0.0
rightTotal = 0.0
for (left, right) in block_count:
leftTotal += left
rightTotal += right
avgLeft = leftTotal / len(block_count)
avgRight = rightTotal / len(block_count)
print "Past %d frames had avg red blocks on (left=%d, right=%d)" % (
AVG_N, avgLeft, avgRight)
# Turn towards the preponderance of red blocks
lastTime = currentTime
if avgLeft > avgRight:
print "Executing blind left turn"
bias = -1
elif avgRight > avgLeft:
print "Executing blind right turn"
bias = 1
else:
bias = 0
if bias != 0:
# Slow forward turn
advance = 1
throttle = 0.5
diffDrive = 1
# Reset pixy's head
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, PIXY_RCS_CENTER_POS)
normalDrive = False
#If hailmary didn't work, hold on to your rosary beads, we're going hunting!
else:
# Need some kind of hunting behavior here
#This situation usally arises when we're on a curve in one direction and want to sharply turn in the other direction
#pan the camera until we find a red block, then go straight toward it.
print "Execute search and destroy"
i = 0
noRedBlocks = True
advance = -1 #if we can't find it, retreat
while (noRedBlocks == True and i <= PIXY_RCS_MAX_POS):
#while redblock not found
#pan for red block
print "Panning for red block. i:%d" % (i)
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, i)
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
largestBlock = blocks[0]
#code stolen from earlier in file, maybe turn it into a function?
if largestBlock.signature == 2:
noRedBlocks = False
panError = PIXY_X_CENTER - blocks[0].x
p = panError / 40.0
if p < 0:
p = -p
if p > 0.8:
p = 0.8
throttle = 0.9 - p
diffDrive = 0.6
print "p: %f, panError: %d, turnError: %d" % (p, panError,
turnError)
advance = 1
i = i + 10
def get_block():
count = pixy.pixy_get_blocks(1, BLOCKS)
if count == 0:
return None
return BLOCKS[0]
def loop():
global blocks, throttle, diffGain, bias, currentTime, lastTime
# TODO python equivilant?
currentTime = datetime.now()
while not pixy.pixy_blocks_are_new() and run_flag:
pass
count = pixy.pixy_get_blocks(BLOCK_BUFFER_SIZE, blocks)
if count < 0:
print 'Error: pixy_get_blocks() [%d] ' % count
pixy.pixy_error(count)
sys.exit(1)
if count > 0:
lastTime = currentTime
# if the largest block is the object to pursue, then prioritize this behavior
if (blocks[0].signature == 1):
panError = PIXY_X_CENTER - blocks[0].x
tiltError = blocks[0].y - PIXY_Y_CENTER
# the target is far and we must advance
if (blocks[0].width < targetSize):
# charge forward
throttle = 100 # charge forward
distError = targetSize - blocks[0].width
# this is in float format
diffGain = 1 - driveGain * float(distError) / targetSize
# the target is too close and we must back off
elif (blocks[0].width > targetSize):
# retreat
throttle = -100
distError = blocks[0].width - targetSize
# this is in float format
diffGain = 1 - float(distError) / targetSize
# this is line following algorithm
elif (blocks[0].signature == 2):
panError = PIXY_X_CENTER-blocks[0].x
tiltError = blocks[0].y-PIXY_Y_CENTER
# charge forward
throttle = 100
diffGain = 0.3
# if none of the blocks make sense, just pause
else:
panError = 0
tiltError = 0
throttle = 0
diffGain = 1
panLoop.update(panError)
tiltLoop.update(tiltError)
set_position_result = pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, panLoop.m_pos)
set_position_result = pixy.pixy_rcs_set_position(PIXY_RCS_TILT_CHANNEL, tiltLoop.m_pos)
# TODO implement this?
# if Pixy sees nothing recognizable, don't move.
time_difference = currentTime - lastTime
if (time_difference.total_seconds() >= timeout) :
print time_difference.total_seconds(), timeout
throttle = 0
diffGain = 1
# this is turning to left
if (panLoop.m_pos > PIXY_RCS_CENTER_POS) :
# should be still int32_t
turnError = panLoop.m_pos - PIXY_RCS_CENTER_POS
# <0.5 is turning left
bias = - float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
# this is turning to right
elif (panLoop.m_pos < PIXY_RCS_CENTER_POS):
# should be still int32_t
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
# <0.5 is turning left
bias = float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain
drive()
return run_flag
