class Motors(): tickSpeed = 0.1 # seconds forwardSpeed = 0.0 leftSpeed = 0.0 rightSpeed = 0.0 reverseSpeed = 0.0 steeringMin = -0.90 # Smallest servo position to use steeringMax = +0.90 frontPrev = 0 left = 1 right = 1 prevRight = -1 sideRatio = 1.0 prevRatio = 1.0 going = 'straight' minRight = 80 steeringDefault = 0.08 distanceMoved = 0 path = [] intervalToDegreeConstant = 0.28 # seconds def __init__(self, thunderBorgInstance, ultraBorgInstance, tickSpeed): self.tb = thunderBorgInstance self.ub = ultraBorgInstance self.tickSpeed = tickSpeed self.safeDistance = 500 # mm self.steeringPosition = 0.0 self.logger = Telemetry(self.__class__.__name__, "log").get() self.ub.SetServoPosition4(self.steeringPosition) if not self.tb.foundChip: boards = ThunderBorg.ScanForThunderBorg() if len(boards) == 0: self.logger.info( 'No ThunderBorg found, check you are attached :)') else: self.logger.info( "No ThunderBorg at address %02X, but we did find boards:" % (self.tb.i2cAddress)) for board in boards: print(' %02X (%d)' % (board, board)) self.logger.info( 'If you need to change the I2C address change the setup line so it is correct, e.g.' ) self.logger.info('self.tb.i2cAddress = 0x%02X' % (boards[0])) sys.exit() # Ensure the communications failsafe has been enabled! failsafe = False for i in range(5): self.tb.SetCommsFailsafe(True) failsafe = self.tb.GetCommsFailsafe() if failsafe: break if not failsafe: self.logger.info('Board %02X failed to report in failsafe mode!' % (self.tb.i2cAddress)) sys.exit() self.driveRight = 1.0 self.driveLeft = 1.0 # Power settings # Total battery voltage to the ThunderBorg self.voltageIn = 3.7 * 4 # Maximum motor voltage, we limit it to 95% to allow the RPi to get uninterrupted power self.voltageOut = self.voltageIn * 0.95 # Setup the power limits if self.voltageOut > self.voltageIn: self.maxPower = 0.5 else: self.maxPower = self.voltageOut / float(self.voltageIn) self.speed = self.maxPower self.tb.MotorsOff() self.center() def center(self): self.ub.SetServoPosition4(0) def move(self, left, right, front, back): distanceMoved = -1 if (front != 'No reading'): if (self.frontPrev != 0): distanceMoved = (self.frontPrev - front) if (distanceMoved > 0 and self.speed > 0): self.forwardSpeed = (self.frontPrev - front) / self.tickSpeed # mm/seconds if (self.forwardSpeed > front and front < 20): self.speed = self.speed / 2 else: if (front < distanceMoved * 1.5 or front < 200): self.speed = 0 self.frontPrev = front else: self.logger.info('Distance moved was not read') self.move(self.driveLeft, self.driveRight, self.speed) def move(self, dLeft, dRight, speed, record=False): pos = Position() if record: pos.left = dLeft pos.right = dRight pos.speed = speed pos.timestamp = time.time() self.path.append(pos) self.tb.SetMotor1(dLeft * speed) self.tb.SetMotor2(dRight * speed) def stop(self): self.move(0, 0, 0) def rotate(self, degrees, speed=1): t = True delay = ((degrees * self.intervalToDegreeConstant) / 45) / speed print('Rotate Degrees: ' + str(degrees) + ' Calculated duration: ' + str(abs(delay))) last_time = time.time() while t: # Go through each entry in the sequence in order # for step in sequence: if degrees < 0: self.tb.SetMotor1(1 * speed) self.tb.SetMotor2(-1 * speed) elif degrees > 0: self.tb.SetMotor1(-1 * speed) self.tb.SetMotor2(1 * speed) now = time.time() if (now - last_time) > abs(delay): t = False self.tb.SetMotor1(0) self.tb.SetMotor2(0) self.tb.MotorsOff() # Turn both motors off def clear_reverse_history(self): del self.path[:] def reverse(self, percent=100): print('Reversing! Steps: ' + str(len(self.path))) startIndex = len(self.path) - 1 while startIndex > 0: t = True delay = (self.path[startIndex].timestamp - self.path[startIndex - 1].timestamp) last_time = time.time() self.tb.SetMotor1(self.path[startIndex].right * (-1) * self.path[startIndex].speed) self.tb.SetMotor2(self.path[startIndex].left * (-1) * self.path[startIndex].speed) while t: now = time.time() if (now - last_time) > abs(delay): t = False self.tb.SetMotor1(0) self.tb.SetMotor2(0) del self.path[startIndex] startIndex = len(self.path) - 1 print('t = ' + str(t) + ' startIndex: ' + str(startIndex) + ' Delay: ' + str(delay) + ' TimeStamp(i): ' + str(self.path[startIndex].timestamp) + ' TimeStamp(i-1): ' + str(self.path[startIndex - 1].timestamp)) def shutdown(self): self.tb.MotorsOff() self.tb.SetCommsFailsafe(False) self.tb.SetLedShowBattery(False) self.tb.SetLeds(0, 0, 0)
class StraightLineVision(): """Vision class to handle PixyCam Data""" COLOUR_WHITE = 1 # White COLOUR_RED = 2 # Red COLOUR_BLUE = 3 # Blue COLOUR_YELLOW = 4 # Yellow COLOUR_GREEN = 5 # Green COLOUR_UNKNOWN = -99 # Default RED_TURN_POSITION = 0 BLUE_TURN_POSITION = 1 YELLOW_TURN_POSITION = 2 GREEN_TURN_POSITION = 3 PIXY_RES_X = 320 PIXY_RES_Y = 200 def __init__(self, steering, motors): super(StraightLineVision, self).__init__() self.__steering = steering self.__motors = motors self.__logger = Telemetry(self.__class__.__name__, "csv").get() self.__logger.info( 'Frame, Block Type, Signature, X, Y, Width, Height, Size, Angle, Distance, Factor, MovingFor, Pan Position, Action') self.__objectHeight = 46 # mm self.__focalLength = 2.4 # mm pixy_init() def initialise(self, visionAttribs, ptc): self.__visionAttributes = visionAttribs self.__tilt_position = self.__visionAttributes.startTiltAngle self.__pan_position = self.__visionAttributes.startPanAngle self.__prev_pan_position = self.__pan_position self.__pan_tilt_controller = ptc self.__pan_tilt_controller.pan_absolute(self.__pan_position) self.__pan_tilt_controller.tilt_absolute( self.__tilt_position) time.sleep(0.01) def track(self, colour_code): found = True blocks = BlockArray(100) pan_dir = 'straight' self.__steering.reset() while found == True: current_block_position = BlockPosition() frame = -1 btype = -1 angle = -1 bdist = -1 frame = frame + 1 count = pixy_get_blocks(100, blocks) strOp = "" if count > 0: for index in range(0, count): current_block_position.colour = blocks[index].signature btype = blocks[index].type current_block_position.x = blocks[index].x current_block_position.y = blocks[index].y current_block_position.width = blocks[index].width current_block_position.height = blocks[index].height current_block_position.size = current_block_position.width * \ current_block_position.height angle = blocks[index].angle bdist = (self.__objectHeight * self.__focalLength) / current_block_position.height if (int(current_block_position.colour) == int(colour_code)): if current_block_position.x < ((self.PIXY_RES_X / 2) * 0.9): pan_dir = 'left' elif current_block_position.x > ((self.PIXY_RES_X / 2) + (self.PIXY_RES_X / 2 * 0.1)): pan_dir = 'right' else: pan_dir = 'straight' if((current_block_position.size > self.__visionAttributes.targetMinSize) and (current_block_position.x > ((self.PIXY_RES_X / 2) - (current_block_position.width))) and (current_block_position.x < ((self.PIXY_RES_X / 2) + (current_block_position.width)))): status = "Found colour " + \ str(current_block_position.colour) centered = True pan_dir = self.__pan(pan_dir) self.__motors.move( 1, 1, self.__visionAttributes.topSpeed, True) else: status = "Found colour but position not correct| pan: " + pan_dir centered = False pan_dir = self.__pan(pan_dir) action = pan_dir time.sleep(0.01) else: status = "Colour Not Matched| pan:" + pan_dir action = "tracking" else: status = "Pixy didn't get anything" + \ " | pan:" + pan_dir found = False strOp = ('%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %s, %d, %f, %s' % ( frame, btype, current_block_position.colour, current_block_position.x, current_block_position.y, current_block_position.width, current_block_position.height, current_block_position.size, angle, bdist, status, colour_code, self.__pan_position, pan_dir)) self.__logger.info(strOp) #current_block_position.centered = centered return current_block_position def __pan(self, pan_dir, steer=True): if pan_dir == 'left': new_pan_position = self.__pan_position + \ (self.__pan_tilt_controller.abs_pan_per_degree * 2) if self.__pan_position > self.__visionAttributes.maxPanAngle: pan_dir = 'right' self.__pan_position = 0 elif pan_dir == 'right': self.__pan_position = self.__pan_position - \ (self.__pan_tilt_controller.abs_pan_per_degree * 2) if self.__pan_position < self.__visionAttributes.minPanAngle: pan_dir = 'left' self.__pan_position = 0 else: self.__pan_position = 0 if (self.__pan_position < self.__visionAttributes.maxPanAngle) and (self.__pan_position > self.__visionAttributes.minPanAngle): # self.__pan_tilt_controller.pan_absolute( # self.__pan_position) if steer == True: self.__steering.steerAbsolute( self.__pan_position * -2.0) return pan_dir def dispose(self): pixy_close()
class Vision(): """Vision class to handle PixyCam Data""" COLOUR_WHITE = 7 # White COLOUR_RED = 2 # Red COLOUR_BLUE = 3 # Blue COLOUR_YELLOW = 4 # Yellow COLOUR_GREEN = 5 # Green COLOUR_UNKNOWN = -99 # Default RED_TURN_POSITION = 0 BLUE_TURN_POSITION = 1 YELLOW_TURN_POSITION = 2 GREEN_TURN_POSITION = 3 PIXY_RES_X = 320 PIXY_RES_Y = 200 ball_positions = [ BlockPosition(COLOUR_RED), BlockPosition(COLOUR_BLUE), BlockPosition(COLOUR_YELLOW), BlockPosition(COLOUR_GREEN) ] __biggest_block_in_frame = dict() def __init__(self, steering, motors): super(Vision, self).__init__() self.__steering = steering self.__motors = motors self.__logger = Telemetry(self.__class__.__name__, "csv").get() self.__logger.info( 'Frame, Block Type, Signature, X, Y, Width, Height, Size, Angle, Distance, Factor, MovingFor, Pan Position, Action' ) self.__objectHeight = 46 # mm self.__focalLength = 2.4 # mm pixy_init() def initialise(self, visionAttribs, ptc): self.__visionAttributes = visionAttribs self.__tilt_position = self.__visionAttributes.startTiltAngle self.__pan_position = self.__visionAttributes.startPanAngle self.__pan_tilt_controller = ptc self.__pan_tilt_controller.pan_absolute(0) self.__pan_tilt_controller.tilt_absolute(0) def scan(self): print("self.__pan_position = " + str(self.__pan_position)) str(self.__pan_tilt_controller.pan_absolute(self.__pan_position)) str(self.__pan_tilt_controller.tilt_absolute(self.__tilt_position)) time.sleep(0.5) not_in_view = True first_colour_found = False colour_code = self.COLOUR_UNKNOWN colours_found = False blocks = BlockArray(100) frame = 0 strOp = "" factor = False while colours_found != True: try: count = pixy_get_blocks(100, blocks) if count > 0: first_colour_found = True frameKey = 'frame' + str(frame) self.__biggest_block_in_frame = BlockPosition( self.COLOUR_UNKNOWN, frameKey) for index in range(0, count): bsign = blocks[index].signature btype = blocks[index].type bx = blocks[index].x by = blocks[index].y bwidt = blocks[index].width bheig = blocks[index].height bsize = bwidt * bheig angle = blocks[index].angle bdist = (self.__objectHeight * self.__focalLength) / bheig factor = 0 if (self.__biggest_block_in_frame.size < bsize): self.__biggest_block_in_frame.size = bsize self.__biggest_block_in_frame.colour = int(bsign) self.__biggest_block_in_frame.x = int(bx) self.__biggest_block_in_frame.y = int(by) self.__biggest_block_in_frame.width = int(bwidt) self.__biggest_block_in_frame.size = int(bsize) self.__biggest_block_in_frame.pan_position = self.__pan_position factor = self.__update_if_better_block_position( self.__biggest_block_in_frame) strOp = ( '%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %s, %d, %f, %s' % (frame, btype, bsign, bx, by, bwidt, bheig, bsize, angle, bdist, str(factor), colour_code, self.__pan_position, 'scanning')) self.__pan_position = self.__pan_position + \ self.__pan_tilt_controller.abs_pan_per_degree self.__pan_tilt_controller.pan_absolute(self.__pan_position) if strOp != "": self.__logger.info(strOp) frame += 1 time.sleep(0.02) colours_found = self.__all_colours_found( ) and self.__pan_position > ( self.__visionAttributes.maxPanAngle) * 0.95 except KeyboardInterrupt: pixy_close() print("Closed Pixy") raise except: tb = traceback.format_exc() e = sys.exc_info()[0] print(tb) if (self.__motors): self.__motors.shutdown() raise def __update_if_better_block_position(self, current_block_position): if ((current_block_position.size > self.__visionAttributes.targetMinSize) and ((current_block_position.x + current_block_position.width) < 220) and ((current_block_position.y > 15)) and ((current_block_position.y < 140))): ball_position = self.__get_ball_position_of_colour( current_block_position.colour) x_distance_from_center = abs(self.ball_positions[ball_position].x - self.PIXY_RES_X / 2) new_x_distance_from_center = abs(current_block_position.x - self.PIXY_RES_X / 2) if (new_x_distance_from_center < x_distance_from_center): self.ball_positions[ball_position] = current_block_position return True else: return False else: return False def __get_ball_position_of_colour(self, colour): index = 0 for ball_position in self.ball_positions: if ball_position.colour == colour: return index else: index = index + 1 return -1 def __all_colours_found(self): for ball_position in self.ball_positions: if ball_position.pan_position == 360: return False return True def goto_ball_position(self, ball_position, previous_position=0): reached = False self.__pan_position = 0 self.__pan_tilt_controller.pan(self.__pan_position) turn_by = (ball_position.pan_position - previous_position) * -135 self.__motors.rotate(turn_by, 0.5) # self.__pan_tilt_controller.tilt(-0.05) self.__pan_position = 0 self.__pan_tilt_controller.pan(self.__pan_position) while reached == False: tracked = False current_block_position = self.track(ball_position.colour) tracked = current_block_position != None if tracked: current_block_position.factor = current_block_position.pan_position # if(current_block_position.y < current_block_position.height / 2): # while (current_block_position.y < current_block_position.height / 2): # self.__pan_tilt_controller.tilt( # self.__pan_tilt_controller.current_tilt - self.__pan_tilt_controller.abs_tilt_per_degree) self.drive_to(current_block_position, current_block_position.colour) print("Size: " + str(current_block_position.size)) reached = current_block_position.size >= self.__visionAttributes.targetMaxSize else: self.__motors.stop() self.__steering.steerAbsolute(0) self.__motors.reverse() self.__motors.clear_reverse_history() def track(self, colour_code): centered = False blocks = BlockArray(100) pan_dir = 'left' current_block_position = BlockPosition() frame = -1 while centered == False: frame = frame + 1 count = pixy_get_blocks(100, blocks) if count > 0: for index in range(0, count): current_block_position.colour = blocks[index].signature btype = blocks[index].type current_block_position.x = blocks[index].x current_block_position.y = blocks[index].y current_block_position.width = blocks[index].width current_block_position.height = blocks[index].height current_block_position.size = current_block_position.width * \ current_block_position.height angle = blocks[index].angle bdist = (self.__objectHeight * self.__focalLength ) / current_block_position.height if (int(current_block_position.colour) == int(colour_code) ): if current_block_position.x > 160: pan_dir = 'right' else: pan_dir = 'left' if ((current_block_position.size > self.__visionAttributes.targetMinSize) and (current_block_position.x < ((self.PIXY_RES_X / 2) - (current_block_position.width / 2))) and (current_block_position.x > (self.PIXY_RES_X / 2) - (current_block_position.width)) # and ((current_block_position.y > 15)) # and ((current_block_position.y < 140)) ): self.__pan_tilt_controller.pan_absolute( self.__pan_position) self.__steering.steerAbsolute(self.__pan_position * 2) current_block_position.pan_position = self.__pan_position strOp = ( '%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %s, %d, %f, %s' % (frame, btype, current_block_position.colour, current_block_position.x, current_block_position.y, current_block_position.width, current_block_position.height, current_block_position.size, angle, bdist, "Found Colour & Size", colour_code, self.__pan_position, 'tracking')) self.__logger.info(strOp) centered = True else: strOp = ( '%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %s, %d, %f, %s' % (frame, btype, current_block_position.colour, current_block_position.x, current_block_position.y, current_block_position.width, current_block_position.height, current_block_position.size, angle, bdist, "Size Not Matched", colour_code, self.__pan_position, 'tracking')) self.__logger.info(strOp) else: strOp = ( '%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %s, %d, %f, %s' % (frame, btype, current_block_position.colour, current_block_position.x, current_block_position.y, current_block_position.width, current_block_position.height, current_block_position.size, angle, bdist, "Colour Not Matched", colour_code, self.__pan_position, 'tracking')) self.__logger.info(strOp) if centered == False: print("Centered == False && count > 0") pan_dir = self.__pan(pan_dir) time.sleep(0.01) else: print("Centered == False && count == 0") pan_dir = self.__pan(pan_dir) time.sleep(0.01) return current_block_position def __pan(self, pan_dir): if pan_dir == 'left': self.__pan_position = self.__pan_position + \ self.__pan_tilt_controller.abs_pan_per_degree if self.__pan_position > self.__visionAttributes.maxPanAngle: pan_dir = 'right' elif pan_dir == 'right': self.__pan_position = self.__pan_position - \ self.__pan_tilt_controller.abs_pan_per_degree if self.__pan_position < self.__visionAttributes.minPanAngle: pan_dir = 'left' if (self.__pan_position < self.__visionAttributes.maxPanAngle) and ( self.__pan_position > self.__visionAttributes.minPanAngle): self.__pan_tilt_controller.pan_absolute(self.__pan_position) self.__steering.steerAbsolute(self.__pan_position * -2) print('__pan_position applied to ' + str(self.__pan_position)) return pan_dir def drive_to(self, current_block_position, colour_code): print("Driving to: " + str(current_block_position.pan_position) + " : " + str(current_block_position.colour)) self.__steering.steerAbsolute(current_block_position.pan_position * -2) self.__motors.move(1, 1, 0.3, True) time.sleep(0.02) # self.track(current_block_position.colour) # def approach(self, current_block_position, colour_code): # reached = False # blocks = BlockArray(100) # frame = 0 # while reached == False: # try: # if('colour' in self.__biggest_block_in_frame): # if (self.__biggest_block_in_frame['colour'] == colour_code): # # print('colour match') # # if(self.__biggest_block_in_frame['bsize'] > self.__visionAttributes.targetMinSize # and self.__biggest_block_in_frame['bsize'] < self.__visionAttributes.targetMaxSize): # self.__steering.steerAbsolute(0) # else: # self.__motors.move(1, 1, 0) # # if(self.__biggest_block_in_frame['bsize'] >= self.__visionAttributes.targetMaxSize): # self.__motors.move(1, 1, 0) # # reached = True # else: # # reached = False # else: # self.__motors.move(1, 1, 0) # else: # self.__motors.move(1, 1, 0) # # count = pixy_get_blocks(100, blocks) # if count > 0: # # # Blocks found # # frameKey = 'frame' + str(frame) # # # self.__biggest_block_in_frame = {frameKey: 0} # for index in range(0, count): # bsign = blocks[index].signature # btype = blocks[index].type # bx = blocks[index].x # by = blocks[index].y # bwidt = blocks[index].width # bheig = blocks[index].height # bsize = bwidt * bheig # angle = blocks[index].angle # bdist = (self.__objectHeight * # self.__focalLength) / bheig # factor = 0 # if (int(bsign) == int(colour_code)): # if((frameKey in self.__biggest_block_in_frame and self.__biggest_block_in_frame[frameKey] < bsize) # or ((frameKey in self.__biggest_block_in_frame) == False)): # self.__biggest_block_in_frame[frameKey] = bsize # self.__biggest_block_in_frame['colour'] = int( # bsign) # self.__biggest_block_in_frame['bx'] = int( # bx) # self.__biggest_block_in_frame['bwidt'] = int( # bwidt) # self.__biggest_block_in_frame['bsize'] = int( # bsize) # # if self.__biggest_block_in_frame['bx'] > 160: # panDirection = 'right' # else: # panDirection = 'left' # if panDirection == 'left': # self.__pan_position += self.__pan_tilt_controller.abs_pan_per_degree # if self.__pan_position > self.__visionAttributes.maxPanAngle: # panDirection = 'right' # elif panDirection == 'right': # self.__pan_position -= self.__pan_tilt_controller.abs_pan_per_degree # if self.__pan_position < self.__visionAttributes.minPanAngle: # panDirection = 'left' # self.__pan_tilt_controller.pan_absolute( # self.__pan_position) # # self.__biggest_block_in_frame['factor'] = self.__pan_position # if('factor' in self.__biggest_block_in_frame): # factor = self.__biggest_block_in_frame['factor'] # if(factor > 0): # # go Left # if (factor > 0.33): # # spin # print( # 'spinning left @ factor:' + str(factor)) # self.__motors.rotate( # factor * 135 * -1) # self.__pan_position = 0 # self.__pan_tilt_controller.pan_absolute( # self.__pan_position) # else: # # steer # factor = factor / -1 # self.__steering.steerAbsolute( # factor) # self.__motors.move(1, 1, 1) # # elif(self.__biggest_block_in_frame['factor'] < 0): # # goright # if (factor < -0.33): # # spin # print( # 'spinning Right @ factor:' + str(factor)) # self.__motors.rotate( # factor * 135 * -1) # self.__pan_position = 0 # self.__pan_tilt_controller.pan_absolute( # self.__pan_position) # else: # # steer # factor = factor / -1 # self.__steering.steerAbsolute( # factor) # self.__motors.move(1, 1, 1) # else: # print('going straight') # factor = 0 # self.__steering.steerAbsolute(factor) # self.__motors.move(1, 1, 1) # else: # print('going nowhere') # # self.__motors.move(1, 1, 1) # else: # self.__biggest_block_in_frame = {} # strOp = ('%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %f, %d, %f, %s' % ( # frame, btype, bsign, bx, by, bwidt, bheig, bsize, angle, bdist, factor, colour_code, self.__pan_position, 'approaching')) # self.__logger.info(strOp) # frame += 1 # time.sleep(0.02) # # except KeyboardInterrupt: # self.dispose() # raise # except: # self.dispose() # # tb = traceback.format_exc() # e = sys.exc_info()[0] # print(tb) # if(self.__motors): # self.__motors.shutdown() # raise # # def search(self, colour_code): # found = False # blocks = BlockArray(100) # frame = 0 # panDirection = 'right' # # while found == False: # try: # count = pixy_get_blocks(100, blocks) # if count > 0: # # print('count > 0') # # Blocks found # # frameKey = 'frame' + str(frame) # # print(count) # # self.__biggest_block_in_frame = {frameKey: 0} # for index in range(0, count): # bsign = blocks[index].signature # btype = blocks[index].type # bx = blocks[index].x # by = blocks[index].y # bwidt = blocks[index].width # bheig = blocks[index].height # bsize = bwidt * bheig # angle = blocks[index].angle # bdist = (self.__objectHeight * # self.__focalLength) / bheig # factor = 0 # if (int(bsign) == int(colour_code)): # if(self.__biggest_block_in_frame[frameKey] < bsize): # self.__biggest_block_in_frame[frameKey] = bsize # self.__biggest_block_in_frame['colour'] = int( # bsign) # self.__biggest_block_in_frame['bx'] = int( # bx) # self.__biggest_block_in_frame['bwidt'] = int( # bwidt) # self.__biggest_block_in_frame['bsize'] = int( # bsize) # # if(self.__biggest_block_in_frame[frameKey] > self.__visionAttributes.targetMinSize # and ((self.__biggest_block_in_frame['bx'] + self.__biggest_block_in_frame['bwidt']) < 220)): # found = True # return self.__biggest_block_in_frame[frameKey] # else: # if panDirection == 'left': # self.__pan_position += self.__pan_tilt_controller.abs_pan_per_degree # if self.__pan_position > self.__visionAttributes.maxPanAngle: # panDirection = 'right' # elif panDirection == 'right': # self.__pan_position -= self.__pan_tilt_controller.abs_pan_per_degree # if self.__pan_position < self.__visionAttributes.minPanAngle: # panDirection = 'left' # self.__pan_tilt_controller.pan_absolute( # self.__pan_position) # found = False # # strOp = ('%d, %d, %d, %f, %f, %f, %f, %f, %f, %f, %f, %d, %f, %s' % ( # frame, btype, bsign, bx, by, bwidt, bheig, bsize, angle, bdist, factor, colour_code, self.__pan_position, 'searching')) # # else: # strOp = "" # # if strOp != "": # self.__logger.info(strOp) # frame += 1 # time.sleep(0.01) # except KeyboardInterrupt: # # pixy_close() # # raise # except: # tb = traceback.format_exc() # e = sys.exc_info()[0] # print(tb) # if(self.__motors): # self.__motors.shutdown() # raise def dispose(self): pixy_close()
class Main(): ub = UltraBorg3.UltraBorg() tb = ThunderBorg3.ThunderBorg() MODE_STRAIGHT_LINE_SPEED = 0 MODE_OVER_THE_RAINBOW = 1 MODE_MAZE_SOLVE = 2 MODE_DERANGED_GOLF = 3 MODE_DUCK_SHOOT = 4 MODE_OBSTACLE_COURSE = 5 MODE_PI_NOON = 6 MODE_TEST = 99 mode = MODE_OBSTACLE_COURSE def __init__(self): self.ub.Init() self.tb.Init() self.t1 = datetime.now() self.tickSpeed = 0.05 self.us = Ultrasonics(self.ub) self.motors = Motors(self.tb, self.ub, self.tickSpeed) self.steering = Steering(self.tb, self.ub, self.tickSpeed) self.teleLogger = Telemetry("telemetry", "csv").get() self.ptc = PanTiltController(self.ub, 270, 135) self.joystick_controller = JoystickController(self.tb, self.ub) battMin, battMax = self.tb.GetBatteryMonitoringLimits() battCurrent = self.tb.GetBatteryReading() print('Battery monitoring settings:') print(' Minimum (red) %02.2f V' % (battMin)) print(' Half-way (yellow) %02.2f V' % ((battMin + battMax) / 2)) print(' Maximum (green) %02.2f V' % (battMax)) print() print(' Current voltage %02.2f V' % (battCurrent)) print(' Mode %s' % (self.mode)) print() def log(self): if (self.motors.speed != 0): self.teleLogger.info( '%(left)f, %(front)f, %(right)f, %(back)f, %(distanceMoved)f, %(forwardSpeed)f, %(direction)s, %(degree)f, %(ratio)f', { "left": self.us.left, "front": self.us.front, "right": self.us.right, "back": self.us.back, "distanceMoved": self.motors.distanceMoved, "forwardSpeed": self.motors.forwardSpeed, "direction": self.steering.going, "degree": self.steering.steeringPosition, "ratio": self.steering.sideRatio }) def run(self): try: if (self.mode == self.MODE_STRAIGHT_LINE_SPEED): print("Straight line speed") self.modeStraightLineSpeed() elif (self.mode == self.MODE_OVER_THE_RAINBOW): print("Rainbow") self.modeOverTheRainbow() elif (self.mode == self.MODE_DERANGED_GOLF or self.mode == self.MODE_PI_NOON or self.mode == self.MODE_OBSTACLE_COURSE): print("Joystick control: " + str(self.mode)) self.joystick_controller.run() else: # Wait for Buttons print('Pan -62 = ' + str(self.ptc.pan(-62))) time.sleep(1) print('Pan 0 = ' + str(self.ptc.pan(0))) time.sleep(1) # # print('Pan 62 = ' + str(self.ptc.pan(62))) # time.sleep(1) # # print('Pan absolute 1.0 = ' + str(self.ptc.pan_absolute(1.0))) # time.sleep(1) # # print('Pan absolute -1.0 = ' + str(self.ptc.pan_absolute(-1.0))) # time.sleep(1) # # print('Pan absolute 0.0 = ' + str(self.ptc.pan_absolute(0.0))) # time.sleep(1) # # print('Tilt -30 = ' + str(self.ptc.tilt(-30))) # time.sleep(1) # print('Tilt 0 = ' + str(self.ptc.tilt(0))) # time.sleep(1) # # print('Tilt 30 = ' + str(self.ptc.tilt(30))) # time.sleep(1) # # print('Tilt absolute 0.6 = ' + str(self.ptc.tilt_absolute(0.6))) # time.sleep(1) # # print('Tilt absolute -0.6 = ' + # str(self.ptc.tilt_absolute(-0.6))) # time.sleep(1) # # print('Tilt absolute 0.0 = ' + str(self.ptc.tilt_absolute(0.0))) # time.sleep(1) except KeyboardInterrupt: # User has pressed CTRL+C self.ub.SetServoPosition2(0) t2 = datetime.now() delta = t2 - self.t1 print("Run complete in : " + str(delta.total_seconds())) print('Done') if (self.motors): self.motors.shutdown() except Exception as e: tb = traceback.format_exc() e = sys.exc_info()[0] print(tb) if (self.motors): self.motors.shutdown() def modeStraightLineSpeed(self): self.straight_line_speed = StraightLineVision(self.steering, self.motors) self.teleLogger.info( 'left, front, right, back, distanceMoved, forwardSpeed, direction, steering position, ratio' ) self.steering.reset() slVa = VisionAttributes() slVa.startTiltAngle = 0.6 slVa.startPanAngle = 0 slVa.targetMinSize = 1000 slVa.targetMaxSize = 18000 slVa.minPanAngle = -0.5 slVa.maxPanAngle = 0.5 slVa.colour = Vision.COLOUR_WHITE slVa.targetColorPattern = Vision.COLOUR_WHITE slVa.topSpeed = 0.6 slVa.topSpinSpeed = 1.0 self.ptc.tilt(0.5) slsPtc = PanTiltController(self.ub, 270, 135) slsPtc.initPanServo(5000, 1000) self.straight_line_speed.initialise(slVa, slsPtc) self.motors.stop() prev_block_position = None t1 = datetime.now() self.straight_line_speed.track(self.straight_line_speed.COLOUR_WHITE) t2 = datetime.now() delta = t2 - self.t1 print("Run complete in: " + str(delta.total_seconds())) def modeOverTheRainbow(self): self.vision = Vision(self.steering, self.motors) slVa = VisionAttributes() slVa.startTiltAngle = 0.12 slVa.startPanAngle = -1.00 slVa.targetMinSize = 20 slVa.targetMaxSize = 2200 slVa.minPanAngle = -1.0 slVa.maxPanAngle = 1.0 slVa.targetColorPattern = Vision.COLOUR_WHITE slVa.topSpeed = 1.0 slVa.topSpinSpeed = 1.0 self.motors.move(-1, -1, 0.3) time.sleep(0.8) self.motors.stop() rainbowPtc = PanTiltController(self.ub, 270, 135) rainbowPtc.initPanServo(5000, 1000) self.vision.initialise(slVa, rainbowPtc) time.sleep(0.5) self.vision.scan() print("Scan Complete") index = 0 prev_position = 0 for ball_position in self.vision.ball_positions: ball_position = self.vision.ball_positions[0] print("Size: " + str(ball_position.size) + ', x :' + str(ball_position.x) + ', y :' + str(ball_position.y) + ', pan-position :' + str(ball_position.pan_position) + ', angle : ' + str(ball_position.pan_position * 135) + ', Colour:' + str(ball_position.colour)) if (index > 0): prev_position = self.vision.ball_positions[index - 1].pan_position index = index + 1 self.vision.goto_ball_position(ball_position, prev_position)