def step():
    #io.sonarMonitor(True)
    
    s = io.getSonars()
    x,y,theta = io.getPosition()
    robot.slimeX.append(x)
    robot.slimeY.append(y)
Exemplo n.º 2
0
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
Exemplo n.º 3
0
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)
Exemplo n.º 4
0
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)
Exemplo n.º 7
0
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)
Exemplo n.º 8
0
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)
Exemplo n.º 9
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)
Exemplo n.º 10
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)
Exemplo n.º 11
0
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) 
Exemplo n.º 12
0
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)
Exemplo n.º 13
0
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)
Exemplo n.º 14
0
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)
Exemplo n.º 15
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])
Exemplo n.º 16
0
def step():
    distance = distanceFromVoltage(io.getAnalogInputs()[0])
    print io.getSonars()[3], distance
    io.setForward((distance-0.5)*2) #replace with your code
Exemplo n.º 17
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)