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()
def brainStop(): for i in range(len(robot.slimeX)): robot.window.drawPoint(robot.slimeX[i], robot.slimeY[i], 'red') print 'Hex Code for Tutor:\n%s' % checkoff.generate_code(globals())
def step(): global inp robot.count += 1 inp = io.SensorInput(cheat=True) for c in ('orange','cyan','blue','red','gold'): robot.map.clearColor(c) if robot.map.doHeatMap: robot.map.heatMap() # 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,d) in zip(sonarDist.sonarPoses,inp.sonars): if NOISE_ON: d = noiseModel.noisify(d,gridSquareSize) if d < 1.5: discreteSonars.append((d,util.lineIndices(robot.map.pointToIndices(inp.odometry.transformPose(sonarPose)), robot.map.pointToIndices(sonarDist.sonarHit(d, sonarPose, inp.odometry))))) # update map for (d,cells) in discreteSonars: robot.map.sonarHit(cells[-1]) # if necessary, make new plan secondary_sonars = discreteSonars[:3] + discreteSonars[5:] if robot.plan is None or tooCloseToWall(discreteSonars[3:5], secondary_sonars): print 'REPLANNING' robot.plan = search.ucSearch(search.MazeSearchNode(robot.map, robot.map.pointToIndices(inp.odometry.point()),None,0), lambda x: x == robot.map.pointToIndices(goalPoint), lambda x: 0) # graphics (draw robot's plan, robot's location, goalPoint) # do not change this block if robot.map.showPassable: robot.map.markNotPassable() if robot.plan is not None: robot.map.markCells(robot.plan,'blue') robot.map.markCell(robot.map.pointToIndices(inp.odometry.point()),'gold') robot.map.markCell(robot.map.pointToIndices(goalPoint),'green') # move to target point (similar to driving task in DL2) currentPoint = inp.odometry.point() currentAngle = inp.odometry.theta destinationPoint = robot.map.indicesToPoint(robot.plan[0]) while currentPoint.isNear(destinationPoint,0.1) and len(robot.plan)>1: robot.plan.pop(0) destinationPoint = robot.map.indicesToPoint(robot.plan[0]) if not currentPoint.isNear(destinationPoint,0.1): angle = util.fixAnglePlusMinusPi(currentPoint.angleTo(destinationPoint)-currentAngle) if abs(angle)<0.1: #if close enough to pointing, use proportional control on forward velocity fv = 2*currentPoint.distance(destinationPoint) rv = 0 else: #otherwise, use proportional control on angular velocity fv = 0 rv = 2*angle else: raise Exception,'Goal Reached!\n\n%s' % checkoff.generate_code(globals()) robot.map.render() io.Action(fvel=fv,rvel=rv).execute()