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)