Beispiel #1
0
 def __init__(self):
     QThread.__init__(self)
     self.code = ""
     self.carte = Carte()
     self.planification = Cheminement(self.carte)
     self.voltageRestant = ""
     self.ileCible = -1
     self.positionTresor = []
     self.destination = []
     self.pause = True
     self.reset = False
     self.startCycle = False
     self.demiCercle = 180
     self.tempAttenteRotation = 9
     self.delaiAjustement = 4.5
     self.metreSeconde = 15
     self.metre = 100
     self.actionPrecedente = ""
     self.actionCourrante = ""
     self.nouvelleAction = False
     self.finCycle = False
     self.connected = False
     self.noPathFound = False
     self.aIle = False
     self.diametreIle = 2.125
     self.deltaPosition = 15
     self.deltaAngle = 4
     self.deltaDiametreIle = 0.125
Beispiel #2
0
 def reInitialiserVariables(self):
     self.code = ""
     self.carte = Carte()
     self.planification = Cheminement(self.carte)
     self.ileCible = -1
     self.destination = []
     self.pause = True
     self.reset = False
     self.startCycle = False
     self.repereCycle = [0,0]
     self.distanceMinimum = 60
     self.cycle = True
     self.enMouvement = False
     self.possedeTresor = False
     self.rechargeActiver = False
     self.auTresor = False
     self.nouvelleAction = False
     self.aIle = False
     self.retour = ""
     self.diametreIle = 2.125
Beispiel #3
0
    def run(self):
            self.repereCycle = [0,0]
            self.distanceMinimum = 60
            self.cycle = True
            self.enMouvement = False
            self.possedeTresor = False
            self.rechargeActiver = False
            self.auTresor = False
            self.retour = ""

            nombreRobotEntreLimitEtNoeudTresor = 1.3

            global trajectoireCalculer

            IpRobot = "192.168.0.48"
            port = 50007
            buf = 1024
            addr = (IpRobot, port)
            UDPSock = socket(AF_INET, SOCK_STREAM)

            UDPSock.connect(addr)
            self.connected = True
            while self.cycle:
                if self.reset == True:
                    UDPSock.sendto("6", addr)
                    time.sleep(1)
                    self.retour = UDPSock.recv(buf)
                    print self.retour
                    self.reInitialiserVariables()
                try:
                    if len(repere) == 2 and len(listePositions) >= 1 and len(positionRobotTrouvee) == 2 and self.pause == False:

                        if abs(repere[0] - self.repereCycle[0]) > 5 or abs(repere[1] - self.repereCycle[1]) > 5:
                            self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                            self.planification = Cheminement(self.carte)
                            self.repereCycle = repere

                        if self.enMouvement:
                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Mouvement"
                            self.nouvelleAction = True
                            deltaX = trajectoireCalculer[0].x - positionRobotTrouvee[0]
                            deltaY = positionRobotTrouvee[1] - trajectoireCalculer[0].y
                            angle = degrees(atan2(deltaY,deltaX))

                            if angle < 0:
                                angle = 360 + angle

                            angleRotation = angle - orientationRobotTrouvee

                            if angleRotation < 0:
                                angleRotation = 360 + angleRotation

                            angleText = str(int(angleRotation))

                            while len(angleText) < 4:
                                angleText = '0' + angleText

                            distanceCalcule = np.sqrt(np.power((trajectoireCalculer[0].x - positionRobotTrouvee[0]), 2)
                                                    + np.power((trajectoireCalculer[0].y - positionRobotTrouvee[1]), 2)) * ratioCMPixel

                            distanceText = str(int(distanceCalcule))

                            while len(distanceText) < 3:
                                distanceText = '0' + distanceText

                            demande = '1' + angleText + distanceText
                            UDPSock.sendto(demande, addr)
                            if angleRotation > 180:
                                angleRotation = 360 - angleRotation
                            tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + distanceCalcule/self.metre * self.metreSeconde
                            time.sleep(tempAttente)
                            self.retour = UDPSock.recv(buf)

                            if self.distancePositionDestination() < self.distanceMinimum:
                                self.enMouvement = False
                            trajectoireCalculer.pop(0)

                            if len(trajectoireCalculer) == 0:
                                self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                                self.planification = Cheminement(self.carte)
                                trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination)

                                if self.rechargeActiver == True:
                                    self.destination = (self.carte.recupererSommet(0).x,self.carte.recupererSommet(0).y)
                                    trajectoireCalculer.append(self.carte.recupererSommet(0))
                                elif self.auTresor == True:
                                    distanceTresor = sys.maxint

                                    for position in listePositionsTresor:
                                        distanceCalcule = np.sqrt(np.power((position[0] - positionRobotTrouvee[0]), 2)
                                                        + np.power((position[1] - positionRobotTrouvee[1]), 2))
                                        noeudTresorTemp = None

                                        for noeudId in self.carte.dictSommets:
                                            noeud = self.carte.recupererSommet(noeudId)

                                            if noeud.estTresor == True:

                                                if position[1] < repere[1]:

                                                    if position[0] - self.deltaPosition < noeud.x < position[0] + self.deltaPosition and \
                                                                                    noeud.y - self.deltaPosition < position[1] + nombreRobotEntreLimitEtNoeudTresor * self.carte.robotPixel < noeud.y + self.deltaPosition:
                                                        noeudTresorTemp = self.carte.recupererSommet(noeud.id)
                                                        break
                                                else:

                                                    if position[0] - self.deltaPosition < noeud.x < position[0] + self.deltaPosition and \
                                                                                    noeud.y - self.deltaPosition < position[1] - nombreRobotEntreLimitEtNoeudTresor * self.carte.robotPixel < noeud.y + self.deltaPosition:
                                                        noeudTresorTemp = self.carte.recupererSommet(noeud.id)
                                                        break

                                        if distanceCalcule < distanceTresor and noeudTresorTemp != None:
                                            self.destination = position
                                            distanceTresor = distanceCalcule
                                            self.noeudTresor = noeudTresorTemp

                                    self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                                    self.planification = Cheminement(self.carte)
                                    trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination)
                                    trajectoireCalculer.append(self.noeudTresor)
                                    self.destination = (self.noeudTresor.x, self.noeudTresor.y)

                                elif self.possedeTresor == True:

                                    self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                                    self.planification = Cheminement(self.carte)
                                    noeud = self.planification.trouverPointPlusProcheIle(listePositions[self.ileCible],positionRobotTrouvee)
                                    self.destination = (noeud.x,noeud.y)
                                    trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,(self.destination[0],self.destination[1]))
                                    trajectoireCalculer.append(noeud)

                        elif self.possedeTresor == False and self.enMouvement == False \
                                and self.rechargeActiver == False and len(self.code) == 0 and self.auTresor == False:

                            self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                            self.planification = Cheminement(self.carte)
                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Calcule Trajectoire a station de base"
                            self.nouvelleAction = True
                            self.destination = (self.carte.recupererSommet(0).x,self.carte.recupererSommet(0).y)
                            trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination)

                            if len(trajectoireCalculer) == 0:
                                raise PathNotFoundException()

                            trajectoireCalculer.append(self.carte.recupererSommet(0))
                            self.enMouvement = True
                            self.rechargeActiver = True

                        elif self.possedeTresor == False and self.enMouvement == False \
                                and self.rechargeActiver == True and len(self.code) == 0 and self.auTresor == False and self.aIle == False:

                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Ajustement"
                            self.nouvelleAction = True

                            angle = 90
                            limitVoltage = 4.5

                            while angle - self.deltaAngle >= orientationRobotTrouvee  or orientationRobotTrouvee >= angle + self.deltaAngle:
                                angleRotation = angle - orientationRobotTrouvee
                                print "angle: " + str(orientationRobotTrouvee)

                                if angleRotation < 0:
                                    angleRotation = 360 + angleRotation

                                angleText = str(int(angleRotation))
                                print "rotoation " + angleText

                                while len(angleText) < 4:
                                    angleText = '0' + angleText

                                demande = "1" + angleText + "000"
                                UDPSock.sendto(demande, addr)

                                if angleRotation > 180:
                                    angleRotation = 360 - angleRotation

                                tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement
                                time.sleep(tempAttente)
                                self.retour = UDPSock.recv(buf)
                                self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1]

                            time.sleep(1)
                            demande = '3'
                            UDPSock.sendto(demande,addr)
                            time.sleep(15)
                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Recharge et request au serveur"
                            self.nouvelleAction = True

                            while float(self.voltageRestant) < limitVoltage or not(self.retour.find("couleur") > 0 or self.retour.find("forme") > 0):
                                try:
                                    self.retour = UDPSock.recv(buf)
                                    self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1]
                                except:
                                    pass

                            self.code = self.retour[len(self.retour) - 5]

                            if self.retour.find("couleur") > 0:
                                for index in range(0,len(listeCouleurs)):
                                    if self.retour.find(listeCouleurs[index]) > 0:
                                        self.ileCible = index
                                        break

                            if self.retour.find("forme") > 0:
                                for index in range(0, len(listeFormes)):
                                    if self.retour.find(listeFormes[index]) > 0:
                                        self.ileCible = index
                                        break

                            print self.ileCible

                            time.sleep(10)

                            self.rechargeActiver = False

                        elif self.possedeTresor == False and self.enMouvement == False \
                                and self.rechargeActiver == False and len(self.code) == 1 and self.auTresor == False and self.aIle == False:

                            if len(listePositionsTresor) < 0:
                                positionXTresorText = ""
                                positionYTresorText = ""

                                orientationRobotText = str(int(orientationRobotTrouvee))

                                while len(orientationRobotText) < 3:
                                    orientationRobotText = '0' + orientationRobotText

                                positionRobotXText = str(int(positionRobotTrouvee[0]))

                                while len(positionRobotXText) < 4:
                                    positionRobotXText = '0'+positionRobotXText

                                positionRobotYText = str(int(positionRobotTrouvee[1]))

                                while len(positionRobotYText) < 4:
                                    positionRobotYText = '0'+positionRobotYText

                                repereXText = str(int(repere[0]))

                                while len(repereXText) < 4:
                                   repereXText = '0' + repereXText

                                repereYText = str(int(repere[1]))

                                while len(repereYText) < 4:
                                    repereYText = '0' + repereYText


                                ratioText = "%.2f" % round(ratioCMPixel, 2)

                                while ratioText < 4:
                                    ratioText = '0' + ratioText

                                demande = '2' + orientationRobotText  + positionRobotYText + repereXText + repereYText + ratioText

                                UDPSock.sendto(demande,addr)
                                self.retour = UDPSock.recv(buf)

                                index = 0

                                while self.retour[index] != " ":
                                    positionXTresorText = positionXTresorText + self.retour[index]
                                    index += 1
                                index += 1

                                while index < len(self.retour) - 4:
                                    positionYTresorText = positionYTresorText + self.retour[index]
                                    index += 1

                                positionTresor.append(int(positionXTresorText))
                                positionTresor.append(int(positionYTresorText))
                            else:

                                self.actionPrecedente = self.actionCourrante
                                self.actionCourrante = "Calcule trajectoire au tresor"
                                self.nouvelleAction = True
                                distanceTresor = sys.maxint

                                for position in listePositionsTresor:
                                    distanceCalcule = np.sqrt(np.power((position[0] - positionRobotTrouvee[0]), 2)
                                                    + np.power((position[1] - positionRobotTrouvee[1]), 2))
                                    noeudTresorTemp = None
                                    for noeudId in self.carte.dictSommets:
                                        noeud = self.carte.recupererSommet(noeudId)
                                        if noeud.estTresor == True:
                                            if position[1] < repere[1]:
                                                if position[0] - 15 < noeud.x < position[0] + 15 and noeud.y - 15 < position[1] + 1.3 * self.carte.robotPixel < noeud.y + 15:
                                                    noeudTresorTemp = self.carte.recupererSommet(noeud.id)
                                                    break
                                            else:
                                                if position[0] - 15 < noeud.x < position[0] + 15 and noeud.y - 15 < position[1] - 1.3 * self.carte.robotPixel < noeud.y + 15:
                                                    noeudTresorTemp = self.carte.recupererSommet(noeud.id)
                                                    break
                                    if distanceCalcule < distanceTresor and noeudTresorTemp != None:
                                        self.destination = position
                                        distanceTresor = distanceCalcule
                                        self.noeudTresor = noeudTresorTemp

                                self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                                self.planification = Cheminement(self.carte)
                                trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,self.destination)

                                if len(trajectoireCalculer) == 0:
                                    raise PathNotFoundException()

                                trajectoireCalculer.append(self.noeudTresor)
                                self.destination = (self.noeudTresor.x, self.noeudTresor.y)

                            self.enMouvement = True
                            self.auTresor = True

                        if self.possedeTresor == False and self.enMouvement == False \
                                and self.rechargeActiver == False and len(self.code) == 1 and self.auTresor == True and self.aIle == False:

                            if self.destination[1] < repere[1]:
                                angle = 90
                            else:
                                angle = 270

                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Ajustement"
                            self.nouvelleAction = True

                            while angle-self.deltaAngle >= orientationRobotTrouvee  or orientationRobotTrouvee >= angle+self.deltaAngle:

                                angleRotation = angle - orientationRobotTrouvee
                                print "angle: " + str(orientationRobotTrouvee)

                                if angleRotation < 0:
                                    angleRotation = 360 + angleRotation

                                angleText = str(int(angleRotation))
                                print "rotoation " + angleText

                                while len(angleText) < 4:
                                    angleText = '0' + angleText

                                demande = "1" + angleText + "000"
                                UDPSock.sendto(demande, addr)

                                if angleRotation > 180:
                                    angleRotation = 360 - angleRotation

                                tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement
                                time.sleep(tempAttente)
                                self.retour = UDPSock.recv(buf)
                                self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1]

                            demande = '4'
                            UDPSock.sendto(demande,addr)
                            time.sleep(5)
                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Prise du robot"
                            self.nouvelleAction = True
                            self.retour = UDPSock.recv(buf)
                            self.possedeTresor = True

                        elif self.enMouvement == False and self.rechargeActiver == False \
                                and len(self.code) == 1 and self.auTresor == True and self.possedeTresor == True and self.aIle == False:

                            self.carte.genererCarte(repere, listePositions, ratioCMPixel,listePositionsTresor,self.diametreIle)
                            self.planification = Cheminement(self.carte)
                            noeud = self.planification.trouverPointPlusProcheIle(listePositions[self.ileCible],positionRobotTrouvee)
                            self.destination = (noeud.x,noeud.y)
                            trajectoireCalculer = self.planification.trouverTrajectoire(positionRobotTrouvee,(self.destination[0],self.destination[1]))

                            if len(trajectoireCalculer) == 0:
                                raise PathNotFoundException()

                            trajectoireCalculer.append(noeud)
                            self.auTresor = False
                            self.enMouvement = True
                            self.aIle = True

                        elif self.enMouvement == False and self.rechargeActiver == False \
                                and len(self.code) == 1 and self.auTresor == False and self.possedeTresor == True and self.aIle == True:

                            deltaX = listePositions[self.ileCible][0] - positionRobotTrouvee[0]
                            deltaY = positionRobotTrouvee[1] - listePositions[self.ileCible][1]
                            angle = degrees(atan2(deltaY,deltaX))

                            if angle < 0:
                                angle = 360 + angle

                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Ajustement"
                            self.nouvelleAction = True

                            while angle - self.deltaAngle >= orientationRobotTrouvee  or orientationRobotTrouvee >= angle + self.deltaAngle:
                                angleRotation = angle - orientationRobotTrouvee
                                print "angle: " + str(orientationRobotTrouvee)

                                if angleRotation < 0:
                                    angleRotation = 360 + angleRotation

                                angleText = str(int(angleRotation))
                                print "rotoation " + angleText

                                while len(angleText) < 4:
                                    angleText = '0' + angleText

                                demande = "1" + angleText + "000"
                                UDPSock.sendto(demande, addr)

                                if angleRotation > 180:
                                    angleRotation = 360 - angleRotation

                                tempAttente = (angleRotation)/self.demiCercle * self.tempAttenteRotation + self.delaiAjustement
                                time.sleep(tempAttente)
                                self.retour = UDPSock.recv(buf)
                                self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1]

                            couleur = ""
                            if listeCouleurs[self.ileCible] == "bleu":
                                couleur = "1"
                            elif listeCouleurs[self.ileCible] == "vert":
                                couleur = "2"
                            elif listeCouleurs[self.ileCible] == "rouge":
                                couleur = "3"
                            elif listeCouleurs[self.ileCible] == "jaune":
                                couleur = "4"

                            demande = '5' + couleur
                            UDPSock.sendto(demande,addr)
                            time.sleep(15)
                            self.actionPrecedente = self.actionCourrante
                            self.actionCourrante = "Depot du robot"
                            self.nouvelleAction = True
                            self.retour = UDPSock.recv(buf)
                            self.finCycle = True
                            self.reset = True

                        if self.retour != "":

                            print self.retour
                            self.voltageRestant = self.retour[len(self.retour) - 4] + self.retour[len(self.retour) - 3] + self.retour[len(self.retour) - 2] + self.retour[len(self.retour) - 1]

                except PathNotFoundException as e:
                    self.noPathFound = True

                    if self.diametreIle > 1.5:
                        self.diametreIle -= self.deltaDiametreIle

                    else:
                        demiCercle = 179
                        demande = "10179010"
                        UDPSock.sendto(demande, addr)
                        tempAttente = (demiCercle)/self.demiCercle * self.tempAttenteRotation + 10/self.metre * self.metreSeconde
                        time.sleep(tempAttente)
                        self.retour = UDPSock.recv(buf)
                    continue