def collectData():
#####################################
##
## VARIABLE DECLARATION
##
#####################################
sensorDistances = [SNS_RANGE
] * 360 ##A list of all measured sensor distances
##for each angle, 0-359.
cartAngle = 0 ##The angle measurement returned from
##getCartesianAngle().
#####################################
for i, scan in enumerate(SENSOR.iter_scans(), start=1):
##Collect and store sensor data. Distances are stored in such a way
##that the corresponding index for each value represents its angle on
##the standard Cartesian plane. Due to the clockwise rotation of the
##sensor, values are also read in clockwise before being adjusted here.
##Distances are converted from millimeters to inches.
for (_, angle, distance) in scan:
if (distance == 0.0):
continue
cartAngle = getCartesianAngle(floor(angle))
if (sensorDistances[cartAngle] < SNS_RANGE):
cartAngle = getCartesianAngle(ceil(angle))
if (sensorDistances[cartAngle] < SNS_RANGE):
cartAngle = getCartesianAngle(round(angle))
sensorDistances[cartAngle] = distance * 0.0393
if (i == TOTAL_SCANS):
break
##Prevents adafruit_rplidar.py runtime error when attempting to collect
##data again
SENSOR.stop()
SENSOR.disconnect()
SENSOR.connect()
return sensorDistances
def rotateMachine(turnCW, stopMin, stopMax):
#####################################
##
## VARIABLE DECLARATION
##
#####################################
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.
#####################################
while ROBOT.step(32) != -1:
for i in range(3):
if (turnCW):
WHEELS[i].setVelocity(TURN_SPEED)
else:
WHEELS[i].setVelocity(-1 * TURN_SPEED)
sensorDistances = SENSOR.getRangeImage()[::-1]
for i in range(360):
distance = sensorDistances[i] * 39.3701
angle = getCartesianAngle(i)
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):
for i in range(3):
WHEELS[i].setVelocity(0.0)
break
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.
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):
WHEELS[x].setVelocity(MAX_SPEED * wheelSpeeds[x])
watchAngles = getPathAngles(DES_OPP_ANGLE)
while ROBOT.step(32) != -1:
sensorDistances = SENSOR.getRangeImage()[::-1]
for i in range(360):
distance = sensorDistances[i] * 39.3701
angle = getCartesianAngle(i)
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):
for i in range(3):
WHEELS[i].setVelocity(0.0)
break
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.
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):
WHEELS[x].setVelocity(MAX_SPEED * wheelSpeeds[x])
while ROBOT.step(32) != -1:
sensorDistances = SENSOR.getRangeImage()[::-1]
for i in range(360):
distance = sensorDistances[i] * 39.3701
angle = getCartesianAngle(i)
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):
for i in range(3):
WHEELS[i].setVelocity(0.0)
break
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 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 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