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 setup():
"""
One time setup. Inialize pixy and set sigint handler
"""
global blocks
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"
blocks = pixy.BlockArray(BLOCK_BUFFER_SIZE)
signal.signal(signal.SIGINT, handle_SIGINT)
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 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