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
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
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