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()
