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