def step(): sonars = io.getSonars() # current discretized sonar reading left = discretize(sonars[0], sonarMax / numObservations, numObservations - 1) # GRAPHICS if robot.g is not None: # update observation model graph robot.g.updateObsLabel(left) robot.g.updateObsGraph( [obsModel(s).prob(left) for s in xrange(numStates)]) # update belief state robot.estimator.update(left) # GRAPHICS if robot.g is not None: # update belief graph robot.g.updateBeliefGraph( [robot.estimator.belief.prob(s) for s in xrange(numStates)]) # DL6 Angle Controller (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if not theta: theta = 0 print 'Angle too large!' e = desiredRight - distanceRight ROTATIONAL_VELOCITY = Kp * e - Ka * theta io.setForward(FORWARD_VELOCITY) io.setRotational(ROTATIONAL_VELOCITY)
def step(): sonars = io.getSonars() # current discretized sonar reading left = discretize(sonars[0], sonarMax/numObservations, numObservations-1) # GRAPHICS if robot.g is not None: # update observation model graph robot.g.updateObsLabel(left) robot.g.updateObsGraph([obsModel(s).prob(left) for s in xrange(numStates)]) # update belief state robot.estimator.update(left) # GRAPHICS if robot.g is not None: # update belief graph robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) for s in xrange(numStates)]) # DL6 Angle Controller (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if not theta: theta = 0 print 'Angle too large!' e = desiredRight-distanceRight ROTATIONAL_VELOCITY = Kp*e - Ka*theta io.setForward(FORWARD_VELOCITY) io.setRotational(ROTATIONAL_VELOCITY)
def step(): sonars = io.getSonars() (distanceRight, theta) = getDistanceRightAndAngle(sonars) print 'd_o =', distanceRight, ' theta =', theta robot.distances.append(distanceRight) lastAngle = 0 if not theta == None: lastAngle = theta Kps = [1, 3, 10, 30, 100, 300] Kas = [0.632355, 1.095344, 1.99899, 3.46400, 6.324455, 10.95435] i = 2 Kp = Kps[i] Ka = Kas[i] #delay before starting to adjust values if len(robot.distances) > 10: io.setForward(forwardVelocity) rotationalVelocity = Kp * (desiredRight - distanceRight) - Ka * lastAngle #print rotationalVelocity else: io.setForward(0) rotationalVelocity = 0 robot.rvels.append(rotationalVelocity) io.setRotational(rotationalVelocity) inp = io.SensorInput() print inp.odometry.x
def step(): global confident, targetX, targetTheta inp = io.SensorInput() sonars = inp.sonars # current discretized sonar reading left = discretize(sonars[0], sonarMax / numObservations, numObservations - 1) if not confident: # GRAPHICS if robot.g is not None: # update observation model graph robot.g.updateObsLabel(left) robot.g.updateObsGraph( [obsModel(s).prob(left) for s in xrange(numStates)]) if DO_ESTIMATION: # update belief state robot.estimator.update(left) (location, confident) = confidentLocation(robot.estimator.belief) # GRAPHICS if robot.g is not None: # update belief graph robot.g.updateBeliefGraph( [robot.estimator.belief.prob(s) for s in xrange(numStates)]) if confident: targetX = (parkingSpot - location) * ( xMax - xMin) / float(numStates) + inp.odometry.x print 'I am at x =', location, ': proceeding to parking space' # DL6 Angle Controller (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if not theta: theta = 0 print 'Angle too large!' e = desiredRight - distanceRight ROTATIONAL_VELOCITY = Kp * e - Ka * theta io.setForward(FORWARD_VELOCITY) io.setRotational(ROTATIONAL_VELOCITY) else: inp.odometry.theta = util.fixAnglePlusMinusPi(inp.odometry.theta) if inp.odometry.x > targetX + .05 and abs( inp.odometry.theta) < math.pi / 4: io.Action(fvel=-0.2, rvel=0).execute() #drive backwards if necessary elif inp.odometry.x < targetX and abs( inp.odometry.theta) < math.pi / 4: io.Action(fvel=0.2, rvel=0).execute() #drive to desired x location elif inp.odometry.theta < targetTheta - .05: io.Action(fvel=0, rvel=0.2).execute() #rotate elif inp.sonars[3] > .3: io.Action(fvel=0.2, rvel=0).execute() #drive into spot else: io.Action(fvel=0, rvel=0).execute( ) #congratulate yourself (or call insurance company)
def step(): voltage, _, _, _ = io.getAnalogInputs() x, y, t = io.getPosition() robot.voltages.append(voltage) robot.distances.append(-x) io.setForward(5 * (current_distance - 0.5))
def step(): voltage,_,_,_ = io.getAnalogInputs() x,y,t = io.getPosition() robot.voltages.append(voltage) robot.distances.append(-x) io.setForward(5*(current_distance-0.5))
def step(): global pathInd, angHist x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) #print robot.goal currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.maze.indicesToPoint(robot.path[pathInd]) dx = destinationPoint.x-currentPoint.x dy = destinationPoint.y - currentPoint.y #atanVal = math.atan((destinationPoint.y-currentPoint.y)/(destinationPoint.x - currentPoint.x)) atanVal2 = math.atan2(dy,dx) thresh = 0.1 destAng = util.fixAngle02Pi(atanVal2) print 'CurrentPoint'+str(currentPoint) print 'Dest Point'+str(destinationPoint) ## botw = 0.44 ## cellw = (bounds[worldname][2] - bounds[worldname][0])/world.width ## cellh = (bounds[worldname][3] - bounds[worldname][1])/world.height ## numCells = botw/cellw ## print numCells ## ## chckx = robot.path[pathInd] ## chcky = robot.path[pathInd] ## if not robot.maze.isPassable((robot.path[pathInd][0]+ 1,robot.path[pathInd][1])): ## dx ## elif not robot.maze.isPassable((robot.path[pathInd][0]- 1,robot.path[pathInd][1])): ## ## elif not robot.maze.isPassable((robot.path[pathInd][0],robot.path[pathInd][1] + 1)): ## ## elif not robot.maze.isPassable((robot.path[pathInd][0],robot.path[pathInd][1] - 1)): if abs(dx) <= thresh and abs(dy) <= thresh: pathInd +=1 #angHist = currentAngle print '\n\n Im at destination! \n\n'+str(currentPoint) elif abs(destAng - currentAngle) <= thresh: io.setRotational(0) io.setForward(1) print "GOOOOOOO FORWARD" else: #if destAng - currentAngle >=0: io.setForward(0) a = util.fixAngle02Pi(destAng- currentAngle) if a > math.pi: io.setRotational(-a/2) else: io.setRotational(a) print "TURNNNNNNNNN"
def step(): sonars = io.getSonars() robot.distance.append(sonars[3]) distance = sonars[3] if distance != 0.5: io.setForward(gain * (distance - 0.5)) else: io.setForward(0)
def forward_boundary(sonars): turned_left = False for i in xrange(len(sonars)): if sonars[i] < WALL_DISTANCE and i >= 3: turn_left(0.7) io.setForward(0) turned_left = True if not turned_left: io.setForward(FORWARD_ROLL_VELOCITY)
def step(): vNeck,vLeft,vRight,vCommon = io.getAnalogInputs() if lightExists(vLeft, vRight): forward = 0.1 * (desiredDistance - max(vLeft, vRight)) rotational = vLeft - vRight io.setForward(forward) io.setRotational(rotational) else: wall_follower_step()
def step(): sonars = io.getSonars() distance = sonarDist.getDistanceRight(sonars) print "Distance to wall: %.03f" % distance robot.distance.append(distance) # append new distance to list # your code here (to calculate rotationalVelocity) rotationalVelocity = (desiredDistance - distance) * proportionalityConstant io.setForward(0.1) io.setRotational(rotationalVelocity)
def step(): x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) # the following lines compute the robot's current position and angle currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = util.fixAnglePlusMinusPi(theta) fv, rv = driver.drive(robot.path, currentPoint, currentAngle) io.setForward(fv) io.setRotational(rv)
def step(): x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) checkoff.update(globals()) # the following lines compute the robot's current position and angle currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = fixAnglePlusMinusPi(theta) forward_v, rotational_v = drive(robot.path, currentPoint, currentAngle) io.setForward(forward_v) io.setRotational(rotational_v)
def step(): global confident, targetX, targetTheta inp = io.SensorInput() sonars = inp.sonars # current discretized sonar reading left = discretize(sonars[0], sonarMax/numObservations, numObservations-1) if not confident: # GRAPHICS if robot.g is not None: # update observation model graph robot.g.updateObsLabel(left) robot.g.updateObsGraph([obsModel(s).prob(left) for s in xrange(numStates)]) if DO_ESTIMATION: # update belief state robot.estimator.update(left) (location, confident) = confidentLocation(robot.estimator.belief) # GRAPHICS if robot.g is not None: # update belief graph robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) for s in xrange(numStates)]) if confident: targetX = (parkingSpot-location)*(xMax-xMin)/float(numStates)+inp.odometry.x print 'I am at x =',location,': proceeding to parking space' # DL6 Angle Controller (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if not theta: theta = 0 print 'Angle too large!' e = desiredRight-distanceRight ROTATIONAL_VELOCITY = Kp*e - Ka*theta io.setForward(FORWARD_VELOCITY) io.setRotational(ROTATIONAL_VELOCITY) else: inp.odometry.theta = util.fixAnglePlusMinusPi(inp.odometry.theta) if inp.odometry.x>targetX+.05 and abs(inp.odometry.theta)<math.pi/4: io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary elif inp.odometry.x<targetX and abs(inp.odometry.theta)<math.pi/4: io.Action(fvel=0.2,rvel=0).execute() #drive to desired x location elif inp.odometry.theta<targetTheta-.05: io.Action(fvel=0,rvel=0.2).execute() #rotate elif inp.sonars[3]>.3: io.Action(fvel=0.2,rvel=0).execute() #drive into spot else: io.Action(fvel=0,rvel=0).execute() #congratulate yourself (or call insurance company)
def step(): #io.setRotational(0.5) vNeck, vLeft, vRight, _ = io.getAnalogInputs() #print vNeck,'Left:',vLeft,'Right:',vRight, 'Difference: ', vRight-vLeft #io.setForward(0) #io.setRotational(0) #move robot backwards or forwards at a rate proportional to the amount of light the photodiodes are reading average = (vLeft + vRight) / 2 print vLeft, vRight, average if vLeft > 0.18 or vRight > 0.18: io.setForward(-average / 10) seekLight(vNeck) print "light is too close" elif vLeft > 0.16 or vRight > 0.16: seekLight(vNeck) io.setForward(0) print "TARGET IN SIGHT" elif vLeft > 0.155 or vRight > 0.155: seekLight(vNeck) io.setForward(average / 10) print "seeking light" else: io.setRotational(0) io.setForward(0) print "light is off"
def wall_follower_step(): sonars = io.getSonars() (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if theta == None: theta = robot.rvels[-1] if distanceRight == None: distanceRight = robot.distances[-1] print 'd_o =',distanceRight,' theta =',theta robot.distances.append(distanceRight) rotationalVelocity = Kp * (desiredRight - distanceRight) + Ka * (desiredAngle - theta) robot.rvels.append(rotationalVelocity) io.setRotational(rotationalVelocity) io.setForward(0.1)
def step(): global pathInd, angHist x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) #print robot.goal currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.maze.indicesToPoint(robot.path[pathInd]) #to make angVal be current angle measured from -pi to pi if math.pi - theta < 0: currangVal = theta - (math.pi*2) else: currangVal = theta #how much angle has changed already dAng = currangVal - angHist atanVal = math.atan((destinationPoint.y-currentPoint.y)/(destinationPoint.x - currentPoint.x)) thresh = 0.05 destAng = print 'dAng: ' + str(dAng) print 'angVal: ' + str(currangVal) print 'Theta: ' + str(theta) print 'AngHist' + str(angHist) print 'CurrentPoint'+str(currentPoint) print 'Dest Point'+str(destinationPoint) if abs(destinationPoint.x-currentPoint.x) <= .1 and abs(destinationPoint.y - currentPoint.y) <= 0.1: pathInd +=1 angHist = currangVal print 'Im at destination!'+str(currentPoint) elif -thresh < dAng - atanVal < thresh: io.setRotational(0) io.setForward(.1) print 'Within thresh'+str(dAng-atanVal) print 'GO forward' else : io.setForward(0) io.setRotational(dAng - atanVal) print 'Atan' + str(atanVal) print 'Theta'+ str(theta) print 'Turning' + str(dAng - atanVal)
def step(): global pathInd, angHist x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) #print robot.goal currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.maze.indicesToPoint(robot.path[pathInd]) print 'Angle' + str(theta) print angHist print currentPoint print destinationPoint atanVal = math.atan((destinationPoint.y-currentPoint.y)/(destinationPoint.y - currentPoint.x)) thresh = 0.05 print "atan: " + str(atanVal) print angHist - atanVal if angHist + atanVal -thresh <= currentAngle <= angHist + atanVal + thresh: io.setForward(.1) io.setRotational(0) print 'GO' else: io.setForward(0) if abs(angHist + atanVal - currentAngle) < -.5: io.setRotational(-.1) print 'turnR0.1' elif abs(angHist + atanVal - currentAngle) < 0: io.setRotational(-.01) print 'turnR0.01' elif abs(angHist + atanVal - currentAngle) > .5: io.setRotational(.1) print 'turnL0.1' else: io.setRotational(.01) print 'turnL0.01' if abs(destinationPoint.x-currentPoint.x) <= .5 and abs(destinationPoint.y - currentPoint.y) <= 0.5: pathInd +=1 #angHist = currentAngle print 'True' else: print 'False'
def step(): global i x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.path[i] thetad = currentPoint.angleTo(destinationPoint) if util.nearAngle(currentAngle,thetad,math.pi/180.0): io.setForward(0.1) io.setRotational(0) if currentPoint.distance(destinationPoint) < 0.02: i += 1 print i else: theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle) io.setRotational(theta_constant) io.setForward(0)
def step(): global i x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) currentPoint = util.Point(x, y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.path[i] thetad = currentPoint.angleTo(destinationPoint) if util.nearAngle(currentAngle, thetad, math.pi / 180.0): io.setForward(0.1) io.setRotational(0) if currentPoint.distance(destinationPoint) < 0.02: i += 1 print i else: theta_constant = util.fixAnglePlusMinusPi(thetad - currentAngle) io.setRotational(theta_constant) io.setForward(0)
def step(): global pathInd, angHist x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) #print robot.goal currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.maze.indicesToPoint(robot.path[pathInd]) dx = destinationPoint.x-currentPoint.x dy = destinationPoint.y - currentPoint.y #atanVal = math.atan((destinationPoint.y-currentPoint.y)/(destinationPoint.x - currentPoint.x)) atanVal2 = math.atan2(dy,dx) thresh = 0.01 destAng = fixAngle02Pi(atanVal2) print 'CurrentPoint'+str(currentPoint) print 'Dest Point'+str(destinationPoint) if atanVal + angHist < 0: destAng = 2* math.pi + atanVal + angHist else: destAng = atanVal + angHist print 'CurrAng'+str(currentAngle) if abs(dx) <= thresh and abs(dy) <= thresh: pathInd +=1 angHist = currentAngle print '\n\n Im at destination! \n\n'+str(currentPoint) elif ## elif -thresh< atanVal < thresh: ## io.setRotational(0) ## io.setForward(math.sqrt(dx**2 + dy**2)) ## print 'Within thresh \n Atan:'+str(atanVal) ## print 'GO forward' elif -thresh < (destAng - currentAngle) < thresh : io.setRotational(0) io.setForward(math.sqrt(dx**2 + dy**2)) print 'Within thresh'+str(destAng - currentAngle) print 'GO forward' print atanVal elif destAng - currentAngle > math.pi: io.setForward(0) io.setRotational(-(destAng - currentAngle)/2) print 'Atan:'+str(atanVal) print 'Turning Right' + str(destAng - currentAngle) print atanVal elif destAng - currentAngle < math.pi: io.setForward(0) io.setRotational(destAng - currentAngle) print 'Atan' + str(atanVal) print 'Turning' + str(destAng - currentAngle) print atanVal
def boundary_finder(sonars): if sonars[3] > WALL_DISTANCE + BUFFER: print 'setting forward velocity' io.setForward(FORWARD_ROLL_VELOCITY) elif sonars[3] < WALL_DISTANCE - BUFFER: print 'setting other velocity' io.setForward(BACKWARD_ROLL_VELOCITY) else: io.setForward(0) print sonars[3]
def step(): #io.sonarMonitor(True) #read in the sonar readings from the robot. #s will be a list of 8 values, with the value at index #0 representing the left-most sonar s = io.getSonars() #print the reading from the central sonar print s[1] #set the robot's forward and rotational velocities to 0 if s[1] < 0.2: io.setForward(-0.30 + s[1]) print("backwards " + str(-0.20 + s[1])) elif s[1] == 0.2: io.setForward(0) print("stop") elif s[1] > 0.3: io.setForward(s[1] - 0.3) print("forward " + str(s[1] - 0.3)) else: io.setForward(0) io.setRotational(0)
def step(): #io.sonarMonitor(True) s = io.getSonars() x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) if s[3] < 0.2: #s[3] is the right sonar io.setForward(0.1) io.setRotational(0.4) elif s[2] == 0.3 and s[3] == 0.3: #s[2] is the right-center sonar io.setForward(0.05) io.setRotational(0) else: io.setForward(0.1) if s[2] > 0.4: io.setRotational(-0.4) elif s[2] < 0.3: io.setRotational(0.5) else: io.setRotational(0) print str(s[2]) + ',' + str(s[3])
def step(): if len(robot.path) == 0: io.setForward(0) io.setRotational(0) return x, y, theta = io.getPosition() robot.slimeX.append(x) robot.slimeY.append(y) currentPoint = util.Point(x,y).add(robot.initialLocation) currentAngle = theta destinationPoint = robot.maze.indicesToPoint(robot.path[0]) desiredTheta = math.atan2(float(destinationPoint.y - currentPoint.y), float(destinationPoint.x - currentPoint.x)) if desiredTheta < 0: desiredTheta += math.pi * 2 if util.nearAngle(desiredTheta, theta, 0.1): io.setForward(0.3) io.setRotational(0) else: io.setForward(0) if desiredTheta > theta: if desiredTheta - theta < math.pi: io.setRotational(.5) else: io.setRotational(-.5) else: if desiredTheta - theta < math.pi: io.setRotational(-.5) else: io.setRotational(.5) if closeToPoint(currentPoint, destinationPoint, 0.1): if len(robot.path) > 0: robot.path.pop(0)
def brainStart(): io.setForward(0.1) #start robot moving forward at 0.1 m/s
def brainStart(): io.setForward(0.1)
def brainStart(): io.setForward(0.1) # start robot moving forward at 0.1 m/s
def step(): global confident, targetX, targetTheta sonars = io.getSonars() pose = io.getPosition(True) (px, py, ptheta) = pose if confident: ptheta = util.fixAnglePlusMinusPi(ptheta) if px>targetX+.05 and abs(ptheta)<math.pi/4: io.Action(fvel=-0.2,rvel=0).execute() #drive backwards if necessary elif px<targetX and abs(ptheta)<math.pi/4: io.Action(fvel=0.2,rvel=0).execute() #drive to desired x location elif ptheta<targetTheta-.05: io.Action(fvel=0,rvel=0.2).execute() #rotate elif sonars[3]>.3: io.Action(fvel=0.2,rvel=0).execute() #drive into spot else: io.Action(fvel=0,rvel=0).execute() #congratulate yourself (or call insuran return # Quality metric. Important to do this before we update the belief state, because # it is always a prediction parkingSpaceSize = .75 robotWidth = 0.3 margin = (parkingSpaceSize - robotWidth) / 2 # Robot is about .3 meters wide. Parking place is .75 trueX = io.getPosition(True)[0] robot.probMeasures.append(estimateQualityMeasure(robot.estimator.belief, xMin, xMax, numStates, margin, trueX)) trueS = discretize(trueX, (xMax - xMin)/numStates, valueMin = xMin) n = len(robot.probMeasures) if n == 80: brainStop() # current discretized sonar reading left = discretize(sonars[0], sonarMax/numObservations, numObservations-1) robot.data.append((trueS, ideal[trueS], left)) # obsProb obsProb = sum([robot.estimator.belief.prob(s) * OBS_MODEL_TO_USE(s).prob(left) \ for s in xrange(numStates)]) # GRAPHICS if robot.g is not None: # redraw ideal distances (compensating for tk bug on macs) # draw robot's true state if trueS < numStates: robot.g.updateDist() robot.g.updateTrueRobot(trueS) # update observation model graph robot.g.updateObsLabel(left) robot.g.updateObsGraph([OBS_MODEL_TO_USE(s).prob(left) \ for s in xrange(numStates)]) robot.estimator.update(left) (location, confident) = confidentLocation(robot.estimator.belief) if confident: targetX = (parkingSpot-location)*(xMax-xMin)/float(numStates) + px print 'I am at x =',location,': proceeding to parking space' # GRAPHICS if robot.g is not None: # update world drawing # update belief graph robot.g.updateBeliefGraph([robot.estimator.belief.prob(s) \ for s in xrange(numStates)]) # DL6 Angle Controller (distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars) if not theta: theta = 0 e = desiredRight-distanceRight ROTATIONAL_VELOCITY = Kp*e - Ka*theta io.setForward(FORWARD_VELOCITY) io.setRotational(ROTATIONAL_VELOCITY)
def brainStart(): io.setForward(forwardVelocity)
def step(): vNeck,vLeft,vRight,_ = io.getAnalogInputs() print 'Neck:',vNeck,'Left:',vLeft,'Right:',vRight pass #Your code here io.setForward(0) io.setRotational(0)
def step(): distance = distanceFromVoltage(io.getAnalogInputs()[0]) print io.getSonars()[3], distance io.setForward((distance-0.5)*2) #replace with your code
def step(): global inp robot.count += 1 inp = io.SensorInput(cheat=True) # discretize sonar readings # each element in discreteSonars is a tuple (d, cells) # d is the distance measured by the sonar # cells is a list of grid cells (r,c) between the sonar and the point d meters away discreteSonars = [] for (sonarPose, distance) in zip(sonarDist.sonarPoses,inp.sonars): if NOISE_ON: distance = noiseModel.noisify(distance, gridSquareSize) sensorIndices = robot.map.pointToIndices(inp.odometry.transformPose(sonarPose).point()) hitIndices = robot.map.pointToIndices(sonarDist.sonarHit(distance, sonarPose, inp.odometry)) ray = util.lineIndices(sensorIndices, hitIndices) discreteSonars.append((distance, ray)) # figure out where robot is startPoint = inp.odometry.point() startCell = robot.map.pointToIndices(startPoint) # if necessary, make new plan if robot.plan is None: print 'REPLANNING' robot.plan = search.ucSearch(robot.successors, robot.map.pointToIndices(inp.odometry.point()), lambda x: x == robot.goalIndices, lambda x: 0) isGood = True for i in robot.plan: if not robot.map.isPassable(i) and isGood: print 'REPLANNING' robot.plan = search.ucSearch(robot.successors, robot.map.pointToIndices(inp.odometry.point()), lambda x: x == robot.goalIndices, lambda x: 0) isGood = False # graphics (draw robot's plan, robot's location, goalPoint) # do not change this block for w in robot.windows: w.redrawWorld() robot.windows[-1].markCells(robot.plan,'blue') robot.windows[-1].markCell(robot.map.pointToIndices(inp.odometry.point()),'gold') robot.windows[-1].markCell(robot.map.pointToIndices(goalPoint),'green') # update map for (d,cells) in discreteSonars: if d != 5.0: robot.map.sonarHit(cells[-1]) # if we are within 0.1m of the goal point, we are done! if startPoint.distance(goalPoint) <= 0.1: io.Action(fvel=0,rvel=0).execute() code = checkoff.generate_code(globals()) raise Exception('Goal Reached!\n\n%s' % code) # otherwise, figure out where to go, and drive there destinationPoint = robot.map.indicesToPoint(robot.plan[0]) while startPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1: robot.plan.pop(0) destinationPoint = robot.map.indicesToPoint(robot.plan[0]) currentAngle = inp.odometry.theta angleDiff = startPoint.angleTo(destinationPoint)-currentAngle angleDiff = util.fixAnglePlusMinusPi(angleDiff) fv = rv = 0 if startPoint.distance(destinationPoint) > 0.1: if abs(angleDiff) > 0.1: rv = angleDiff else: fv = 1.5*startPoint.distance(destinationPoint) io.setForward(fv) io.setRotational(rv) # render the drawing windows # do not change this block for w in robot.windows: w.render()