Ejemplo n.º 1
0
def controlLoop(freq=100):
    global pose,sensorData,stopCommand,sharedArray
    while stopCommand.value == 0:
        start = time.time()
        while not commandQueue.empty():
            com = commandQueue.get()
            if(com == ACTIVATE):
                wpNav.activate()
            elif(com == DEACTIVATE):
                wpNav.deactivate()
            elif(com == CLEAR):
                wpNav.clearWaypoints()
            elif(com == FOLLOW_START):
                wf.reset()
            elif(com == RAMP_CHANGE):
                ser.receiveData()
                ser.serCont.send('RAMPCH')
            elif(com == SHOOTER_CHANGE):
                ser.receiveData()
                ser.serCont.send('SHOOTER')
            elif(com == ADD_WPS):
                while not wpQueue.empty():
                    wpNav.addWaypoints(wpQueue.get())
            elif(com == ALIGN_RESET):
                wa.reset()
                
        update()
        sharedArray[:] = [pose[0],pose[1],pose[2]] + sensorData
        time.sleep(max(0,1/float(freq) - (time.time()-start)))
    update(stop=True)
Ejemplo n.º 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.receiveEnc()

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

    #-------------------------Store sensor data
    sensorData = ser.receiveSensors(pose)

    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)

    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)
    ser.motSetAngForVels(forSet,angSet)

    #-------------------------Debug Print
    if debug:
        print data,pose,sensorData,forSet,angSet,'\n'
Ejemplo n.º 3
0
def controlLoop(freq=200):
    global pose,sensorData,stopCommand,sharedArray
    while stopCommand.value == 0:
        start = time.time()
        while not wpQueue.empty():
            wpNav.addWaypoints(wpQueue.get())
        while not commandQueue.empty():
            com = commandQueue.get()
            if(com == ACTIVATE):
                wpNav.activate()
            elif(com == DEACTIVATE):
                wpNav.deactivate()
            elif(com == CLEAR):
                wpNav.clearWaypoints()
            elif(com == FOLLOW_START):
                wf.reset()
        update()
        sharedArray[:] = [pose[0],pose[1],pose[2]] + sensorData
        time.sleep(max(0,1/float(freq) - (time.time()-start)))
    update(stop=True)
Ejemplo n.º 4
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