def track():
global taskInput
global Locked_On_Block
global Locked_Block_Index
global Blocks
global Count
global Pan
# while taskInput != "S":
Count = pixy.ccc_get_blocks (100, Blocks)
if Count > 0:
# Block acquisition logic #
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range (0, Count):
Pan_Offset = (pixy.get_frame_width () / 2) - Blocks[Index].m_x;
Tilt_Offset = Blocks[Index].m_y - (pixy.get_frame_height () / 2)
Pan_PID_Controller.Update (Pan_Offset)
Tilt_PID_Controller.Update (Tilt_Offset)
pixy.set_servos (Pan_PID_Controller.Command, Tilt_PID_Controller.Command)
Pan = Pan_PID_Controller.Command;
else:
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK:
Locked_Block_Index = Blocks[0].m_index;
Locked_On_Block = True
else:
Reset ()
def join(self, timeout=None):
""" Stop the thread. """
self._stopevent.set()
pixy.set_lamp(0, 0)
pixy.set_servos(500, 500)
threading.Thread.join(self, timeout)
drive = clsDrive.Drive()
drive.stop()
def run(self):
drive = clsDrive.Drive()
control = clsDrive.Control()
point = 0
pixy.init()
pixy.change_prog("color_connected_components")
pixy.set_lamp(0, 0)
pixy.set_servos(500, 650)
blocks = BlockArray(100)
while not self._stopevent.isSet():
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
x = 0
y = 0
width = 0
for index in range(0, count):
curBlock = blocks[index]
if curBlock.m_signature == 1 and curBlock.m_width > width:
width = curBlock.m_width
x = curBlock.m_x
y = curBlock.m_y
if width > 0:
control.maze(x, y, width)
if control.stop == True:
if control.progress == 0 or control.progress == 1 or control.progress == 4 or control.progress == 5 or control.progress == 6:
drive.turnLeft()
elif control.progress == 2 or control.progress == 3 or control.progress == 7:
drive.turnRight()
control.progress += 1
elif control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 0.7
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
def run(self):
drive = clsDrive.Drive()
control = clsDrive.Control()
pixy.init()
pixy.change_prog("line")
pixy.set_servos(500, 800)
pixy.set_lamp(1, 0)
vectors = VectorArray(100)
intersections = IntersectionLineArray(100)
while not self._stopevent.isSet():
line_get_all_features()
i_count = line_get_intersections(100, intersections)
v_count = line_get_vectors(100, vectors)
print("Scanning")
if i_count > 0 or v_count > 0:
# print('frame %3d:' % (frame))
for index in range(0, v_count):
control.lineFollow(vectors[index].m_x0,
vectors[index].m_y0,
vectors[index].m_x1,
vectors[index].m_y1)
print(str(control.frame))
if control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 1
control.stop = False
drive.HarDrive(control)
else:
control.angle = 0
control.speed = 0.3
control.stop = False
drive.HarDrive(control)
drive.stop()
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range(0, Count):
if Blocks[Index].m_index == Locked_Block_Index:
print('Frame %3d: Locked' % (Frame))
Display_Block(Index, Blocks[Index])
Pan_Offset = (pixy.get_frame_width() /
2) - Blocks[Index].m_x
Tilt_Offset = Blocks[Index].m_y - (
pixy.get_frame_height() / 2)
Pan_PID_Controller.Update(Pan_Offset)
Tilt_PID_Controller.Update(Tilt_Offset)
pixy.set_servos(Pan_PID_Controller.Command,
Tilt_PID_Controller.Command)
else:
print('Frame %3d:' % (Frame))
# Display all the blocks in the frame #
for Index in range(0, Count):
Display_Block(Index, Blocks[Index])
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK:
Locked_Block_Index = Blocks[0].m_index
Locked_On_Block = True
else:
Reset()
# Block acquisition logic #
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range (0, Count):
if Blocks[Index].m_index == Locked_Block_Index:
print ('Frame %3d: Locked' % (Frame))
Display_Block (Index, Blocks[Index], Pan_PID_Controller.Command)
Pan_Offset = (pixy.get_frame_width () / 2) - Blocks[Index].m_x;
Tilt_Offset = Blocks[Index].m_y - (pixy.get_frame_height () / 2)
Pan_PID_Controller.Update (Pan_Offset)
Tilt_PID_Controller.Update (Tilt_Offset)
pixy.set_servos (round(Pan_PID_Controller.Command), round(Tilt_PID_Controller.Command))
else:
print ('Frame %3d:' % (Frame))
# Display all the blocks in the frame #
for Index in range (0, Count):
Display_Block (Index, Blocks[Index], 2048)
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK:
Locked_Block_Index = Blocks[0].m_index;
Locked_On_Block = True
else:
Reset ()
def run(self):
'''pixy.init()
pixy.change_prog("color_connected_components");
pixy.set_lamp(1, 0)
drive = clsDrive.Drive()
control = clsDrive.Control()
blocks = BlockArray(100)
while not self._stopevent.isSet( ):
count = pixy.ccc_get_blocks(100, blocks)
if count > 0:
for index in range(0, count):
curBlock = blocks[index]
print("x " + str(curBlock.m_x) + "- y " + str(curBlock.m_y))
#control.neb(curBlock.m_x, curBlock.m_y)
#control.speed = 0.5
#drive.HarDrive(control)
else:
#drive.joltLeft()
print("Jolt")
'''
tof = VL53L1X.VL53L1X(i2c_bus=1, i2c_address=0x29)
tof.open() # Initialise the i2c bus and configure the sensor
tof.start_ranging(
1
) # Start ranging, 1 = Short Range, 2 = Medium Range, 3 = Long Range
drive = clsDrive.Drive()
control = clsDrive.Control()
control.progress = 2
pixy.init()
pixy.change_prog("color_connected_components")
pixy.set_lamp(0, 0)
pixy.set_servos(500, 650)
blocks = BlockArray(100)
while not self._stopevent.isSet():
count = pixy.ccc_get_blocks(100, blocks)
if count > 0 and control.progress < 6:
x = 0
y = 0
width = 0
for index in range(0, count):
curBlock = blocks[index]
if curBlock.m_signature == control.progress and curBlock.m_width > width:
width = curBlock.m_width
x = curBlock.m_x
y = curBlock.m_y
if width > 0:
control.distance = tof.get_distance()
#print(str(control.distance))
if control.distance < 100:
if control.progress == 5:
drive.stop()
else:
control.angle = 180
control.speed = 0.5
control.stop = False
drive.HarDrive(control)
sleep(1)
drive.stop()
control.progress += 1
else:
#print("control.neb")
print(str(width))
control.neb(x, y, width)
#print(str(control.frame))
if control.frame == 2:
drive.joltRight()
elif control.frame == 0:
drive.joltLeft()
else:
control.angle = 0
control.speed = 0.5
control.stop = False
drive.HarDrive(control)
else:
drive.joltLeft()
elif control.progress == 6:
drive.stop()
break
else:
drive.joltLeft()
tof.close()
if Locked_On_Block:
# Find the block that we are locked to #
for Index in range(0, Count):
if Blocks[Index].m_index == Locked_Block_Index:
print('Frame %3d: Locked' % (Frame))
Display_Block(Index, Blocks[Index])
Pan_Offset = (pixy.get_frame_width() /
2) - Blocks[Index].m_x
Tilt_Offset = Blocks[Index].m_y - (
pixy.get_frame_height() / 2)
Pan_PID_Controller.Update(Pan_Offset)
Tilt_PID_Controller.Update(Tilt_Offset)
pixy.set_servos(int(Pan_PID_Controller.Command),
int(Tilt_PID_Controller.Command))
else:
print('Frame %3d:' % (Frame))
# Display all the blocks in the frame #
for Index in range(0, Count):
Display_Block(Index, Blocks[Index])
# Find an acceptable block to lock on to #
if Blocks[0].m_age > MINIMUM_BLOCK_AGE_TO_LOCK:
Locked_Block_Index = Blocks[0].m_index
Locked_On_Block = True
else:
Reset()
