def update(sensorPoints,pose): global state global desiredDist global timeoutStart,lostTimeout if state == FIND: for i,s in enumerate(sensorPoints[:-1]): if s[2] and distance(pose,s) <= desiredDist: wpNav.initialize() wpNav.clearWaypoints() wpNav.addWaypoint([pose[0],pose[1],pose[2]+(sen.angArray[i]-sen.angArray[-1]),True]) print "Aligning with the wall (using waypoint navigator)" state=ORIENT return wpNav.update(pose) return [6.0,0.0] elif state == ORIENT: if wpNav.state != wpNav.IDLE: return wpNav.update(pose) else: distAngTroller.setDesired(desiredDist,reset=True)#distance(sensorPoints[0],pose),reset=True) print "Found and aligned with wall, following now." state = FOLLOW return [0,0] elif state == FOLLOW: sp = sensorPoints if not sp[-1][2] and not sp[-2][2]: print "Lost wall, trying to find it again." timeoutStart = time.time() state = LOST_TIMEOUT return [5.0,-2.0] elif not sp[-2][2]: dist = distance(sp[-1],pose) elif not sp[-1][2]: dist = distance(sp[-2],pose) else: ax,ay = sp[-2][0]-sp[-1][0],sp[-2][1]-sp[-1][1] bx,by = pose[0]-sp[-1][0],pose[1]-sp[-1][1] x = distance(pose,sp[-1]) dist = x*math.sqrt(1- (ax*bx + ay*by)/(distance(sp[-1],sp[-2])*x)) return [5.0,distAngTroller.update(dist)] elif state == LOST_TIMEOUT: if sensorPoints[-1][2] or sensorPoints[-2][2]: print "Found wall again, back to following it." distAngTroller.setDesired(desiredDist,reset=True) state = FOLLOW return [0,0] elif (time.time()-timeoutStart) > lostTimeout: state=FIND return [5.0,-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'
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