def stepPlotting(self):
if self.drawSlimeTrail:
odo = io.SensorInput(self.drawSlimeTrail == 'Cheat').odometry
self.slimeData.append(odo.xytTuple())
self.plotData['clocktime'].append(time.time())
self.plotData['step'].append(self.step)
self.step += 1
inp = io.SensorInput()
for j in self.plotJobs:
if j.xfunc: self.plotData[j.xname].append(j.callFunc(j.xfunc, inp))
if j.yfunc: self.plotData[j.yname].append(j.callFunc(j.yfunc, inp))
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 step_plotting(self):
if self.draw_slime_trail:
odo = io.SensorInput(self.draw_slime_trail == 'Cheat').odometry
self.slime_data.append(odo.xyt_tuple())
self.plot_data['clocktime'].append(time.time())
self.plot_data['step'].append(self.step)
self.step += 1
inp = io.SensorInput()
for j in self.plot_jobs:
if j.xfunc:
self.plot_data[j.xname].append(j.call_func(j.xfunc, inp))
if j.yfunc:
self.plot_data[j.yname].append(j.call_func(j.yfunc, inp))
def gotolot4():
print 'Now moving forward...'
forward(0.1, 0.555)
print(io.SensorInput().odometry)
print 'turning clockwise...'
#rotate_cw(1, 10)
rotate_cw(0.1, 3 * math.pi / 2.0 + 0.275)
print io.SensorInput().odometry.theta
print 'moving backwards...'
backward(0.1, 0.155)
print io.SensorInput().odometry
firebase.put('/', 'done', 'done')
#firebase.put('/', 'currentlot',4)
pass
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 setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
startTheta = io.SensorInput().odometry.theta
# static plot of robot angle (from start) vs time
robot.gfx.addStaticPlotFunction(
y=('angle', lambda inp: util.fixAnglePlusMinusPi(
(inp.odometry.theta-startTheta))))
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 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 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 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 setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
startTheta = io.SensorInput().odometry.theta
# static plot of robot angle vs eye voltage
robot.gfx.addStaticPlotFunction(x=('angle',
lambda inp: util.fixAnglePlusMinusPi(
(inp.odometry.theta - startTheta))),
y=('eye voltage',
lambda inp: inp.analogInputs[1]))
def step():
inp = io.SensorInput(cheat=True)
# Compute next action (will draw belief state)
act = robot.behavior.step(inp)
# Draw true state
trueState = util.clip(int(round(numStates * (inp.odometry.x - xMin) /\
(xMax - xMin))),
0, numStates-1)
robot.beliefW.drawRect((trueState + 0.3, 0.3), (trueState + 0.7, 0.7),
'gold')
# Execute action
act.execute()
def step(self):
# we *could* store the data anyway, but I'm inclined to think that's
# a waste of resources.
if not self.window:
return
# also, if we have no probes, don't plot anything
if len(self.probes) == 0:
return
inp = io.SensorInput()
for p in self.probes:
if p:
p.step(inp)
self.setExtremes(p.extremes())
self.draw()
def setup():
robot.gfx = gfx.RobotGraphics(drawSlimeTrail=False)
startTheta = io.SensorInput().odometry.theta
# static plot of robot angle vs left eye, right eye and difference
# assumes that left is analog input #2, right is analog input #3
robot.gfx.addStaticPlotFunction(
x=('angle', lambda inp: util.fixAnglePlusMinusPi(
(inp.odometry.theta-startTheta))),
y=('left', lambda inp: inp.analogInputs[1]))
robot.gfx.addStaticPlotFunction(
x=('angle', lambda inp: util.fixAnglePlusMinusPi(
(inp.odometry.theta-startTheta))),
y=('right', lambda inp: inp.analogInputs[2]))
robot.gfx.addStaticPlotFunction(
x=('angle', lambda inp: util.fixAnglePlusMinusPi(
(inp.odometry.theta-startTheta))),
y=('diff', lambda inp: inp.analogInputs[1] - \
inp.analogInputs[2]))
def getNextValues(self, state, inp):
inp = io.SensorInput()
i0 = inp.sonars[0]
i1 = inp.sonars[1]
i2 = inp.sonars[2]
i3 = inp.sonars[3]
i4 = inp.sonars[4]
i5 = inp.sonars[5]
i6 = inp.sonars[6]
i7 = inp.sonars[7]
if self.state == 'DRIVE': # start position
if i4 > 0.5 and i5 > 0.5:
return (state, io.Action(fvel=0.3, rvel=0))
else:
if i7 > 0.35:
return (state, io.Action(fvel=0.03, rvel=0.27))
else:
return ('F', io.Action(fvel=0.03, rvel=0.27))
if self.state == 'F': # follow wall
if 0.30 < i7 and i7 < 0.31:
if i4 > 0.5 and i5 > 0.5:
return ('F', io.Action(fvel=0.3, rvel=0))
elif i5 > 0.5 and i6 > 0.4:
return ('F', io.Action(fvel=0.03, rvel=-0.27))
else:
return ('F', io.Action(fvel=0.03, rvel=0.27))
elif i7 < 0.3:
return ('F', io.Action(fvel=0.03, rvel=0.27))
elif i7 > 0.3 and (i4 < 0.35 or i5 < 0.35):
return ('F', io.Action(fvel=0.03, rvel=0.27))
else:
return ('F', io.Action(fvel=0.12, rvel=-0.27))
def step():
# no_commands = True
#
# while no_commands == True:
# # Check the value of movement_list in the database at an interval of 0.5
# # seconds. Continue checking as long as the movement_list is not in the
# # database (ie. it is None). If movement_list is a valid list, the program
# # exits the while loop and controls the eBot to perform the movements
# # specified in the movement_list in sequential order. Each movement in the
# # list lasts exactly 1 second.
#
# # Get movement list from Firebase
# movement_list = firebase.get('/carpark')
# if movement_list!= '0':
# no_commands = False
# else:
# no_commands = True
# sleep(0.5)
inp = firebase.get('/carpark')
#inp=4
print io.SensorInput().odometry.theta
robot.behavior.step(inp)
io.done(robot.behavior.isDone())
def plotSonar(sonarNum):
robot.gfx.addDynamicPlotFunction(
y=('sonar' + str(sonarNum), lambda: io.SensorInput().sonars[sonarNum]))
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()
def on_step():
global inp
robot.count += 1
inp = io.SensorInput(cheat=True)
# discretize sonar readings
# each element in discrete_sonars 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
discrete_sonars = []
for (sonar_pose, distance) in zip(sonarDist.sonar_poses,inp.sonars):
if NOISE_ON:
distance = noiseModel.noisify(distance, grid_square_size)
if distance >= 5:
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(1, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
continue
sensor_indices = robot.map.point_to_indices(inp.odometry.transformPose(sonar_pose).point())
hit_indices = robot.map.point_to_indices(sonarDist.sonar_hit(distance, sonar_pose, inp.odometry))
ray = util.line_indices(sensor_indices, hit_indices)
discrete_sonars.append((distance, ray))
# figure out where robot is
start_point = inp.odometry.point()
startCell = robot.map.point_to_indices(start_point)
def plan_wont_work():
for point in robot.plan:
if not robot.map.is_passable(point):
return True
return False
# if necessary, make new plan
if robot.plan is None or plan_wont_work(): #discrete_sonars[3][0] < 0.3: ###### or if the robot is about to crash into a wall
print('REPLANNING')
robot.plan = search.uniform_cost_search(robot.successors,
robot.map.point_to_indices(inp.odometry.point()),
lambda x: x == robot.goalIndices,
lambda x: 0)
# make the path less jittery
#print(robot.plan)
to_remove = []
if robot.plan is not None:
for i in range(len(robot.plan)-2):
line = util.LineSeg(util.Point(robot.plan[i][0], robot.plan[i][1]), util.Point(robot.plan[i+1][0], robot.plan[i+1][1]))
if line.dist_to_point(util.Point(robot.plan[i+2][0], robot.plan[i+2][1])) < 0.1:
to_remove.append(robot.plan[i+1])
print(to_remove)
for i in to_remove:
robot.plan.remove(i)
#print(robot.plan)
#print()
# graphics (draw robot's plan, robot's location, goal_point)
# do not change this block
for w in robot.windows:
w.redraw_world()
robot.windows[-1].mark_cells(robot.plan,'blue')
robot.windows[-1].mark_cell(robot.map.point_to_indices(inp.odometry.point()),'gold')
robot.windows[-1].mark_cell(robot.map.point_to_indices(goal_point),'green')
# update map
for (d,cells) in discrete_sonars:
#print(cells)
if d < 5:
robot.map.sonar_hit(cells[-1])
for i in range(len(cells)-1):
robot.map.sonar_pass(cells[i])
# if we are within 0.1m of the goal point, we are done!
if start_point.distance(goal_point) <= 0.1:
io.Action(fvel=0,rvel=0).execute()
code = checkoff.generate_code(globals())
if isinstance(code, bytes):
code = code.decode()
raise Exception('Goal Reached!\n\n%s' % code)
# otherwise, figure out where to go, and drive there
destination_point = robot.map.indices_to_point(robot.plan[0])
while start_point.isNear(destination_point,0.1) and len(robot.plan)>1:
robot.plan.pop(0)
destination_point = robot.map.indices_to_point(robot.plan[0])
currentAngle = inp.odometry.theta
angleDiff = start_point.angleTo(destination_point)-currentAngle
angleDiff = util.fix_angle_plus_minus_pi(angleDiff)
fv = rv = 0
if start_point.distance(destination_point) > 0.1:
if abs(angleDiff) > 0.1:
rv = 4*angleDiff
else:
fv = 4*start_point.distance(destination_point)
io.set_forward(fv)
io.set_rotational(rv)
# render the drawing windows
# do not change this block
for w in robot.windows:
w.render()
def step():
inp = io.SensorInput()
(neck, left, right) = inp.analogInputs[0:3]
print 'neck=', neck, 'left=', left, 'right=', right
robot.behavior.step(inp).execute()
robot.data.append(left-right)
def step():
inp = io.SensorInput()
robot.behavior.step(inp).execute()
io.done(robot.behavior.isDone())
def step():
inp = io.SensorInput()
(left, right) = inp.analogInputs[1:3]
print 'left=', left, 'right=', right, 'diff=', left - right
robot.behavior.step(inp).execute()
def step():
inp = io.SensorInput()
#print inp.sonars[3]
#print "%8.2f,%8.2f,%8.2f" %(inp.sonars[2],inp.sonars[3],inp.sonars[4])
robot.behavior.step(inp).execute()
io.done(robot.behavior.isDone())
def step():
robot.count += 1
inp = io.SensorInput(cheat=False)
robot.behavior.step(inp).execute()
io.done(inp.odometry.point().isNear(goalPoint, gridSquareSize))
def step():
robot.behavior.step(io.SensorInput()).execute()
def step():
inp = io.SensorInput()
print inp.sonars[1],
robot.behavior.step(inp).execute()
io.done(robot.behavior.isDone())
def plotDist():
func = lambda: sonarDist.getDistanceRight(io.SensorInput().sonars)
robot.gfx.addStaticPlotFunction(y=('d_o', func))
def step():
inp = io.SensorInput()
robot.behavior.step(inp).execute()
def step():
robot.behavior.step(io.SensorInput(cheat=True)).execute()
io.done(robot.behavior.isDone())
def plot_sonar(sonarNum):
robot.gfx.add_dynamic_plot_function(
y=('sonar' + str(sonarNum), lambda: io.SensorInput().sonars[sonarNum]))
