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)
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, panLoop, killed, lastFire
global targetTime, targetTimeDifference, turnErrorAccumulator, exploreTime, exploreTimeDifference, turnErrorPrevious
#if ser.in_waiting:
# print "Reading line from serial.."
# code = ser.readline().rstrip()
# print "Got IR code %s" % code
# killed = True
#if code=="58391E4E" or code=="9DF14DB3" or code=="68B92":
# killed = True
#
#if code=="E4F74E5A" or code=="A8FA9FFD":
# killed = False
### Don't Change this ####
#if killed:
# print "I'm hit!"
#motors.setSpeeds(0, 0)
#time.sleep(5)
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 count == 0:
print "no 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
time_difference = currentTime - lastFire
if time_difference.total_seconds() >= 1:
print "Fire!"
ser.write("FIRE\n")
lastFire = currentTime
lastTime = currentTime
# if the largest block is the object to pursue, then prioritize this behavior
throttle = 0
panError = 0
#first get biggest blue block
biggestGreenBlockIndex = 999
biggestTeamBlockIndex = 999
biggestOpponentBlockIndex = 999
currentIndex = 0
targetFound = -1
while currentIndex < 10:
if blocks[
currentIndex].signature == GREEN and biggestGreenBlockIndex == 999:
biggestGreenBlockIndex = currentIndex
if blocks[
currentIndex].signature == Opponent and biggestOpponentBlockIndex == 999:
biggestOpponentBlockIndex = currentIndex
if blocks[
currentIndex].signature == Team and biggestTeamBlockIndex == 999:
biggestTeamBlockIndex = currentIndex
currentIndex = currentIndex + 1
avoidCollision = 0
if biggestTeamBlockIndex < 999:
if blocks[biggestTeamBlockIndex].height > 0.666 * PIXY_MAX_Y:
avoidCollision = 1
targetBlockIndex = min(biggestGreenBlockIndex, biggestOpponentBlockIndex)
targetBigEnough = 0
if targetBlockIndex != 999:
if blocks[targetBlockIndex].width > 30:
targetBigEnough = 1
#print('biggest target index', min(biggestGreenBlockIndex,biggestOpponentBlockIndex), 'time', targetTimeDifference, 'team index', biggestTeamBlockIndex);
if ((biggestGreenBlockIndex < biggestTeamBlockIndex
or biggestOpponentBlockIndex < biggestTeamBlockIndex)
and targetTimeDifference < 5 and avoidCollision == 0
and targetBigEnough == 1):
#if blocks[biggestGreenBlockIndex].signature == GREEN or blocks[biggestOpponentBlockIndex].signature == BLUE: #do we need this if statement?
print("attacking")
targetBlockIndex = min(biggestGreenBlockIndex,
biggestOpponentBlockIndex)
if targetTime == 0:
targetTime = currentTime
turnErrorAccumulator = 0
targetTimeDifference = 0
if targetTimeDifference <= 1:
throttle = 0.15
objectDist = 300
else:
throttle = 0.5
objectDist = refSize / (2 * math.tan(
math.radians(blocks[targetBlockIndex].width * pix2ang_factor)))
diffGain = 1
#print( "Found Green signature",targetBlockIndex);
panError = PIXY_X_CENTER - (
blocks[targetBlockIndex].x
) # +int(35*math.sin(5*targetTimeDifference))) #100 - blocks[biggestGreenBlockIndex].x
temp = blocks[targetBlockIndex].x + 10 * math.sin(
5 * targetTimeDifference)
#print ('object dist', objectDist, 'width',blocks[targetBlockIndex].width, 'newX', temp)
# 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 #max(1,driveGain * float(distError) / refDist)
#print('advance', advance, 'diffDrive', diffDrive)
targetTimeDifference = (currentTime - targetTime).total_seconds()
exploreTime = 0
# if none of the blocks make sense, just pause
else:
if exploreTime == 0:
exploreTime = currentTime
elif exploreTimeDifference < 1.5:
panError = 0
#PIXY_X_CENTER-PIXY_MAX_X
diffDrive = 0.5 #diffGain * abs(float(PIXY_X_CENTER-PIXY_MAX_X)) / PIXY_X_CENTER
throttle = 0.4
bias = 1
advance = 1
print("spinning")
elif exploreTimeDifference < 3:
panError = 0
throttle = 0.6
bias = 0
diffDrive = 0
advance = 1
print("not spinning")
targetTime = 0
#turnErrorAccumulator = 0
targetTimeDifference = 0
print('no target blocks', 'biggest red block index',
biggestTeamBlockIndex)
print(exploreTimeDifference)
exploreTimeDifference = (currentTime - exploreTime).total_seconds()
if exploreTimeDifference >= 3:
exploreTime = 0
exploreTimeDifference = 0
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 "4"
# this is turning to left
if panLoop.m_pos != PIXY_RCS_CENTER_POS:
turnError = PIXY_RCS_CENTER_POS - panLoop.m_pos
turnErrorAccumulator += turnError
derivative = (turnError -
turnErrorPrevious) / float(PIXY_RCS_CENTER_POS)
turnErrorPrevious = turnError
bias = float(turnError) / float(
PIXY_RCS_CENTER_POS) * h_pgain + i_control * float(
turnErrorAccumulator) / float(
PIXY_RCS_CENTER_POS) + d_control * derivative
#print('bias', bias)
## 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
## turnErrorAccumulator -= turnError
## derivative = (turnError - turnErrorPrevious)/float(PIXY_RCS_CENTER_POS);
## turnErrorPrevious = turnError;
## bias = - float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain + i_control*float(turnErrorAccumulator)/float(PIXY_RCS_CENTER_POS)
## print('turn left bias', bias)
## #print "5"
##
## # 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
## turnErrorAccumulator += turnError
## bias = float(turnError) / float(PIXY_RCS_CENTER_POS) * h_pgain+i_control*float(turnErrorAccumulator) / float(PIXY_RCS_CENTER_POS) + d_control*
## print('turn right bias', bias)
# print "6"
drive()
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():
"""
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, panLoop, killed, lastFire
if ser.in_waiting:
print "Reading line from serial.."
code = ser.readline().rstrip()
print "Got IR code %s" % code
killed = True
#if code=="58391E4E" or code=="9DF14DB3" or code=="68B92":
# killed = True
#
#if code=="E4F74E5A" or code=="A8FA9FFD":
# killed = False
if killed:
print "I'm hit!"
motors.setSpeeds(0, 0)
time.sleep(5)
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:
time_difference = currentTime - lastFire
if time_difference.total_seconds() >= 1:
print "Fire!"
ser.write("FIRE\n")
lastFire = currentTime
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