def setup():
"""
One time setup. Inialize pixy and set sigint handler
"""
pixy_init_status = pixy.pixy_init()
if pixy_init_status != 0:
print 'Error: pixy_init() [%d] ' % pixy_init_status
pixy.pixy_error(pixy_init_status)
return
else:
print "Pixy setup OK"
signal.signal(signal.SIGINT, handle_SIGINT)
pixy.pixy_cam_set_brightness(BRIGHTNESS)
pixy.pixy_rcs_set_position(PIXY_RCS_PAN_CHANNEL, PIXY_RCS_CENTER_POS)
if chatty:
sayNow("I may not be the fastest but I have style")
#say("SLEEP 2")
time.sleep(2)
panError = PIXY_X_CENTER - averageX
#objectDist = refSize1 / (2 * math.tan(math.radians(target_block.width * pix2ang_factor)))
pass
throttle = 0.5
#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
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 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
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
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 loop():
"""
Main loop, Gets blocks from pixy, analyzes target location,
chooses action for robot and sends instruction to motors
"""
global startTime, throttle, diffDrive, diffGain, bias, advance, turnError, currentTime, lastTime, objectDist, distError, panError_prev, distError_prev, firstPass, pid_bias, last_turn
currentTime = datetime.now()
# If no new blocks, don't do anything
while not pixy.pixy_blocks_are_new() and run_flag:
pass
if firstPass:
say("Here goes")
startTime = time.time()
firstPass = False
scene.get_frame()
if scene.blocksSeen():
lastTime = currentTime
if finale:
refuseToPlay()
return False
p = scene.panError
if p < 0:
p = -p
incr = p / 300.0
#print "panError: %f, incr: %f" % (scene.panError, incr)
#if incr > 0.65:
# incr = 0.65
throttle = initThrottle # - incr / 1.5
diffDrive = diffDriveStraight + incr
# 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
panLoop.update(scene.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 not scene.seeCenter(): #time_difference.total_seconds() >= timeout:
print "Stopping since see nothing"
throttle = 0.0
diffDrive = 1
turn = 0
# 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
turn = float(turnError) / float(PIXY_RCS_CENTER_POS)
# 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
turn = -float(turnError) / float(PIXY_RCS_CENTER_POS)
pid.setPoint(0)
pid_bias = pid.update(turn)
#print "PID controller: SP=%2.2f PV=%2.2f -> OP=%2.2f" % (0, turn, pid_bias)
last_turn = turn
bias = pid_bias # use PID controller on turn bias
# TODO: parameterize drive()
if bias < -0.3:
say("Going left")
if bias > 0.3:
say("Going right")
drive()
return run_flag
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