示例#1
0
文件: rotate.py 项目: teamhex/teamhex
def update():
    global pose

    #-------------------------Receive Data from Arduino
    # data is [Left Encoder, Right Encoder]
    data = ser.receive()
    #print data
    #-------------------------Update Odometry
    pose = odo.update(data[0],data[1])

    #    if((pose != odo.pose) and debug):
    if debug:
        print "x = "+str(pose[0])+", y = "+str(pose[1])+", theta = "+str(math.degrees(pose[2]))

    #-------------------------Update Waypoint Navigator

    #    [forSet,angSet] = waypointNav.update(pose)
    #mot.setAngForVels(forSet,angSet)

    #-------------------------Update Motor Controller
    [dThetaLdt, dThetaRdt] = odo.getVel()

    [lCommand, rCommand] = mot.update(dThetaLdt, dThetaRdt)
    #print lCommand,rCommand
    ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))
示例#2
0
def update(stop = False):
    global pose,sensorData,updateLock,CONTROLLER,basicFor,basicAng

    #-------------------------Receive Data from Arduino
    # data is [Left Encoder, Right Encoder,
    #          Sensor 0 (leftmost), Sensor 1, Sensor 2 (center, forward),
    #          Sensor 3, Sensor 4 (rightmost)]
    data = ser.receiveData()

    #-------------------------Update Odometry
    pose = odo.update(data[0],data[1])

    #-------------------------Store sensor data
    sensorData = list(data[2:8])

    sp = getSensorPoints()

    for s in sp[1:4]:
        if distance(pose,s) < ROBOT_RADIUS+3:
            if(wpNav.state == wpNav.TRANSLATING):
                clearWayPoints()
            basicFor.value = 0.0
            if wf.state == wf.FOLLOW:
                wf.reset()

    #-------------------------Get forward and angular velocities
    # Can get them from either a basic (forward,angular) velocity
    # controller, or from a waypoint navigator.
    # Synchronized through locking because modifications will only affect this part
    if(CONTROLLER.value == WAYPOINT):
        [forSet,angSet] = wpNav.update(pose)
        if len(wpNav.wp) == 0:
            inWait.value = 1
        else:
            inWait.value = 0
    elif(CONTROLLER.value == BASIC):
        inWait.value = 1
        [forSet,angSet] = basicFor.value,basicAng.value
    elif(CONTROLLER.value == WALL_FOLLOW):
        [forSet,angSet] = wf.update(sp,pose)
    elif(CONTROLLER.value == WALL_ALIGN):
        [forSet,angSet] = wa.update([distance(pose,s) for s in sp])
        if wa.isDone():
            inWait.value = 1
        else:
            inWait.value = 0

    if forSet == 0:
        forMove.value = 0
    else:
        forMove.value = 1
    
    #-------------------------Limit speeds according to empirical results
    if abs(forSet) > MAX_FOR_SPEED:
        forSet = math.copysign(MAX_FOR_SPEED,forSet)
    if abs(angSet) > MAX_ANG_SPEED:
        angSet = math.copysign(MAX_ANG_SPEED,angSet)
    mot.setAngForVels(forSet,angSet)

    #-------------------------Use motor controller to get motor commands
    [dThetaLdt,dThetaRdt] = odo.getVel()
    [lCommand,rCommand] = mot.update(dThetaLdt,dThetaRdt)

    #-------------------------Send commands (send STOPS command if update is told to stop)
    if(stop):
        ser.serCont.send('STOPS')
    else:
        ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))

    #-------------------------Debug Print
    if debug:
        print data,pose,sensorData,forSet,angSet,lCommand,rCommand