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(): 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() (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)