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)
