def main():
#####################################
##
## VARIABLE DECLARATION
##
#####################################
clear = lambda: system('clear') ##Used for clearing the terminal screen.
reset = 0 ##Determines when to clear the terminal
##screen.
sensorDistances = [] ##A list of all measured sensor
##distances for each angle, 0-359.
lastFar = lastCW = lastCCW = 0 ##Keep track of the position statuses
##from the last iteration of analyzing
##sensor data.
#####################################
clear()
WHEELS.set_pwm_freq(PWM_FREQ)
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
mixer.init()
playSoundEff(STNDBY_SOUND)
try:
input("PRESS <ENTER> TO BEGIN")
playSoundEff(BEGIN_SOUND)
sleep(SLEEP_TIME)
while (True):
sensorDistances = collectData()
lastFar, lastCW, lastCCW = interpretData(sensorDistances, lastFar,
lastCW, lastCCW)
except:
#clear()
print("TERMINATING")
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
playSoundEff(FIN_SOUND)
SENSOR.stop()
SENSOR.disconnect()
sleep(SLEEP_TIME)
return
def calculateObjMovement(objAngles, objDistances, allDistances):
#####################################
##
## VARIABLE DECLARATION
##
#####################################
maneuverAngle = 0 ##The angle the machine will move at when
##repositioning.
maneuverDistance = 0.0 ##The approximate distance the machine will
##travel when repositioning.
maneuverFound = False ##Set to True once maneuverAngle and
##maneuverDistance have been determined.
moveObjAngle = 0 ##The angle of the object closest to the machine
##after repositioning with maneuverAngle and
##maneuverDIstance.
pathAngles = [] ##Any angle in the path of the machine when
##repositioning; that is, an angle at which an
##object at said angle, if not far enough, could
##be contacted by the machine while maneuvering
##or could be considered too close to the
##machine after maneuvering.
clearPath = False ##Set to True if isPathClear() returns True,
##finding no objects within the desired movement
##path of the machine.
desiredDistance = 0 ##The distance the machine will look to be away
##from the object closest to itself when
##repositioning.
manObjDistance = 0 ##The distance of the nearest object in the
##direction of maneuverAngle.
insertPoint = 0 ##The position in which an item is added to the
##objAngles and objDistances lists.
wallAnglesCount = 0 ##The amount of adjacent angles on either side
##of the current value being added to objAngles.
##These angles will also be added to objAngles
##if they are detecting the same objects as the
##original angle.
adjustedDistance = 0 ##The returned value of getCollinearDistance().
#####################################
maneuverAngle = findMoveAngle(objAngles)
if(maneuverAngle == -1):
playSoundEff(BLOKD_SOUND)
return
pathAngles = getPathAngles(maneuverAngle)
##Check OPT_DISTANCE maneuverability.
maneuverDistance, closeObjAngle = findMoveDistance(objAngles, objDistances,
maneuverAngle, SNS_OPT_DISTANCE)
clearPath = isPathClear(maneuverAngle, pathAngles, allDistances,
MACH_RADIUS, maneuverDistance + SNS_MIN_DISTANCE)
if(clearPath):
maneuverFound = True
desiredDistance = SNS_OPT_DISTANCE
moveObjAngle = findTargetAngle(maneuverAngle, maneuverDistance,
closeObjAngle, allDistances[closeObjAngle])
else:
##Check SUF_DISTANCE maneuverability.
maneuverDistance, closeObjAngle = findMoveDistance(objAngles,
objDistances, maneuverAngle,
SNS_SUF_DISTANCE)
clearPath = isPathClear(maneuverAngle, pathAngles, allDistances,
MACH_RADIUS, maneuverDistance + SNS_MIN_DISTANCE)
if(clearPath):
maneuverFound = True
desiredDistance = SNS_SUF_DISTANCE
moveObjAngle = findTargetAngle(maneuverAngle, maneuverDistance,
closeObjAngle, allDistances[closeObjAngle])
else:
manObjDistance = allDistances[maneuverAngle]
insertPoint = bisect_left(objAngles, maneuverAngle)
objAngles.insert(insertPoint, maneuverAngle)
objDistances.insert(insertPoint, manObjDistance)
wallAnglesCount = ceil(degrees(atan(MACH_RADIUS/manObjDistance)))
for x in range (maneuverAngle - wallAnglesCount, maneuverAngle
+ wallAnglesCount + 1):
if(x < 0):
x += 360
elif(x > 359):
x -= 360
adjustedDistance = getCollinearDistance(x, maneuverAngle,
manObjDistance)
if(MACH_RADIUS < allDistances[x] <= adjustedDistance):
insertPoint = bisect_left(objAngles, x)
if(len(objAngles) == 0 or insertPoint ==
len(objAngles) or x != objAngles[insertPoint]):
objAngles.insert(insertPoint, x)
objDistances.insert(insertPoint, allDistances[x])
if(maneuverFound):
moveFromObject(maneuverAngle, pathAngles, moveObjAngle, desiredDistance,
allDistances)
else:
calculateObjMovement(objAngles, objDistances, allDistances)
return
def moveToOpponent():
#####################################
##
## VARIABLE DECLARATION
##
#####################################
wheelSpeeds = [0] * 3 ##The speed for each wheel in order to
##reposition at the desired angle; this
###value will act as a percentage, values
###greater than 0 mean ccw rotation, less
##than zero, cw wheel rotation.
speedVars = [0] * 2 ##Variables used to determine wheelSpeeds
##values.
wheelPWMs = [0] * 3 ##The PWM output values for each wheel.
watchAngles = [] ##Angular values that are in the path of the
##machine as it's moving that need to be
##checked for objects being too close.
doneMoving = False ##Set to True if the new desired position is
##reached, or an object is detected too
##close to the machine.
index = 0 ##The index of a given list in which a value
##is stored if present.
#####################################
##See documentation for explanation on how the following equations were
##determined.
speedVars[0] = sin(radians(DES_OPP_ANGLE))
speedVars[1] = cos(radians(DES_OPP_ANGLE))
wheelSpeeds[0] = (speedVars[1]*SPEED_CONSTS[3] \
- speedVars[0]*SPEED_CONSTS[2]) \
/ (SPEED_CONSTS[0]*SPEED_CONSTS[3] \
- SPEED_CONSTS[1]*SPEED_CONSTS[2])
wheelSpeeds[1] = (speedVars[0] - wheelSpeeds[0]*SPEED_CONSTS[1]) \
/ SPEED_CONSTS[3]
wheelSpeeds[2] = -wheelSpeeds[1] - wheelSpeeds[0]
for x in range(3):
if (wheelSpeeds[x] == 0):
wheelPWMs[x] = STOP_TICK
elif (wheelSpeeds[x] > 0):
wheelPWMs[x] = calculatePWM(x, wheelSpeeds[x] * MAX_SPEED, False)
else:
wheelPWMs[x] = calculatePWM(x, abs(wheelSpeeds[x] * MAX_SPEED),
True)
watchAngles = getPathAngles(DES_OPP_ANGLE)
try:
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, wheelPWMs[0])
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, wheelPWMs[1])
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, wheelPWMs[2])
for scan in SENSOR.iter_scans():
for (_, angle, distance) in scan:
angle = getCartesianAngle(round(angle))
distance *= 0.0393
index = bisect_left(watchAngles, angle)
if (index < len(watchAngles) and angle == watchAngles[index]):
if (MACH_RADIUS < distance < SNS_MIN_DISTANCE):
doneMoving = True
break
else:
continue
if (FRONT_ANGLE_MIN <= angle <= FRONT_ANGLE_MAX
and MACH_RADIUS < distance <= SNS_OPT_DISTANCE):
doneMoving = True
break
if (doneMoving):
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
break
##Prevents adafruit_rplidar.py runtime error when attempting to collect
##data again
SENSOR.stop()
SENSOR.disconnect()
SENSOR.connect()
except:
print("TERMINATING")
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
playSoundEff(FIN_SOUND)
SENSOR.stop()
SENSOR.disconnect()
sleep(SLEEP_TIME)
return
def rotateMachine(turnCW, stopMin, stopMax):
#####################################
##
## VARIABLE DECLARATION
##
#####################################
wheelPWMs = [0] * 3 ##The PWM output values for each wheel.
doneMoving = False ##Set to True once the machine is again
##facing the opponent, or an object is
##detected too close to the machine.
adjustedDistance = 0 ##The returned value of
##getCollinearDistance().
index = 0 ##The index of a given list in which a value
##is stored if present.
#####################################
for x in range(3):
if (turnCW):
wheelPWMs[x] = calculatePWM(x, TURN_SPEED, False)
else:
wheelPWMs[x] = calculatePWM(x, TURN_SPEED, True)
try:
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, wheelPWMs[0])
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, wheelPWMs[1])
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, wheelPWMs[2])
for scan in SENSOR.iter_scans():
for (_, angle, distance) in scan:
angle = getCartesianAngle(round(angle))
distance *= 0.0393
if (MACH_RADIUS < distance <= SNS_MIN_DISTANCE):
doneMoving = True
break
adjustedDistance = getCollinearDistance(
angle, DES_OPP_ANGLE, SNS_MAX_DISTANCE)
if (stopMin <= angle <= stopMax
and MACH_RADIUS < distance <= adjustedDistance):
doneMoving = True
break
if (doneMoving):
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
break
##Prevents adafruit_rplidar.py runtime error when attempting to collect
##data again
SENSOR.stop()
SENSOR.disconnect()
SENSOR.connect()
except:
print("TERMINATING")
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
playSoundEff(FIN_SOUND)
SENSOR.stop()
SENSOR.disconnect()
sleep(SLEEP_TIME)
return
def moveFromObject(repositionAngle, watchAngles, targetAngle, desiredDistance,
allDistances):
#####################################
##
## VARIABLE DECLARATION
##
#####################################
stopAngles = [targetAngle] ##The angle values that the machine will look
##at to assess if it has moved the correct
##distance away from the object that was
##deemed too close. Multiple values are used
##as a failsafe in case the targetAngle value
##does not return a distance.
currDistances = [] ##The current distance values for the
##stopAngles values before repositioning and
##updated as repositioning occurs.
lookForShorter = False ##Set to True if majority of currDistances
##values are already greater than the
##desiredDistance value.
wheelSpeeds = [0] * 3 ##The speed for each wheel in order to
##reposition at the desired angle; this
##value will act as a percentage, values
##greater than 0 mean ccw wheel rotation,
##less than zero, cw wheel rotation.
speedVars = [0] * 2 ##Variables used to determine wheelSpeeds
##values.
wheelPWMs = [0] * 3 ##The PWM output values for each wheel.
doneMoving = False ##Set to True if the new desired position is
##reached, or an object is detected too
##close to the machine.
adjustedDistance = 0 ##The returned value of
##getCollinearDistance().
index = 0 ##The index of a given list in which a value
##is stored if present.
#####################################
for x in range(1, ANGLE_ERR + 1):
stopAngles.insert(0, targetAngle - x)
if (stopAngles[0] < 0):
stopAngles[0] += 360
stopAngles.append(targetAngle + x)
if (stopAngles[len(stopAngles) - 1] > 359):
stopAngles[len(stopAngles) - 1] -= 360
stopAngles.sort()
currDistances = [allDistances[x] for x in stopAngles]
if (sum(x > desiredDistance for x in currDistances) > ANGLE_ERR):
lookForShorter = True
##See documentation for explanation on how the following equations were
##determined.
speedVars[0] = sin(radians(repositionAngle))
speedVars[1] = cos(radians(repositionAngle))
wheelSpeeds[0] = (speedVars[1]*SPEED_CONSTS[3] \
- speedVars[0]*SPEED_CONSTS[2]) \
/ (SPEED_CONSTS[0]*SPEED_CONSTS[3] \
- SPEED_CONSTS[1]*SPEED_CONSTS[2])
wheelSpeeds[1] = (speedVars[0] - wheelSpeeds[0]*SPEED_CONSTS[1]) \
/ SPEED_CONSTS[3]
wheelSpeeds[2] = -wheelSpeeds[1] - wheelSpeeds[0]
for x in range(3):
if (wheelSpeeds[x] == 0):
wheelPWMs[x] = STOP_TICK
elif (wheelSpeeds[x] > 0):
wheelPWMs[x] = calculatePWM(x, wheelSpeeds[x] * MAX_SPEED, False)
else:
wheelPWMs[x] = calculatePWM(x, abs(wheelSpeeds[x] * MAX_SPEED),
True)
try:
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, wheelPWMs[0])
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, wheelPWMs[1])
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, wheelPWMs[2])
for scan in SENSOR.iter_scans():
for (_, angle, distance) in scan:
angle = getCartesianAngle(round(angle))
distance *= 0.0393
index = bisect_left(watchAngles, angle)
if (index < len(watchAngles) and angle == watchAngles[index]):
if (MACH_RADIUS < distance < SNS_MIN_DISTANCE):
doneMoving = True
break
index = bisect_left(stopAngles, angle)
if (index < len(stopAngles) and angle == stopAngles[index]):
if (distance == 0.0):
distance = SNS_RANGE
currDistances[index] = distance
if (lookForShorter):
if (sum(x <= desiredDistance
for x in currDistances) == len(currDistances)):
doneMoving = True
break
else:
if (sum(x >= desiredDistance
for x in currDistances) == len(currDistances)):
doneMoving = True
break
if (sum(x >= SNS_MAX_DISTANCE
for x in currDistances) == len(currDistances)):
doneMoving = True
break
if (doneMoving):
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
break
##Prevents adafruit_rplidar.py runtime error when attempting to collect
##data again
SENSOR.stop()
SENSOR.disconnect()
SENSOR.connect()
except:
print("TERMINATING")
WHEELS.set_pwm(PWM_PORTS[0], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[1], START_TICK, STOP_TICK)
WHEELS.set_pwm(PWM_PORTS[2], START_TICK, STOP_TICK)
playSoundEff(FIN_SOUND)
SENSOR.stop()
SENSOR.disconnect()
sleep(SLEEP_TIME)
return