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]))