def __init__(self, racerBot, controller): # pixy parameters self.pixyMinX = 0 self.pixyMaxX = 316 self.pixyMinY = 0 self.pixyMaxY = 208 self.pixyCenterX = (self.pixyMaxX - self.pixyMinX) / 2 self.pixyCenterY = (self.pixyMaxY - self.pixyMinY) / 2 self.targetSignature = 1 self.calibrationSignature = 2 # servo parameters self.servoMinPos = -90 self.servoMaxPos = 90 self.servoPos = 0 # variables for pixy self.oldBlocks = pixy.BlockArray(100) self.newBlocks = pixy.BlockArray(100) self.oldCount = pixy.ccc_get_blocks(100, self.oldBlocks) self.newCount = pixy.ccc_get_blocks(100, self.newBlocks) # controller needs to have an update function must have an # update function that takes in error (in X) and returns # change in angle to send to the servo self.gimbal = controller self.calibrationController = PID_controller(0.06, 0, 0.06) # pixyRacer to control self.racerBot = racerBot # tuning curve parameters self.minAngle = 0 self.maxAngle = 0 self.targetMinAngle = -35 self.targetMaxAngle = 35
def f_getBlocks(): global run class Blocks(Structure): _fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint), ("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint), ("m_index", c_uint), ("m_age", c_uint)] blocks = BlockArray(100) while run == 1: count = pixy.ccc_get_blocks(100, blocks) if count > 0: arr = [] arr.append(blocks[0].m_signature) arr.append(blocks[0].m_width) arr.append(blocks[0].m_height) arr.append(blocks[0].m_x) arr.append(blocks[0].m_y) arr_pub = Int32MultiArray(data=arr) print(str(arr[0]) + " erkannt") f_publish(arr_pub) rate.sleep() else: arr = [] arr.append(0) arr_pub = Int32MultiArray(data=arr) f_publish(arr_pub) rate.sleep() print("Nichts erkannt") print("ende")
def get(Signatur): blocks = BlockArray(100) frame = 0 signal = 0, 0 signal1 = 0, 0 signal2 = 0, 0 count = pixy.ccc_get_blocks(100, blocks) if count > 0: frame = frame + 1 for index in range(0, count): logging.debug('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height)) signal = (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height) if signal[0] == 1 or signal[0] == 0: signal1 = signal if signal[0] == 2 or signal[0] == 0: signal2 = signal if Signatur == 1: return (signal1) elif Signatur == 2: return (signal2)
def get_pixy(): count = pixy.ccc_get_blocks (100, blocks) if count > 0: # print('frame %3d:' % (frame)) # frame = frame + 1 for index in range (0, count): #Anpassung des Pixy-Koordinatensystems an das Roboterkoordinatensystem ''' (0|0) ___________________(315|0) (157.5|-103.5) _______(157.5|103.5) | | | | | | | | | | --> | (0|0) | | | | | | | | | (0|207)__________________(315|207) (-157.5|-103.5)_______(-157.5|103.5) ^ ___|___ |PIXY| ''' X_ball = blocks[index].m_y - 103.5 #Y_pixy_max / 2 Y_ball = (blocks[index].m_x - 157.5)*(-1) #X_pixy_max / 2 print('(X|Y)= ',X_ball,'|',Y_ball) return X_ball,Y_ball
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 trackSignature(self, _signature): print("Starting to track signature") pixy.change_prog("color_connected_components") print("Changed program to CCC") data = pixy.ccc_get_blocks(100, self.blocks) print("Data gathered") #Find the target with largest area to return return dataFormat.format()
def getBlocks(self): blocks = BlockArray(100) count = pixy.ccc_get_blocks(100, blocks) if count > 0: for index in range(0, count): centerx = blocks[index].m_x + blocks[index].m_width / 2 centery = blocks[index].m_y - blocks[index].m_height / 2 return 1, centerx, centery, blocks[index].m_width, blocks[ index].m_height return None, None, None, None, None
def get_block_arrays(block_array, colours=None, max_blocks=100): """Return a list of blocks for each coloru in for *colours*. They are returned as a len(colours) array of arrays in the order specified. If colours is None, all signatures are returned. If no blocks are found, None is returned. Each item in each array takes the form: [x, y, width, height, index, age] Parameters: - block_array This is the BlockArray() needed to collect the blocks. - colours A list of all colour signatures needed. - max_blocks The maximum number of blocks fetched. WARNING: Not yet tested. """ # Build signature mask. if colours is None: sig_mask = [True] * 10 else: sig_mask = [False] * 10 for colour in colours: sig_mask[colour] = True # Now proceed to collect data. no_blocks = pixy.ccc_get_blocks(max_blocks, block_array) if no_blocks == 0: return None # Set up lists. block_lists = [[]] * 10 some_of_interest = False for i in range(no_blocks): sig = block_array[i].m_signature if sig_mask[sig]: some_of_interest = True block_lists[sig].append([ block_array[i].m_x, block_array[i].m_y, block_array[i].m_width, block_array[i].m_height, block_array[i].m_index, block_array[i].m_age ]) if not some_of_interest: # Nothing of interest found. return None to_return = [] for colour in colours: to_return.append(block_lists[colour]) return to_return
def get_pixy(): count = pixy.ccc_get_blocks(100, blocks) if count > 0: # print('frame %3d:' % (frame)) # frame = frame + 1 for index in range(0, count): # print('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height)) X_ball = blocks[index].m_x Y_ball = blocks[index].m_y print('(X|Y)= ', X_ball, '|', Y_ball)
def read_raw(self): global Ball_Status if self.sim_mode == True: X_ball, Y_ball = test_data else: count = pixy.ccc_get_blocks(100, self.blocks) if count > 0: for index in range(0, count): X_ball = self.blocks[index].m_x Y_ball = self.blocks[index].m_y #if X_ball is not None: #print('(X|Y)= ', X_ball, '|', Y_ball) return X_ball, Y_ball
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 getBlocks(self): """Get various information about the most prominent circle in the image Return 1: centrex [int]; the x centre of the nearest circle Return 2: centrey [int]; the y centre of the nearest circle Return 3: blockWidth [int]; Return 4: blockHeight [int]; """ blocks = BlockArray(100) count = pixy.ccc_get_blocks(100, blocks) if count > 0: for index in range (0, count): centerx = blocks[index].m_x + blocks[index].m_width/2 centery = blocks[index].m_y - blocks[index].m_height/2 return 1,centerx, centery, blocks[index].m_width, blocks[index].m_height return None,None,None,None,None
def get_block_object(): blocks = BlockArray(2) count = pixy.ccc_get_blocks(2, blocks) if count > 1: result = ('[BLOCK: SIG=%d X=%2d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[1].m_signature, blocks[1].m_x, blocks[1].m_y, blocks[1].m_width, blocks[1].m_height)) output = [] output.append(blocks[1].m_x) output.append(blocks[1].m_y) print(result) print(output) return output else: print("Nothing found")
def detect_color(frame): #print ('colors') global BoxFoundTimeStamp global LeftLane global SafeToShift global TimeToSafety global ShiftLaneDelay if frame == FrameThreashold: pixy.change_prog ("color_connected_components"); count = pixy.ccc_get_blocks (100, blocks) ''' if count > 0: #print('frame %3d:' % (frame)) #frame = frame + 1 for index in range (0, count): print ('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height))''' FoundBox = False if count > 0: for i in range(count): if(blocks[i].m_signature == GreenBox): FoundBox = True if(SafeToShift): backward() time.sleep(0.5) BoxFoundTimeStamp = time.time() if(LeftLane): shift_right(180 - LeftShiftAngle) time.sleep(ShiftLaneDelay) LeftLane = False SafeToShift = False forward(100) time.sleep(0.5) shift_left(LeftShiftAngle) else: shift_left(LeftShiftAngle) time.sleep(ShiftLaneDelay) LeftLane = True SafeToShift = False forward(100) time.sleep(0.5) shift_right(180 - LeftShiftAngle) if( (not FoundBox) and ( (time.time() - BoxFoundTimeStamp) > TimeToSafety) ): SafeToShift = True
def getSig(): global pixyCam #jsonify({'some':'data'}) if pixyCam == -1: return "Can't connect to PixyCamera" count = pixy.ccc_get_blocks (100, blocks) ret = {"C":count} for i in range(count): ret ["S"+str(i)] = blocks[i].m_signature ret ["X"+str(i)] = blocks[i].m_x ret ["Y"+str(i)] = blocks[i].m_y ret ["W"+str(i)] = blocks[i].m_width ret ["H"+str(i)] = blocks[i].m_height return ret
def get_pixy_data(self): pixy.set_lamp(2, 0) blocks = BlockArray(40) last_counts = [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ] iter = 0 #sample = 0; while (1): save = True piece_lst = [] # red = 0; # purple = 0; # blue = 0; # green = 0; # pink = 0; color = '' # start = time.time(); count = pixy.ccc_get_blocks(40, blocks) if count > 0: for index in range(count): if blocks[index].m_signature == 1: # red += 1; color = 'red' elif blocks[index].m_signature == 2: # blue += 1; color = 'blue' elif blocks[index].m_signature == 3: # purple += 1; color = 'purple' elif blocks[index].m_signature == 4: # green += 1; color = 'green' elif blocks[index].m_signature == 5: # pink += 1; color = 'pink' #if not (blocks[index].m_x in range(40, 252)) or not (blocks[index].m_y in range(0, 207)): piece_lst.append( 'SIG=%s X=%d Y=%d' % (color, blocks[index].m_x, blocks[index].m_y)) # print('[SIG = %s X = %d Y = %d AGE = %3d]'%(color, blocks[index].m_x, blocks[index].m_y, blocks[index].m_age)); last_counts[iter] = count iter = (iter + 1) % 25 if (all(x == last_counts[0] for x in last_counts)): return piece_lst
def detectObject( self ): # Method for detection of the object inside a pallet. The method returns the first detected object blocks = BlockArray(1) objectFound = False pixy.set_lamp(1, 0) # Turning the pixi light on while objectFound == False: count = pixy.ccc_get_blocks(1, blocks) if count > 0: objectFound = True print('Object Detected: Signature %d' % blocks[0].m_signature) time.sleep(2) pixy.set_lamp(0, 0) # Turning the pixi light off return blocks[0].m_signature
def getGoodSig(): global pixyCam if pixyCam == -1: return "Can't connect to PixyCamera" count = pixy.ccc_get_blocks (100, blocks) ret = {} for i in range(count): blk = {} blk ["S"] = blocks[i].m_signature blk ["X"] = blocks[i].m_x blk ["Y"] = blocks[i].m_y blk ["W"] = blocks[i].m_width blk ["H"] = blocks[i].m_height ret[i] = blk return ret
def get_pixy_data(self): _fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint)] pixy.set_lamp(2, 0) blocks = BlockArray(100) num_iter = 25 last_counts = [] for i in range(num_iter): last_counts.append(0) iter = 0 while (1): save = True piece_lst = [] color = '' count = pixy.ccc_get_blocks(100, blocks) if count > 0: # print('count' + str(count)) for index in range(count): if blocks[index].m_signature == 1: color = 'red' elif blocks[index].m_signature == 2: color = 'blue' elif (blocks[index].m_signature == 3) or (blocks[index].m_signature == 5): color = 'pink' elif blocks[index].m_signature == 4: color = 'green' # if not (blocks[index].m_x in range(40, 252)) or not (blocks[index].m_y in range(0, 207)): # save = False; # else: piece_lst.append( 'SIG=%s X=%d Y=%d' % (color, blocks[index].m_x, blocks[index].m_y)) # print('[SIG = %s X = %d Y = %d]'%(color, blocks[index].m_x, blocks[index].m_y)); last_counts[iter] = count iter = (iter + 1) % num_iter if save and (all(x == last_counts[0] for x in last_counts)): return piece_lst
def get_base_position(): objects_seen = pixy.ccc_get_blocks(100, blocks) #TODO: add procedure for if there is no base detected if objects_seen > 0: lowest_distance_to_center = 1000 target_block = None # choose the most centered block in the frame for i in range(0, objects_seen): if blocks[i].m_signature == 1: # print('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[i].m_signature, blocks[i].m_x, blocks[i].m_y, blocks[i].m_width, blocks[i].m_height)) distance_to_center = math.sqrt((blocks[i].m_x - x_center)**2 + (blocks[i].m_y - y_center)**2) if distance_to_center < lowest_distance_to_center: lowest_distance_to_center = distance_to_center target_block = blocks[i] if target_block: position_vector = (target_block.m_x - base_center_target[0], base_center_target[1] - target_block.m_y) return position_vector
def ColorSensor(): pixy.init() pixy.change_prog("color_connected_components") class Blocks(Structure): _fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint), ("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint), ("m_index", c_uint), ("m_age", c_uint)] blocks = BlockArray(100) frame = 0 i = 0 check = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0] ret = check while i < 20: count = pixy.ccc_get_blocks(100, blocks) if count > 0: #print 'frame %3d:' % (frame) frame = frame + 1 for index in range(0, count): check[blocks[index].m_signature - 1] += 1 #print blocks[index].m_signature #print '[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height) for j in range(0, count): if (check[j] - 1 == 1 or check[j] == 2 or check[j] == 3): ret[check[j] - 1] += 1 print check #print ret i += 1 time.sleep(.5) return ret #ColorSensor()
def forward(eCount): global distAlongY global yLength global distAlongY blocks = BlockArray(100) a_star.motors(50, 50) leftCount = 0 # use left count to determine when to stop rightCount = 0 leftAcc = 0 rightAcc = 0 countAcc = 0 encoders1 = a_star.read_encoders() time2 = time.perf_counter() while (leftCount < eCount): # left encoder count is less than goal count if ((time.perf_counter() - time2) > 0.05): # look at encoder values every 0.05 second encoders2 = a_star.read_encoders() righten = encoders2[1] - encoders1[1] leften = encoders2[0] - encoders1[0] # only update if difference is positive (not wraparound case) if (righten > 0): rightError = 35 - righten rightCount += righten if (leften > 0): leftError = 35 - leften leftCount += leften # Update distAlongY distAlongY = int(leftCount / countToCM) if not goingDown: distAlongY = int(yLength) - int(distAlongY) # only apply integral control on fifth iteration if (countAcc > 5): leftAcc += leftError rightAcc += rightError # get motor parameters from PI method left = piControl(leftError, leftAcc, 0) right = piControl(rightError, rightAcc, 1) # angle correction and motor control if (leftCount - rightCount > 10): a_star.motors(left, right + 40) print("adjusting right") if (rightCount - leftCount > 10): a_star.motors(left + 40, right) print("adjusting left") else: a_star.motors(left, right) # look at the camera input count = pixy.ccc_get_blocks(100, blocks) if (count > 0): sig = int(blocks[0].m_signature) if (sig == 2): print(sig) # make sure block is large enough to be relevant and signature is red (1) else: if (blocks[0].m_width > 50 and blocks[0].m_height > 50): a_star.motors(0, 0) # stops if a block is detected encInitL, encInitR = a_star.read_encoders() print("detected an obstacle") print(blocks[0].m_signature) avoidObstacle() encFinL, encFinR = a_star.read_encoders() # adjust right and left counts to account for extra distance leftCount -= (encFinL - encInitL) rightCount -= (encFinR - encInitR) leftCount += int(35 * countToCM) rightCount += int(35 * countToCM) # reset time 2 /encoders time2 = time.perf_counter() encoders1 = encoders2 countAcc += 1 stop(1)
pixy.init() pixy.change_prog("color_connected_components") class Blocks(Structure): _fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint), ("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint), ("m_index", c_uint), ("m_age", c_uint)] blocks = BlockArray(100) frame = 0 pixyTable = NetworkTables.getTable('Pixy') while 1: count = pixy.ccc_get_blocks(100, blocks) if count > 0: print('frame %3d:' % (frame)) frame = frame + 1 #print('[BLOCK: SIG=%d X=%3d Y=%3d WIDTH=%3d HEIGHT=%3d]' % (blocks[index].m_signature, blocks[index].m_x, blocks[index].m_y, blocks[index].m_width, blocks[index].m_height)) pixyTable.putNumber('sig', blocks[0].m_signature) pixyTable.putNumber('x', blocks[0].m_x) pixyTable.putNumber('y', blocks[0].m_y) pixyTable.putNumber('width', blocks[0].m_width) pixyTable.putNumber('height', blocks[0].m_height) pixyTable.putNumber('angle', blocks[0].m_angle) pixyTable.putNumber('age', blocks[0].m_age) pixyTable.putBoolean('visible', True) else: pixyTable.putBoolean('visible', False)
print("Test PixyCam") pixy.init() pixy.change_prog("color_connected_components") class Blocks(Structure): _fields_ = [("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint), ("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint), ("m_index", c_uint), ("m_age", c_uint)] blocks = BlockArray(100) frame = 0 pixy.set_lamp(0, 0) a_star.motors(70, 70) # move straight forward, need to use pi control!!! print("begin obstacle search") while 1: count = pixy.ccc_get_blocks(100, blocks) # 100 blocks probably overkill... if (count > 0): # make sure block is large enough to be relevant if (blocks[0].m_width > 50 and blocks[0].m_height > 50): a_star.motors(0, 0) # stops if a block is detected pixy.set_lamp(1, 1) # turns on lamp if a block is detected time.sleep(2) pixy.set_lamp(0, 0) print("obstacle detected, motor stopped")
def forwardForDetection(): blocks = BlockArray(100) #start pi control a_star.motors(70, 70) leftCount = 0 # use leftCount as goal encoder count rightCount = 0 leftAcc = 0 rightAcc = 0 countAcc = 0 startTime = time.perf_counter() encoders1 = a_star.read_encoders() time2 = time.perf_counter() while 1: if ((time.perf_counter() - time2) > 0.05): # look at encoder values every 0.05 second encoders2 = a_star.read_encoders() righten = encoders2[1] - encoders1[1] leften = encoders2[0] - encoders1[0] if (righten > 0): rightError = 35 - righten rightCount += righten if (leften > 0): leftError = 35 - leften leftCount += leften if (countAcc > 5): leftAcc += leftError rightAcc += rightError # calculate new motor parameters and apply to motor left = piControl(leftError, leftAcc, 0) right = piControl(rightError, rightAcc, 1) # angle correction and motor control if (leftCount - rightCount > 10): a_star.motors(left, right + 40) if (rightCount - leftCount > 10): a_star.motors(left + 40, right) else: a_star.motors(left, right) # reset time 2 /encoders time2 = time.perf_counter() encoders1 = encoders2 countAcc += 1 # look at the camera input count = pixy.ccc_get_blocks(100, blocks) if (count > 0): sig = int(blocks[0].m_signature) # make sure block is large enough to be relevant and signature is green if (sig == 1): print(sig) else: if (blocks[0].m_width > 50 and blocks[0].m_height > 50): a_star.motors(0, 0) # stops if a block is detected print("detected a corner") print(blocks[0].m_signature) break stop(2) return leftCount
print("Pixy2 Python SWIG Example -- Pan/Tilt Tracking Demo") # Initialize pan/tilt controllers # Pan_PID_Controller = PID_Controller(PAN_GAIN, 0, PAN_GAIN, True) Tilt_PID_Controller = PID_Controller(TILT_GAIN, 0, TILT_GAIN, True) pixy.init() pixy.change_prog("color_connected_components") Reset() Blocks = BlockArray(1) Frame = 0 while 1: Count = pixy.ccc_get_blocks(1, Blocks) if Count > 0: Frame = Frame + 1 # 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_Offset = (pixy.get_frame_width() / 2) - Blocks[Index].m_x Tilt_Offset = Blocks[Index].m_y - (
def coordinates(numCords, mode, lamp, lTime): #numCords is size of coordinate array, mode =1 for laser, =2 for pump pixy.init () pixy.change_prog ("color_connected_components"); class Blocks (Structure): _fields_ = [ ("m_signature", c_uint), ("m_x", c_uint), ("m_y", c_uint), ("m_width", c_uint), ("m_height", c_uint), ("m_angle", c_uint), ("m_index", c_uint), ("m_age", c_uint) ] if (mode == 1): #LASER MODE Xmin = 80 Xmax = 245 Ymin = 0 Ymax = 143 elif (mode == 2): #PUMP MODE Xmin = 65 Xmax = 245 Ymin = 0 Ymax = 143 blocks = BlockArray(100) frame = 0 sz=numCords #size of coordinate array if (lamp==1): set_lamp(1,0) count = pixy.ccc_get_blocks (100, blocks) time.sleep(lTime) set_lamp(0,0) elif(lamp==2): set_lamp(1,1) count = pixy.ccc_get_blocks (100, blocks) time.sleep(lTime) set_lamp(0,0) elif(lamp==0): count = pixy.ccc_get_blocks (100, blocks) n = 0 temp = numpy.zeros((sz,2)) for k in range (1, count): X = blocks[k].m_x #retreive x and y coordinates of block centerpoint Y = blocks[k].m_y if X>Xmin and X<Xmax and Y>Ymin and Y<Ymax: #check for ROI temp[n] = (Y,X-Xmin) #arrange cords in (Y,X) for sorting by Y n+=1 order = numpy.argsort(temp,0) #create sort order array index = numpy.zeros((sz,2)) cords = numpy.zeros((sz+1,2)) #array to hold sorted coordinates cords[0,0] = n #first item in coordinate array is number of coordinates #arranges coordinates acording to values in order[] for k in range (0,sz): cords[sz-k,:] = temp[order[k,0],:] cords[1:, [0,1]] = cords[1:,[1,0]] #switch cords from (Y,X) to (X,Y) cords = cords.astype(numpy.int) #change cords[] to integer array #print('count', count) print('number', n) #print('\ntemp\n', temp) #print('order\n', order) print('cords\n',cords) return cords
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()
def detect_color(frame): #print ('colors') if frame == 20: pixy.change_prog("color_connected_components") count = pixy.ccc_get_blocks(5, blocks)
def get_all_bot_positions(self): pixy.change_prog("color_connected_components") count = pixy.ccc_get_blocks(10, self.bots) return (self.bots, count)