def getNextValues(self, state, inp):
# inp is a tuple (distanceRight, angle)
error = desiredRight - inp[0]
if inp[1] == None:
output = io.Action(forwardVelocity, rvel=0)
output = io.Action(forwardVelocity, rvel=k3 * error - k4 * inp[1])
return state, output
def getNextValues(self, state, inp):
if state == "initialize":
if not wall_ahead(inp.sonars):
fvel = 0.03
rvel = 0.0
return (state, io.Action(fvel=fvel, rvel=rvel))
else:
fvel = 0.0
rvel = 0.0
return ("follow", io.Action(fvel=fvel, rvel=rvel))
if state == "follow":
if wall_ahead(inp.sonars):
fvel = 0.0
rvel = 0.03
print "turning left", "cause wall ahead"
return ("follow", io.Action(fvel=fvel, rvel=rvel))
elif right_wall_too_close(inp.sonars):
fvel = 0.01
rvel = 0.03
print "turning left", "cause wall too close"
return ("follow", io.Action(fvel=fvel, rvel=rvel))
elif right_wall_too_far(inp.sonars):
fvel = 0.01
rvel = -0.03
print "turning right", "cause wall too far"
return ("follow", io.Action(fvel=fvel, rvel=rvel))
def get_next_values(self, state, inp):
(goalPoint, sensors) = inp
robotPose = sensors.odometry
robotPoint = robotPose.point()
robotTheta = robotPose.theta
nearGoal = robotPoint.is_near(goalPoint, self.dist_eps)
headingTheta = robotPoint.angleTo(goalPoint)
r = robotPoint.distance(goalPoint)
if nearGoal:
# At the right place, so do nothing
a = io.Action()
elif util.near_angle(robotTheta, headingTheta, self.angle_eps):
# Pointing in the right direction, so move forward
a = io.Action(
fvel=util.clip(r *
self.forwardGain, -self.maxFVel, self.maxFVel))
else:
# Rotate to point toward goal
headingError = util.fix_angle_plus_minus_pi(headingTheta -
robotTheta)
a = io.Action(
rvel=util.clip(headingError *
self.rotationGain, -self.maxRVel, self.maxRVel))
return (nearGoal, a)
def getNextValues(self, state, inp):
forwardDist = min(inp.sonars[3], inp.sonars[4])
wallDist = inp.sonars[7]
if state == 'searching':
delta = forwardDist - self.targetDist
if abs(delta) > self.tolerance:
return (state, self.stepForward(delta))
else:
return ('following', io.Action(fvel=0, rvel=0))
if state == 'following':
steerAwayDelta = self.getSteerAwayDelta(inp)
print '1-', steerAwayDelta
if steerAwayDelta is not None:
delta = forwardDist - self.targetDist
return (state, self.stepCCW(delta))
steerToDelta = self.getSteerToDelta(inp)
print '2-', steerToDelta
if steerToDelta is not None:
delta = wallDist - self.targetDist
return (state, self.stepCW(delta))
return (state, self.stepForward(0.1))
return (state, io.Action(fvel=0, rvel=0))
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 getNextValues(self, state, inp):
print inp.sonars # list
print inp.odometry.theta
if inp.sonars[2] >= 0.5 and inp.sonars[1] >= 0.5:
return (state, io.Action(fvel=0.5, rvel=0))
if inp.sonars[2] < 0.5 and inp.sonar[1] > 3:
return (state, io.Action(fvel=0, rvel=0.5))
if inp.sonars[2] < 0.5 and inp.sonar[1] <= 3:
return (state, io.Action(fvel=0, rvel=-1))
def getNextValues(self, state, inp):
currentDist = min(inp.sonars[3], inp.sonars[4])
delta = currentDist - self.targetDist
if abs(delta) > self.tolerance:
fvel = self.coeff * delta * self.fvelMax
fvel = max(min(fvel, self.fvelMax), -self.fvelMax)
return (currentDist, io.Action(fvel=fvel, rvel=0))
else:
return (currentDist, io.Action(fvel=0, rvel=0))
def getNextValues(self, state, inp):
if state == 'start':
if inp.sonars[3] >=.5:
return ('moveForward', io.Action(fvel = 0.3, rvel = 0.0))
else:
return('stop', io.Action(fvel = 0.0, rvel = 0.0))
elif state == 'moveForward':
if inp.sonars[3] >=.5:
return ('moveForward', io.Action(fvel = 0.3, rvel = 0.0))
else:
return ('stop', io.Action(fvel = 0.0, rvel = 0.0))
else:
return('stop', io.Action(fvel = 0.0, rvel = 0.0))
def forwardx(speed, x): #moving from x to 0
while io.SensorInput().odometry.x > x:
print 'Sonar:' + str(io.SensorInput().sonars[2])
if io.SensorInput().sonars[2] == 0.18 or (
io.SensorInput().sonars[2] > 0.18
and io.SensorInput().sonars[2] < 0.25):
io.Action(fvel=0, rvel=0).execute()
sleep(0.1)
io.Action(0, 0).execute()
else:
io.Action(fvel=speed, rvel=0).execute()
sleep(0.1)
io.Action(0, 0).execute()
def forwardy(speed, y): #moving from y to y=0
while io.SensorInput().odometry.y > y:
print 'Sonar:' + str(io.SensorInput().sonars[2])
if io.SensorInput().sonars[2] == 0.18 or (
io.SensorInput().sonars[2] > 0.18
and io.SensorInput().sonars[2] < 0.25):
io.Action(0, 0).execute()
sleep(0.1)
io.Action(0, 0).execute()
else:
io.Action(speed, 0).execute()
sleep(0.1)
io.Action(0, 0).execute()
def getNextValues(self, state, inp):
p = 0
print state
if state == 0:
# print inp.sonars[0]
print inp.sonars[2]
if p == 0:
if inp.sonars[2]< 0.3:
nextstate = 0
output = (io.Action(fvel = -0.5, rvel = 0))
elif inp.sonars[2]>=0.5:
if inp.sonars[0] == 5 and inp.sonars[3] == 5 and inp.sonars[4]>0.6:
nextstate = 2
output = (io.Action(fvel = 0.5, rvel = -0.2))
else:
nextstate = 0
output = (io.Action(fvel = 0.5, rvel = 0))
elif inp.sonars[2]>=0.3 and inp.sonars[2]<0.5:
nextstate = 1
output = (io.Action(fvel = 0, rvel = 0.2))
elif p == 1:
if inp.sonars[2]>=0.3 and inp.sonars[2]<0.5:
nextstate = 1
output = (io.Action(fvel = 0, rvel = 0.2))
if inp.sonars[4]>0.5 and inp.sonars[3]>0.75:
nextstate = 2
output = (io.Action(fvel = 0, rvel = -0.2))
elif state == 1:
v = inp.sonars[3]
c = m.sqrt(2)*inp.sonars[4]
print v
print c
if v >= c-0.005 and v < c+0.005:
p = 1
nextstate = 0
output = (io.Action(fvel = 0.5, rvel = 0))
else:
nextstate = 1
output = (io.Action(fvel = 0, rvel = 0.2))
elif state == 2:
print state
if inp.sonars[2]>1 and inp.sonars[3]<0.75:
nextstate = 0
output = (io.Action(fvel = 0.5, rvel = 0))
else:
nextstate = 2
output = (io.Action(fvel = 0, rvel = -0.2))
return(nextstate, output)
def actionToPoint(goalPoint, robotPose, forwardGain, rotationGain, maxVel,
angleEps):
"""
Internal procedure that returns an action to take to drive
toward a specified goal point.
"""
rvel = 0
fvel = 0
robotPoint = robotPose.point()
# Compute the angle between the robot and the goal point
headingTheta = robotPoint.angleTo(goalPoint)
# Difference between the way the robot is currently heading and
# the angle to the goal. This is an angular error relative to the
# robot's current heading, in the range +pi to -pi.
headingError = util.fixAnglePlusMinusPi(headingTheta - robotPose.theta)
if util.nearAngle(robotPose.theta, headingTheta, angleEps):
# The robot is pointing toward the goal, so it's okay to move
# forward. Note that we are actually doing two proportional
# controllers simultaneously: one to reduce angular error
# and one to reduce distance error.
distToGoal = robotPoint.distance(goalPoint)
fvel = distToGoal * forwardGain
rvel = headingError * rotationGain
else:
# The robot is not pointing close enough to the goal, so don't
# start moving foward yet. This is a proportional controller
# to reduce angular error.
rvel = headingError * rotationGain
return io.Action(fvel=util.clip(fvel, -maxVel, maxVel),
rvel=util.clip(rvel, -maxVel, maxVel))
def getNextValues(self, state, inp):
# Replace this definition
print 'DynamicMoveToPoint', 'state=', state, 'inp=', inp
assert isinstance(inp, tuple), 'inp should be a tuple'
assert len(inp) == 2, 'inp should be of length 2'
assert isinstance(inp[0], util.Point), 'inp[0] should be a Point'
return (state, io.Action())
def getNextValues(self, state, inp):
#Your code here
nextState = 0
k = 10 # checkoff1: case1 k=0.5, case2 k=10, case3 k=100
diff = dDesired - inp
forward = -(k * diff)
return (nextState, io.Action(fvel=forward, rvel=0))
def rotate_cw(speed, setangle):
io.Action(fvel=0, rvel=-1 * speed).execute()
sleep(5)
io.Action(fvel=0, rvel=0).execute()
angle = True
while angle:
io.Action(fvel=0, rvel=-1 * speed).execute()
sleep(0.1)
io.Action(fvel=0, rvel=0).execute()
if io.SensorInput().odometry.theta != 0:
angle = float(io.SensorInput().odometry.theta) > setangle
print angle, io.SensorInput().odometry.theta
#ebot.wheels(1*speed, -1*speed)
#sleep(duration)
pass
def getNextValues(self, state, inp):
state, orig_angle = state
angle = util.fixAnglePlusMinusPi(inp.odometry.theta)
front_dist = inp.sonars[2]
eps = 0.01
if state == 'forward':
if front_dist <= 0.5:
next_state = ('rotate', angle)
forward = 0.0
rotation = 0.1
else:
next_state = (Q1, Q2)
forward = 0.1
rotation = 0.0
elif state == 'rotate':
if not util.nearAngle(abs(angle - orig_angle), math.pi / 2.0, eps):
next_state = (Q3, Q4)
forward = 0.0
rotation = 0.1
else:
next_state = (Q5, Q6)
forward = 0.1
rotation = 0.0
print next_state, forward, rotation
return (next_state, io.Action(fvel=forward, rvel=rotation))
def getNextValues(self, state, inp):
currentError = desiredRight - inp
lastError = desiredRight - state
k1 = 100
k2 = -97.36
velocity = k1 * currentError + k2 * lastError
output = io.Action(fvel=forwardVelocity, rvel=velocity)
return inp, output
def getNextValues(self, state, inp):
left = inp.analogInputs[1]
right = inp.analogInputs[2]
gain = 1.0 # try varying this to increase speed
ctrl_out = 5.0 + gain * (left - right)
nextstate = ctrl_out
v_out = ctrl_out # change to v_out = state for extra delay
return (nextstate, io.Action(fvel=0.0, rvel=0.0, voltage=v_out))
def getNextValues(self, state, inp): #taking in sensory data as its input
print inp.sonars# list
print inp.odometry.theta
if state == 0: #Detect wall
if inp.sonars[2] < 0.5: #Rotate cw while moving fwd when d<0.5
return (1, io.Action(fvel = 0.05, rvel = 0.2))
else: #Keep going until d<0.5
return (0, io.Action(fvel = 0.1, rvel = 0.0))
if state == 1: #Snake-ing
if inp.sonars[2] > 0.5 and inp.sonars[4] > 0.5: #rotate back to wall(ccw)
return (1, io.Action(fvel = 0.05, rvel = -0.2))
elif inp.sonars[2] > 0.5 and inp.sonars[4] < 0.5: #keep turning out of wall(cw)
return (1, io.Action(fvel = 0.05, rvel = 0.2))
elif inp.sonars[2] < 0.5:
return (0, io.Action(fvel = 0.05, rvel = 0.2))
def backward(speed, y): #moving from y=0
while io.SensorInput().odometry.y < y:
print 'Sonar:' + str(io.SensorInput().sonars[5])
if io.SensorInput().sonars[5] == 0.18 or (
io.SensorInput().sonars[5] > 0.18
and io.SensorInput().sonars[5] < 0.25):
io.Action(0, 0).execute()
sleep(0.1)
io.Action(0, 0).execute()
else:
io.Action(-speed, 0).execute()
sleep(0.1)
io.Action(0, 0).execute()
#ebot.wheels(speed, speed)
pass
def getNextValues(self, state, inp):
# print inp.sonars # list
# print inp.odometry.theta
sensor_data = round(inp.sonars[2], 2)
# speed = abs(0.2 * sensor_data - 0.1)
if sensor_data > 1:
output = io.Action(fvel=0.1, rvel=0)
elif sensor_data >= 0.53:
output = io.Action(fvel=0.05, rvel=0)
print '====='
elif 0.47 < sensor_data < 0.53:
output = io.Action(fvel=0, rvel=0)
print "******"
elif sensor_data <= 0.47:
output = io.Action(fvel=-0.05, rvel=0)
print '-----'
return state, output
def getNextValues(self, state, inp):
if state != self.startState:
newS = desiredRight - inp
newO = io.Action(fvel=forwardVelocity,
rvel=300 * (desiredRight - inp) + -272 * state)
return (newS, newO)
else:
newS = desiredRight - inp
return (newS, state)
def getNextValues(self, state, inp):
print 'DynamicMoveToPoint', 'state=', state, 'inp=', inp
assert isinstance(inp,tuple), 'inp should be a tuple'
assert len(inp) == 2, 'inp should be of length 2'
assert isinstance(inp[0],util.Point), 'inp[0] should be a Point'
goal_point, sensors = inp
pose = sensors.odometry
goal_theta = util.fixAngle02Pi(pose.point().angleTo(goal_point))
if not util.nearAngle(goal_theta, pose.theta, 0.04):
print "Goal theta is", goal_theta
print "Pose theta is", pose.theta
if is_between(goal_theta, pose.theta, pose.theta + math.pi):
return (state, io.Action(rvel=0.03, fvel=0.0))
else:
return (state, io.Action(rvel=-0.03, fvel=0.0))
return (state, io.Action(rvel=0.00, fvel=pose.point().distance(goal_point)))
def getNextValues(self, state, inp):
################
# Your code here
################
error = dDesired - inp
output = self.k * error
velocity = output
return state, io.Action(fvel=velocity, rvel=0)
def getNextValues(self, state, inp):
rSensed.append(inp)
k1 = 100
k2 = -97.36
Dist.append(k1 * (desiredRight - rSensed[len(rSensed) - 1]) + k2 *
(desiredRight - rSensed[len(rSensed) - 2]))
return (state, io.Action(fvel=forwardVelocity,
rvel=Dist[len(Dist) - 1]))
# Your code here
pass
def getNextValues(self, state, inp):
# Your code here
k1 = 300
k2 = -271.74
dDesired = 0.5
diff = dDesired - inp
turn = k1 * diff + k2 * state
nextState = diff
return (nextState, io.Action(fvel=0.1, rvel=turn))
pass
def getNextValues(self, state, inp):
#################
k = -25
dDesired = 0.5
nextState = 0
diff = inp - dDesired
turn = (k * diff)
return (nextState, io.Action(fvel=0.1, rvel=turn))
################
pass
def actionToPose(goalPose, robotPose, forwardGain, rotationGain,
maxVel, angleEps, distEps):
"""
Internal procedure that returns an action to take to drive
toward a specified goal pose.
"""
if robotPose.distance(goalPose) < distEps:
finalRotError = util.fixAnglePlusMinusPi(goalPose.theta -robotPose.theta)
return io.Action(rvel = finalRotError * rotationGain)
else:
return actionToPoint(goalPose.point(), robotPose, forwardGain,
rotationGain, maxVel, angleEps)
def getNextValues(self, state, inp):
# print inp.sonars # list
# print inp.odometry.theta
front = round(inp.sonars[2], 2)
upright = round(inp.sonars[3], 2)
right = round(inp.sonars[4], 2)
if state == 'searching':
if front <= 0.3:
nextState = 'left'
output = io.Action(fvel=0, rvel=0)
elif front > 0.3:
nextState = 'searching'
output = io.Action(fvel=0.1, rvel=0)
elif state == 'left':
if front >= 0.3 and upright >= round(
math.sqrt(2) * right + 0.01, 2):
nextState = 'forward'
output = io.Action(fvel=0, rvel=0)
elif front < 0.3 or upright < round(
math.sqrt(2) * right + 0.01, 2):
nextState = 'left'
output = io.Action(fvel=0, rvel=0.1)
elif state == 'forward':
if front <= 0.3:
nextState = 'left'
output = io.Action(fvel=0, rvel=0)
elif upright >= round(math.sqrt(2) * 0.5 + 0.01, 2):
nextState = 'right'
output = io.Action(fvel=0, rvel=0)
else:
nextState = 'forward'
output = io.Action(fvel=0.1, rvel=0)
elif state == 'right':
if upright >= round(math.sqrt(2) * 0.5 + 0.01, 2):
nextState = 'right'
output = io.Action(fvel=0, rvel=-0.1)
elif upright < round(math.sqrt(2) * 0.5 + 0.01, 2):
nextState = 'forward'
output = io.Action(fvel=0, rvel=0)
print nextState, front, right, upright, round(math.sqrt(2) * right, 2)
return nextState, output
class WallFollower(sm.SM):
startState = io.Action(
fvel=0, rvel=0
) # stored in startState is an instance of io.Action at the first time step
def getNextValues(self, state, inp):
if state != self.startState:
newS = desiredRight - inp
newO = io.Action(fvel=forwardVelocity,
rvel=300 * (desiredRight - inp) + -272 * state)
return (newS, newO)
else:
newS = desiredRight - inp
return (newS, state)
