def moveToBottle_2(currentPosition, bottlePosition, particles): cx = currentPosition[0] cy = currentPosition[1] ctheta = math.radians(currentPosition[2]) # We store the theta in degree wx = bottlePosition[0] wy = bottlePosition[1] angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta, currentPosition) turn.turn(math.degrees(angle)) particles = [ updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles) ] preAngle = interface.getMotorAngle(motors[0])[0] _gostraight = gostraight.go() distance, angle = _gostraight.run(30) aftAngle = interface.getMotorAngle(motors[0])[0] disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124)) particles = [ update(particles[i], disMoved) for i in range(numberOfParticles) ] particles = [ updateRotation(particles[i], angle) for i in range(numberOfParticles) ] return particles
def navigateToWayPoint(wx, wy, currentPosition, particles, sonic, range_index): Drawer = pds.Canvas() _gostraight = gostraight.go() bottle_particles = [] cx = currentPosition[0] cy = currentPosition[1] ctheta = math.radians(currentPosition[2]) # We store the theta in degree #Calculate the distance and Angle to turn angle, distance = calculate_angle_distance(cx, cy, wx, wy, ctheta, currentPosition) #Let it rotate towards that position turn.turn(math.degrees(angle)) particles = [ updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles) ] everymotion = 20.0 stop_times = int(distance / everymotion) remain_distance = distance - stop_times * everymotion bottle_particles = [] for index in range( stop_times): #Every 20 cm, stop and search for the bottle preAngle = interface.getMotorAngle(motors[0])[0] _gostraight.run(everymotion) aftAngle = interface.getMotorAngle(motors[0])[0] disMoved = abs((aftAngle - preAngle) / (math.pi * 0.124)) particles = [ update(particles[i], disMoved) for i in range(numberOfParticles) ] particles = mcl(particles) Drawer.drawParticles(particles) if index >= 1: #bottle_particles.append(detect_bottle_2(sonic,getCurrentPosition(particles),bottle_particles)) bottle_particles = detect_bottle(sonic, getCurrentPosition(particles), bottle_particles, range_index) if bottle_particles: print 'Bottle_position:', bottle_particles print 'current_Position:', getCurrentPosition(particles) #Drawer.drawParticles(bottle_particles) #sorted_d = fuse_information(bottle_particles,range_index) # particles = moveToBottle(getCurrentPosition(particles),getBottlePosition_2(sorted_d),particles) particles = moveToBottle_2(getCurrentPosition(particles), getBottlePosition(bottle_particles), particles) return particles
def navigateToWayPoint(wx, wy, currentPosition, particles): cx = currentPosition[0] cy = currentPosition[1] ctheta = math.radians(currentPosition[2]) # We store the theta in degree distance = math.hypot(wx - cx, wy - cy) # This is the main distance print "navigating to " + str(wx) + "," + str(wy) + " from " + str(currentPosition) print "distance is " + str(distance) angle = (math.atan2(wy - cy, wx - cx)) - ctheta # math.atan2 returns in radians print "angle before is " + str(angle) if abs(angle) >= math.pi: if angle > 0: angle = -((math.pi * 2) - angle) else: angle = -((-math.pi * 2) - angle) print "angle to turn is " + str(angle) if abs(math.degrees(angle) - 90) < 8: turn.turn(90) particles = [updateRotation(particles[i], 90) for i in range(numberOfParticles)] elif abs(math.degrees(angle) + 90) < 8: turn.turn(-90) particles = [updateRotation(particles[i], -90) for i in range(numberOfParticles)] else: turn.turn(math.degrees(angle)) particles = [updateRotation(particles[i], math.degrees(angle)) for i in range(numberOfParticles)] Angle_before = interface.getMotorAngle(0)[0] _gostraight = gostraight.go() distanceMoved = _gostraight.run(distance) print "dist moved: " + str(distanceMoved) if distanceMoved[1] is 90: particles = [update(particles[i], distanceMoved[0]) for i in range(numberOfParticles)] return particles,True else: Angle_after = interface.getMotorAngle(0)[0] d = (Angle_after - Angle_before) / (-math.pi * 0.124) d = abs(d) print 'd:', d particles = [update(particles[i], d) for i in range(numberOfParticles)] # print "Particles are now " + str(particles) # update particles # particles = mcl(particles) # print "Current Position is " + str(currentPosition) return particles,False
] # canvas = pds.Canvas() # mymap = pds.Map() # pds.drawWall(mymap, canvas) particles = [initialPosition for i in range(numberOfParticles)] # canvas.drawParticles(particles) signatures = prb.SignatureContainer() # bumper = bumper.Bumper() # bumperThread = threading.Thread(name='bumper', target=bumper.getTouch) # bumperThread.start() _sonnic = sonic.Sonic() _gostraight = gostraight.go() _dir = 1 def ConvertToHisto(readings): step = 10 s = [0] * (200 / step) for currenttuple in readings: currentAngle = currenttuple[0] currentDistance = currenttuple[1] for idx, freq in enumerate(s): # print "Comparing current Distance " + str(currentDistance) + " against " + str(step*(idx+1)) if (step * (idx + 1) > currentDistance): # print str(currentDistance) + " has been allocated to " + str(idx+1)
import navigateToWayPoint as nwp import turn as turn import gostraight import sonic lengthOfPi = 10 waypoint1 = (84, 30, 0) waypointa = (157.5, 30, 0) waypointab = (126, 30, 1) waypointb = (126, 120, 0) waypointbc = (126, 112, 1) waypointc = (42, 80, 0) waypoints = [waypointa, waypointab, waypointb, waypointbc, waypointc] ob = gostraight.go() dir = 1 row = 1 noOfRows = 4 halfDistance = 100 while (1): distance = ob.run(800) if (row == noOfRows): ob.goback(halfDistance - 20) turn.turn(90 * dir) ob.run(lengthOfPi) turn.turn(-90 * dir) ob.run(20) turn.turn(180) dir *= -1