def step():
#io.sonarMonitor(True)
s = io.getSonars()
x,y,theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
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():
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():
#io.sonarMonitor(True)
s = io.getSonars()
x, y, theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
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
sonars = io.getSonars()
#boundary_finder(sonars)
boundary_follower(sonars)
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
sonars = io.getSonars()
#boundary_finder(sonars)
boundary_follower(sonars)
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 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():
sonars = io.getSonars()
(distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars)
print 'd_o =',distanceRight,' theta =',theta
robot.distances.append(distanceRight)
Kp = 1
Ka = 0.632
rotationalVelocity = Ka*((Kp/Ka)*(0.4 - distanceRight) - theta)
io.setRotational(rotationalVelocity)
computePlotValues(rotationalVelocity)
def step():
sonars = io.getSonars()
(distanceRight, theta) = sonarDist.getDistanceRightAndAngle(sonars)
print 'd_o =',distanceRight,' theta =',theta
if theta == None:
theta = robot.rvels[-1]
if distanceRight == None:
distanceRight = robot.distances[-1]
robot.distances.append(distanceRight)
rotationalVelocity = Kp * (desiredRight - distanceRight) + Ka * (desiredAngle - theta)
robot.rvels.append(rotationalVelocity)
io.setRotational(rotationalVelocity)
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():
#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():
distance = distanceFromVoltage(io.getAnalogInputs()[0])
print io.getSonars()[3], distance
io.setForward((distance-0.5)*2) #replace with your code
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)
