Example #1
0
 def send_message(self, message = '', ident = 99):
     if ident == 99:
         self.message_id += 1
         self.message_id %= 10
         ident = self.message_id
     tstr = str(ident) + ";" + message
     dbg_print(tstr, 6, self.identitaet)
     self.brick.message_write(self.outbox, tstr)
Example #2
0
 def message_read(self, inbox1, inbox2, leeren):
     if random.choice([0,1])==0:
         msg = self.liste.pop(0)
     else:
         msg = "m;"+str(self.ident)+";h,300"
         self.ident+=1
         self.ident%=10
         self.ident+=10
     dbg_print("pseudoBrick.message_read() - "+str(msg),7)
     time.sleep(1)
     return (11, msg)
Example #3
0
 def message_write(self, outbox, message):
     dbg_print("pseudoBrick.message_write(): "+message,7)
     time.sleep(0.1)
     typ, ident, _ = message.split(';')
     if typ == 'm':
         self.liste.append('r;'+str(ident)+';resp')
     elif typ == 'r':
         self.liste.append('a;'+str(ident)+';ack')
     elif typ == 'a':
         pass
     else:
         dbg_print("pseudoBrick.message_write() - parse-error")
Example #4
0
 def go_to_point(self, x, y):
     while(True):
         self.blockiert_lock.acquire()
         if self.blockiert:
             self.blockiert_lock.release()
             time.sleep(0.2)
             continue
         self.blockiert = True
         self.blockiert_lock.release()
         break
     self.go_back(15)
     self.position_lock.acquire()
     ausrichtung = self.ausrichtung
     vektor = berechneVektor(self.position, {'x': x, 'y': y})
     dbg_print("go_to_point: (%d,%d) von (%d,%d) Vektor=(entf:%f, winkel:%f)" % (x,y,self.position['x'],self.position['y'],vektor['entfernung'],vektor['winkel']), 1, self.identitaet)
     self.position_lock.release()
     while(True):
         self.blockiert_lock.acquire()
         if self.blockiert:
             self.blockiert_lock.release()
             time.sleep(0.2)
             continue
         self.blockiert = True
         self.blockiert_lock.release()
         break
     if ((((ausrichtung - vektor['winkel'] )%360)+360)%360) > 180: 
         self.turnleft(int(round(vektor['winkel'])-90))
     else:
         self.turnright(int(450-round(vektor['winkel']))%360)
     while(True):
         self.blockiert_lock.acquire()
         if self.blockiert:
             self.blockiert_lock.release()
             time.sleep(0.2)
             continue
         self.blockiert = True
         self.blockiert_lock.release()
         break
     self.go_forward(vektor['entfernung'])
     while(True):
         self.blockiert_lock.acquire()
         if self.blockiert:
             self.blockiert_lock.release()
             time.sleep(0.2)
             continue
         self.blockiert_lock.release()
         break
     self.status_lock.acquire()
     if self.status == 0:
         self.status_lock.release()
         dbg_print('go_to_point(): true', 1, self.identitaet)
         return True
     elif self.status == 1:
         self.status_lock.release()
         dbg_print('go_to_point(): false', 1, self.identitaet)
         return False
     else:
         self.status_lock.release()
         dbg_print('go_to_point(): else', 1, self.identitaet)
         return False
Example #5
0
def berechneVektor(standort = {'x':0.0, 'y':0.0}, ziel = {'x': 0.0, 'y': 0.0}):
    relativ_ziel = {'x': abs(ziel['x'] - standort['x']), 'y': abs(ziel['y'] - standort['y'])}
    dbg_print("StO: (%d,%d), Z: (%d,%d), rZ: (%d,%d)"%(standort['x'],
                                                       standort['y'],
                                                       ziel['x'], 
                                                       ziel['y'], 
                                                       relativ_ziel['x'], 
                                                       relativ_ziel['y']), 1, 0)
    entfernung = math.sqrt(relativ_ziel['x'] ** 2 + relativ_ziel['y'] ** 2)
    if relativ_ziel['x'] == 0:
        winkel = 90
    elif relativ_ziel['x'] < 0:
        winkel = math.atan(float(relativ_ziel['y']) / float(relativ_ziel['x'])) * (180.0 / math.pi) + 180
    else:
        winkel = math.atan(float(relativ_ziel['y']) / float(relativ_ziel['x'])) * (180.0 / math.pi) + 360
    return {'winkel': winkel % 360, 'entfernung': entfernung, 'rel_x': relativ_ziel['x'], 'rel_y': relativ_ziel['y']}
Example #6
0
 def go_to_point(self, handle, x_axis , y_axis):
     dbg_print('Going to Point (' + str(x_axis) + ',' + str(y_axis) + ')', 2, self.factory.robots[handle].identitaet)
     reached = self.factory.robots[handle].go_to_point(x_axis, y_axis)#FIXME: sollte blockieren
     if reached == True:
         self.factory.robots[handle].position_lock.acquire()
         self.callRemote(command.ArrivedPoint,
                         handle = self.factory.robots[handle].handle, 
                         x_axis = self.factory.robots[handle].position['x'],
                         y_axis = self.factory.robots[handle].position['y'])
         self.factory.robots[handle].position_lock.release()
         return {'ack': 'got point'}
     else: 
         self.factory.robots[handle].position_lock.acquire()
         self.callRemote(command.ArrivedPoint,
                         handle = self.factory.robots[handle].handle,
                         x_axis = self.factory.robots[handle].position['x'],
                         y_axis = self.factory.robots[handle].position['y'])
         self.factory.robots[handle].position_lock.release()
         return {'ack': 'Punkt nicht erreicht'}
Example #7
0
    def exploration_simple(self):
        state = 0
        while(True):
            self.abbruch_lock.acquire()
            if self.abbruch:
                self.abbruch_lock.release()
                break
            self.abbruch_lock.release()
            self.blockiert_lock.acquire()
            if not self.blockiert:
                self.blockiert = True
                self.blockiert_lock.relSease()
                dbg_print("exploration_simple - wahl=%d" % state,1, self.identitaet)

                if state == 0:
                    self.go_forward(0)
                elif state == 1:
                    self.go_back(15)
                elif state == 2:
                    linksrechts = random.choice([0, 1]) #0=links || 1=rechts
                    grad = random.randint(30, 160) # 30 - 160
                    self.sensor_lock.acquire()
                    if self.sensor == 0 or self.sensor == 1:
                        if linksrechts == 0:
                            self.turnleft(grad)
                        else:
                            self.turnright(grad)
                    elif self.sensor == 3:
                        self.turnright(grad)
                    elif self.sensor == 2:
                        self.turnleft(grad)
                    else:
                        dbg_print("Sensorfehler sensor = %d"%self.sensor, 1, self.identitaet)
                        if linksrechts == 0:
                            self.turnleft(grad)
                        else:
                            self.turnright(grad)
                    self.sensor = -1
                    self.sensor_lock.release()
                state += 1
                state %= 3
            else:
                self.blockiert_lock.release()
Example #8
0
 def dispatch(self):
     dbg_print("run dispatch", 2, self.identitaet)
     count = 0
     while(True):
         if count % 100000 == 0:
             dbg_print("dispatch() - #" + str(count), 3, self.identitaet)
         try:
             _, message = self.recv_message()
             dbg_print("message: " + str(message), 9, self.identitaet)
             try:
                 t_id, payload = str(message).split(';')
                 payload = payload.strip("\x00")
             except:
                 dbg_print("message-parsing-error: falsches Format", self.identitaet)
             dbg_print("ident=" + str(t_id) + " msg=" + str(payload), 4, self.identitaet)
             csv = payload.split(',') 
             dbg_print("payload %s,%s"%(csv[0], csv[1]), 1, self.identitaet)
             if int(csv[0]) == 1: #nach Zeitintervall 500ms update_position (Entfernung)
                 dbg_print("Update: " + str(csv[1]) + " Einheiten gefahren",1, self.identitaet)
                 self.position_lock.acquire()
                 tmpPosition = berechnePunkt(self.ausrichtung, int(csv[1]), self.position)
                 tmpYaw = self.ausrichtung
                 self.position_lock.release()
                 self.protokoll.callRemote(command.SendData, handle=self.handle, point_tag=map.POINT_FREE, x_axis=tmpPosition["x"], y_axis=tmpPosition["y"], yaw=tmpYaw)
             elif int(csv[0]) == 2: #kollision update_position (Entfernung)
                 self.status_lock.acquire()
                 self.status = 1 # hit
                 self.status_lock.release()
                 point = map.POINT_DODGE_CENTER
                 self.payload_lock.acquire()
                 if self.payload == -9:
                     point = map.POINT_TARGET
                     tmp = berechnePunkt(self.ausrichtung,int(csv[1]),self.position)
                     dbg_print("Ziel gefunden bei (%d,%d)"% (tmp['x'],tmp['y']), 1, self.identitaet)
                     self.payload = -1
                 else:
                     self.sensor_lock.acquire()
                     self.sensor = int(csv[2])
                     if self.sensor == 2:
                         point = map.POINT_DODGE_LEFT
                     elif self.sensor == 3:
                         point = map.POINT_DODGE_RIGHT
                     self.sensor_lock.release()
                 self.payload_lock.release()
                 dbg_print("Kollision/Ziel: " + str(csv[1]) + " Einheiten gefahren",1, self.identitaet)
                 
                 self.position_lock.acquire()
                 self.position = berechnePunkt(self.ausrichtung,int(csv[1]),self.position)
                 self.protokoll.callRemote(command.SendData, handle=self.handle, point_tag=point, x_axis=self.position["x"], y_axis=self.position["y"], yaw=self.ausrichtung)
                 self.position_lock.release()
                 dbg_print(str(self.position),2, self.identitaet)
                 self.blockiert_lock.acquire()
                 self.blockiert = False
                 self.blockiert_lock.release()
             elif int(csv[0]) == 3: #strecke ohne vorkommnisse abgefahren
                 self.status_lock.acquire()
                 self.status = 0 #arrived
                 dbg_print("status: arrived", 1, self.identitaet)
                 self.status_lock.release()
                 dbg_print(str(csv[1]) + " Einheiten gefahren",1, self.identitaet)
                 self.position_lock.acquire()
                 self.position = berechnePunkt(self.ausrichtung,int(csv[1]),self.position)
                 self.position_lock.release()
                 self.blockiert_lock.acquire()
                 self.blockiert = False
                 self.blockiert_lock.release()
             elif int(csv[0]) == 4: #beendet rueckwaerts und drehen
                 dbg_print("Drehen oder Zurueck",1, self.identitaet)
                 self.blockiert_lock.acquire()
                 self.blockiert = False
                 self.blockiert_lock.release()
             elif int(csv[0]) == 5:
                 self.payload_lock.acquire()
                 self.payload = int(csv[1])
                 dbg_print("US-gemessen "+str(self.payload),1,self.identitaet)
                 self.payload_lock.release()
                 self.blockiert_lock.acquire()
                 self.blockiert = False
                 self.blockiert_lock.release()
             elif int(csv[0]) == 6: #intervall anstossen
                 pass
             elif int(csv[0]) == 9: #ziel gefunden gleich kommt 2
                 dbg_print("Ziel gefunden", self.identitaet)
                 self.payload_lock.acquire()
                 self.payload = -9
                 self.payload_lock.release()
             else:
                 dbg_print("csv konnt nicht geparst werden", self.identitaet)
         except Exception as err:
             if DEBUGLEVEL>9:
                 print err
                 
         count += 1
Example #9
0
 def recv_message(self):
     t = self.brick.message_read(self.inbox, self.inbox, True)
     dbg_print(t, 6, self.identitaet)
     return t
Example #10
0
 def exploration_radar(self):
     step = 20
     forward = 10
     state = 1
     direction = 1
     first_mesurement = 0
     second_mesurement = 0 
     while(True):
         self.abbruch_lock.acquire()
         if self.abbruch:
             self.abbruch_lock.release()
             break
         self.abbruch_lock.release()
         self.blockiert_lock.acquire()
         if not self.blockiert:
             self.blockiert = True
             self.blockiert_lock.release()
             dbg_print("exploration_radar - wahl=%d" % state, 1, self.identitaet)
             if state == 0:
                 if direction < 2:
                     self.turnright(90)
                 else:
                     self.turnleft(90)
                 direction += 1
                 direction %= 4
             elif state == 1:
                 self.scan_ultrasonic()
                 while True:
                     self.payload_lock.acquire()
                     if self.payload > -1:
                         first_mesurement = self.payload
                         dbg_print("first_mesurement: "+str(first_mesurement), 1, self.identitaet)
                         self.payload = -1
                         self.payload_lock.release() 
                         break
                     self.payload_lock.release()
                     time.sleep(0.5)
                 
                 if first_mesurement < 1.5*forward:
                     state = 0 #-> drehen
                     first_mesurement = 0
                     continue
             elif state == 2:
                 self.go_forward(forward)
             elif state == 3:
                 if first_mesurement < 255:
                     self.scan_ultrasonic()
                     while True:
                         self.payload_lock.acquire()
                         if self.payload > -1:
                             second_mesurement = self.payload
                             dbg_print("sec_mesurement: "+str(second_mesurement), 1, self.identitaet)
                             self.payload = -1
                             self.payload_lock.release() 
                             break
 
                         self.payload_lock.release() 
                         time.sleep(0.5)
                     
                     if second_mesurement < first_mesurement:
                         self.calibrationFactor = float(forward)/(first_mesurement - second_mesurement)
                         dbg_print("calibrationFactor: %.2f" % self.calibrationFactor, 1, self.identitaet)
                 else:
                     self.blockiert_lock.acquire()
                     self.blockiert = False
                     self.blockiert_lock.release()
             elif state == 4:
                 first_mesurement = 0
                 second_mesurement = 0
                 if direction < 2:
                     self.turnright(90)
                 else:
                     self.turnleft(90)
                 direction += 1
                 direction %= 4
             elif state == 5:
                 self.scan_ultrasonic()
                 while True:
                     self.payload_lock.acquire()
                     if self.payload > -1:
                         first_mesurement = self.payload
                         dbg_print("first_mesurement: "+str(first_mesurement), 1, self.identitaet)
                         self.payload = -1
                         self.payload_lock.release() 
                         break
                     self.payload_lock.release() 
                     time.sleep(0.5)
                     
                 if first_mesurement < 1.5*step:
                     state = 0# -> drehen
                     first_mesurement = 0
                     continue
             elif state == 6:
                 self.go_forward(step)
                 step += 20
             elif state == 7:
                 if first_mesurement < 255:
                     self.scan_ultrasonic()
                     while True:
                         self.payload_lock.acquire()
                         if self.payload > -1:
                             second_mesurement = self.payload
                             dbg_print("second_mesurement: "+str(second_mesurement), 1, self.identitaet)
                             self.payload = -1
                             self.payload_lock.release() 
                             break
                         self.payload_lock.release() 
                         time.sleep(0.5)
                         
                     if second_mesurement < first_mesurement:
                         self.calibrationFactor = float(step)/(first_mesurement - second_mesurement)
                         dbg_print("calibrationFactor: %.2f" % self.calibrationFactor, 1, self.identitaet)
                 else:
                     self.blockiert_lock.acquire()
                     self.blockiert = False
                     self.blockiert_lock.release()
             state += 1
             state %= 8
             step %= 200
         else:
             self.blockiert_lock.release()
Example #11
0
    def exploration_circle(self):
        step = 10
        state = 0
        first_mesurement = 0
        second_mesurement = 0 
        while(True):
            self.abbruch_lock.acquire()
            if self.abbruch:
                self.abbruch_lock.release()
                break
            self.abbruch_lock.release()
            self.blockiert_lock.acquire()
            if not self.blockiert:
                self.blockiert = True
                self.blockiert_lock.release()
                dbg_print("exploration_circle - wahl=%d" % state, 1, self.identitaet)

                if state == 0:
                    self.scan_ultrasonic()
                    while True:
                        self.payload_lock.acquire()
                        if self.payload > -1:
                            first_mesurement = self.payload
                            dbg_print("first_mesurement: "+str(first_mesurement), 1, self.identitaet)
                            self.payload = -1
                            self.payload_lock.release() 
                            break
                        self.payload_lock.release()
                        time.sleep(0.5)
                    
                    if first_mesurement < 1.5*step:
                        first_mesurement = 0
                        state = 3 #-> drehen
                        continue
                elif state == 1:
                    self.go_forward(step)
                elif state == 2:
                    if first_mesurement < 255:
                        self.scan_ultrasonic()
                        while True:
                            self.payload_lock.acquire()
                            if self.payload > -1:
                                second_mesurement = self.payload
                                dbg_print("second_mesurement: "+str(second_mesurement), 1, self.identitaet)
                                self.payload = -1
                                self.payload_lock.release() 
                                break
                            self.payload_lock.release() 
                            time.sleep(0.5)
                            
                        if second_mesurement < first_mesurement:
                            self.calibrationFactor = float(step)/(first_mesurement - second_mesurement)
                            dbg_print("calibrationFactor: %.2f" % self.calibrationFactor, 1, self.identitaet)
                    else:
                        self.blockiert_lock.acquire()
                        self.blockiert = False
                        self.blockiert_lock.release()
                    step += 10
                elif state == 3:
                    first_mesurement = 0
                    second_mesurement = 0
                    self.turnleft(90)
                state += 1
                state %= 4
                step %= 200
            else:
                self.blockiert_lock.release()
Example #12
0
                                               color = self.factory.robots[bot].color, rhandle = bot)
                deffered.addCallback(self.activate)
                deffered.addErrback(self.failure)

    def activate(self, handle):
        self.factory.robots[handle['rhandle']].handle = handle['handle']
        print "handle=%s" % (self.factory.robots[handle['rhandle']].handle)
        deffered = self.protocol.callRemote(command.Activate, handle = self.factory.robots[handle['rhandle']].handle)
        deffered.addCallback(self.activated)
        deffered.addErrback(self.failure)

    def activated(self, ACK):
        print 'active'
        self.active = True

    def failure(self, error):
        print 'Error: %s:%s\n%s' % (self.host, self.port, error)

if __name__ == '__main__' and DEBUGLEVEL > 0:
    if len(sys.argv) < 3:
        print >> sys.stderr, "Usage: python explorer.py [IP MCC] [ANZAHL DER NXT]"
        exit(1)
    dbg_print("__main__ start")
    test = NXTClient(sys.argv[1], int(sys.argv[2]))
    try:
        s = raw_input('--> ')
    except:
        pass    
    test.factory.robots = []
    dbg_print("__main__ fertig")