Beispiel #1
0
def update(stop=False):
    global pose, sensorPoints

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

    """
    #-------------------------Update Sensor Values

    #sensorPoints = sensor.update(data[2:7],pose)
    #print sensorPoints
    
    if waypointNav.state == waypointNav.TRANSLATING:
        for i in sensorPoints[1:6]:
            if ((pose[0]-i[0])**2 + (pose[1]-i[1])**2) < 9.0**2:
                waypointNav.wp = []
                waypointNav.addWaypoint([pose[0],pose[1],pose[2],False])
    """
    # ------------------------- Update Waypoint Navigator
    [forSet, angSet] = waypointNav.update(pose)

    if abs(forSet) > 3:
        forSet = 3 * abs(forSet) / forSet
    if abs(angSet) > 3:
        angSet = 3 * abs(angSet) / angSet
    if waypointNav.active:
        mot.setAngForVels(forSet, angSet)

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

    # print "x = "+str(pose[0])+", y = "+str(pose[1])+", theta = "+str(math.degrees(pose[2]))

    if stop:
        ser.serCont.send("STOPS")
    else:
        s = mot.getMotorCommandBytes(lCommand, rCommand)
        ser.sendCommand(s)
Beispiel #2
0
def update(stop = False, rotate=False):
    global pose,sensorPoints

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

    #print data, pose
    #-------------------------Update Sensor Values

    sensorPoints = sensor.update(data[2:7],pose)

    #-------------------------Debug Print

    [forSet,angSet] = waypointNav.update(pose)
    if abs(angSet) > 3.5:
        angSet = 3.5*abs(angSet)/angSet
    if abs(forSet) > 3.5:
        forSet = 3.5*abs(forSet)/forSet

    if rotate:
        mot.setAngForVels(0,3.5)
    else:
        mot.setAngForVels(forSet,angSet)
    #mot.setAngForVels(.5,0)

    [dThetaLdt,dThetaRdt] = odo.getVel()
    [lCommand,rCommand] = mot.update(dThetaLdt,dThetaRdt)

    if debug:
        print pose,sensorPoints

    if(stop):
        ser.sendCommand(mot.getMotorCommandBytes(0,0))
    else:
        ser.sendCommand(mot.getMotorCommandBytes(lCommand,rCommand))