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 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():
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():
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 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()
def brainStart():
io.setForward(forwardVelocity)
