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, 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 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 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