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)
s = io.getSonars()
x,y,theta = io.getPosition()
robot.slimeX.append(x)
robot.slimeY.append(y)
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 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 computePlotValues(rotationalVelocity):
robot.rvels.append(rotationalVelocity)
current = time.time()
if robot.pTi is not None:
dT = current-robot.pTi
else:
dT = 0.1
robot.pTi = current
t = io.getPosition()[2]
if robot.pTh is not None:
robot.rvels2.append(util.fixAnglePlusMinusPi(t-robot.pTh)/dT)
robot.pTh = t
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 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():
#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 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)
