def move_robot(particles, direction, amount):
distNoise = 0.1 * amount
driveAngleNoise = 0.0002 * amount
turnAngleNoise = 0.001 * amount
if direction == 'f':
remaining, stopDir = mvmt.goForward(amount / 100.0)
particle.add_uncertainty(particles, distNoise, driveAngleNoise)
move_particles(particles, (amount - remaining), 0)
return remaining, stopDir
elif direction == 'b':
remaining, stopDir = mvmt.goBack(amount / 100.0)
particle.add_uncertainty(particles, distNoise, driveAngleNoise)
move_particles(particles, -(amount - remaining), 0)
return remaining, stopDir
elif direction == 'r':
mvmt.goRight(amount)
particle.add_uncertainty(particles, 0, turnAngleNoise)
move_particles(particles, 0, -amount)
return 0, 'n'
elif direction == 'l':
mvmt.goLeft(amount)
particle.add_uncertainty(particles, 0, turnAngleNoise)
move_particles(particles, 0, amount)
return 0, 'n'
else:
print("Not a valid direction")
return -1, 'n'
def move_robot(oneMeter, particles, direction, amount):
distNoise = 0.1
driveAngleNoise = 0.0002 * amount
turnAngleNoise = 0.001 * amount
if (direction == 'r' or direction == 'l') and amount < 8:
amount += 5
if direction == 'f':
remaining, stop_dir = mvmt.goForward(amount/100.0, oneMeter)
particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise)
move_particles(particles, (amount - (remaining*100)), 0)
return remaining, stop_dir, particles
elif direction == 'b':
remaining, stop_dir = mvmt.goBack(amount/100.0, oneMeter)
particle.add_uncertainty(particles, (amount - (remaining*100))*distNoise, driveAngleNoise)
move_particles(particles, -(amount - (remaining*100)), 0)
return remaining, stop_dir, particles
elif direction == 'r':
mvmt.goRight(amount)
particle.add_uncertainty(particles, 5, turnAngleNoise)
move_particles(particles, 0, amount)
return 0,'n', particles
elif direction == 'l':
mvmt.goLeft(amount)
particle.add_uncertainty(particles, 5, turnAngleNoise)
move_particles(particles, 0, -amount)
return 0,'n', particles
else:
print("Not a valid direction")
return -1, 'n', particles
def navigateRandom(meters):
distLeft = meters
obstDir = "n"
while (distLeft != 0):
pingDists = mvmt.checkPing()
if (obstDir == "f"):
if (pingDists[0] > pingDists[2]):
mvmt.goLeft(20)
else:
mvmt.goRight(20)
elif (obstDir == "l"):
mvmt.goRight(5)
elif (obstDir == "r"):
mvmt.goLeft(5)
distLeft, obstDir = mvmt.goForward(distLeft)