示例#1
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"state1",
			"state2",
		]

		self.StatesSub2 = [
			"state11",
			"state11_ghost",
			"state12",
			"state12_ghost",
		]

		self.sub1 = "state1"
		self.run1 = True
		self.sub2 = "state11_ghost"
		self.run2 = True

	int functions(int a, int b) {
		return a+b;
	}
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t2 = threading.Thread(target=self.subautomata2)
		self.t2.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 2, 118, 114, 1, 'state1')
		guiSubautomata1.newGuiNode(2, 0, 354, 313, 0, 'state2')

		guiSubautomata1.newGuiTransition((118, 114), (354, 313), (299, 156), 1, 1, 2)
		guiSubautomata1.newGuiTransition((354, 313), (118, 114), (171, 304), 3, 2, 1)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata2
		guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)

		guiSubautomata2.newGuiNode(3, 0, 147, 123, 1, 'state11')
		guiSubautomata2.newGuiNode(4, 0, 407, 347, 0, 'state12')

		guiSubautomata2.newGuiTransition((147, 123), (407, 347), (277, 235), 2, 3, 4)
		guiSubautomataList.append(guiSubautomata2)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run2 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 200
		t_activated = False
		t_fin = 0

		int variables = 10;

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "state1"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "state2"
						t_activated = False
						int transition_code = 12;
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('state2')

			elif(self.sub1 == "state2"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "state1"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('state1')


			# Actuation if
			if(self.sub1 == "state1"):
				int state1_code = 10;
			elif(self.sub1 == "state2"):
				int state2_code = 10;

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata2(self):
		self.run2 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_state11_max = 10


		while(self.run2):
			totala = time.time() * 1000000

			if(self.sub1 == "state1"):
				if ((self.sub2 == "state11_ghost") or (self.sub2 == "state12_ghost")):
					ghostStateIndex = self.StatesSub2.index(self.sub2)
					self.sub2 = self.StatesSub2[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub2 == "state11"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_state11_max):
							self.sub2 = "state12"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('state12')
							t_state11_max = 10


				# Actuation if
				if(self.sub2 == "state11"):
					myMotors->setV(0.2);
				elif(self.sub2 == "state12"):
					myMotors->setV(0.0);
			else:
				if(self.sub2 == "state11"):
					t_state11_max = 10 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]
				elif(self.sub2 == "state12"):
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)

		# Contact to myMotors
		myMotors = self.ic.propertyToProxy('automata.myMotors.Proxy')
		if(not myMotors):
			raise Exception('could not create proxy with myMotors')
		self.myMotors = MotorsPrx.checkedCast(myMotors)
		if(not self.myMotors):
			raise Exception('invalid proxy automata.myMotors.Proxy')
		print('myMotors connected')


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t2.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print('runtime gui enabled')
				else:
					self.displayGui = False
					print('runtime gui disabled')
示例#2
0
文件: try1.py 项目: jorgevelap/TFG
class Automata():
	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"State1",
			"State2",
			"State3",
			"State4",
		]

		self.sub1 = "State1"
		self.run1 = True

	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 80, 160, 1, 'State1')
		guiSubautomata1.newGuiNode(2, 0, 473, 164, 0, 'State2')
		guiSubautomata1.newGuiNode(3, 0, 475, 431, 0, 'State3')
		guiSubautomata1.newGuiNode(4, 0, 94, 417, 0, 'State4')

		guiSubautomata1.newGuiTransition((80, 160), (473, 164), (276, 162), 1, 1, 2)
		guiSubautomata1.newGuiTransition((473, 164), (475, 431), (474, 298), 2, 2, 3)
		guiSubautomata1.newGuiTransition((475, 431), (94, 417), (285, 424), 3, 3, 4)
		guiSubautomata1.newGuiTransition((94, 417), (80, 160), (87, 288), 4, 4, 1)
		guiSubautomataList.append(guiSubautomata1)


		return guiSubautomataList

	def shutDown(self):

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		self.finished = False

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "State1"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "State2"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('State2')

			elif(self.sub1 == "State2"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "State3"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('State3')

			elif(self.sub1 == "State3"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "State4"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('State4')

			elif(self.sub1 == "State4"):
				if(self.finished):
					self.sub1 = "State1"
					print "Finished!"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('State1')


			# Actuation if
			if(self.sub1 == "State1"):
				print "State1"
				time.sleep(2)
			elif(self.sub1 == "State2"):
				print "State2"
				time.sleep(2)
			elif(self.sub1 == "State3"):
				print "State3"
				time.sleep(2)
			elif(self.sub1 == "State4"):
				print "State4"
				time.sleep(2)
				self.finished = True

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);

	def connectToProxys(self):

	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()

	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()

	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()

	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'

if __name__ == '__main__':
	signal.signal(signal.SIGINT, signal.SIG_DFL)
	automata = Automata()
	try:
		automata.connectToProxys()
		automata.readArgs()
		automata.start()
		automata.join()

		sys.exit(0)
	except:
		traceback.print_exc()
		automata.destroyIc()
		sys.exit(-1)
示例#3
0
文件: try3.py 项目: reysam93/TFG
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"TakeOff",
			"GoFront",
			"DoSquare",
			"Landing",
			"END",
		]

		self.StatesSub2 = [
			"TakingOff",
			"TakingOff_ghost",
			"Stabilizing1",
			"Stabilizing1_ghost",
		]

		self.StatesSub3 = [
			"GoingFront",
			"GoingFront_ghost",
			"Stabilizing2",
			"Stabilizing2_ghost",
			"goneFront",
			"goneFront_ghost",
		]

		self.StatesSub4 = [
			"empty1",
			"empty1_ghost",
			"empty2",
			"empty2_ghost",
		]

		self.StatesSub5 = [
			"DoingRigthSide",
			"DoingRigthSide_ghost",
			"DoingTopSide",
			"DoingTopSide_ghost",
			"DoingLeftSide",
			"DoingLeftSide_ghost",
			"DoingBottomSide",
			"DoingBottomSide_ghost",
		]

		self.StatesSub6 = [
			"GoingToTop",
			"GoingToTop_ghost",
			"StabilizingTop",
			"StabilizingTop_ghost",
			"GoneToTop",
			"GoneToTop_ghost",
		]

		self.StatesSub8 = [
			"GoingToLeft",
			"GoingToLeft_ghost",
			"StabilizingLeft",
			"StabilizingLeft_ghost",
			"GoneToLeft",
			"GoneToLeft_ghost",
		]

		self.StatesSub9 = [
			"GoingToBottom",
			"GoingToBottom_ghost",
			"StabilizingBottom",
			"StabilizingBottom_ghost",
			"GoneToBottom",
			"GoneToBottom_ghost",
		]

		self.StatesSub10 = [
			"GoingToRigth",
			"GoingToRigth_ghost",
			"StabilizingRigth",
			"StabilizingRigth_ghost",
			"GoneToRigth",
			"GoneToRigth_ghost",
		]

		self.sub1 = "TakeOff"
		self.run1 = True
		self.sub2 = "TakingOff_ghost"
		self.run2 = True
		self.sub3 = "GoingFront_ghost"
		self.run3 = True
		self.sub4 = "empty1_ghost"
		self.run4 = True
		self.sub5 = "DoingRigthSide_ghost"
		self.run5 = True
		self.sub6 = "GoingToTop_ghost"
		self.run6 = True
		self.sub8 = "GoingToLeft_ghost"
		self.run8 = True
		self.sub9 = "GoingToBottom_ghost"
		self.run9 = True
		self.sub10 = "GoingToRigth_ghost"
		self.run10 = True

	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t2 = threading.Thread(target=self.subautomata2)
		self.t2.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()
		self.t4 = threading.Thread(target=self.subautomata4)
		self.t4.start()
		self.t5 = threading.Thread(target=self.subautomata5)
		self.t5.start()
		self.t6 = threading.Thread(target=self.subautomata6)
		self.t6.start()
		self.t8 = threading.Thread(target=self.subautomata8)
		self.t8.start()
		self.t9 = threading.Thread(target=self.subautomata9)
		self.t9.start()
		self.t10 = threading.Thread(target=self.subautomata10)
		self.t10.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 2, 76, 158, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(5, 3, 323, 136, 0, 'GoFront')
		guiSubautomata1.newGuiNode(14, 5, 587, 181, 0, 'DoSquare')
		guiSubautomata1.newGuiNode(15, 0, 395, 467, 0, 'Landing')
		guiSubautomata1.newGuiNode(16, 0, 89, 421, 0, 'END')

		guiSubautomata1.newGuiTransition((76, 158), (323, 136), (189, 145), 2, 1, 5)
		guiSubautomata1.newGuiTransition((323, 136), (587, 181), (446, 132), 6, 5, 14)
		guiSubautomata1.newGuiTransition((395, 467), (89, 421), (226, 464), 8, 15, 16)
		guiSubautomata1.newGuiTransition((89, 421), (76, 158), (48, 290), 28, 16, 1)
		guiSubautomata1.newGuiTransition((587, 181), (395, 467), (491, 324), 37, 14, 15)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata2
		guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)

		guiSubautomata2.newGuiNode(2, 0, 106, 141, 1, 'TakingOff')
		guiSubautomata2.newGuiNode(3, 0, 283, 219, 0, 'Stabilizing1')

		guiSubautomata2.newGuiTransition((106, 141), (283, 219), (138, 243), 1, 2, 3)
		guiSubautomata2.newGuiTransition((283, 219), (106, 141), (244, 116), 26, 3, 2)
		guiSubautomataList.append(guiSubautomata2)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,5, self.automataGui)

		guiSubautomata3.newGuiNode(9, 0, 127, 107, 1, 'GoingFront')
		guiSubautomata3.newGuiNode(10, 4, 511, 137, 0, 'Stabilizing2')
		guiSubautomata3.newGuiNode(11, 0, 298, 291, 0, 'goneFront')

		guiSubautomata3.newGuiTransition((127, 107), (511, 137), (319, 123), 2, 9, 10)
		guiSubautomata3.newGuiTransition((511, 137), (298, 291), (405, 214), 3, 10, 11)
		guiSubautomata3.newGuiTransition((298, 291), (127, 107), (212, 199), 27, 11, 9)
		guiSubautomataList.append(guiSubautomata3)

		# Creating subAutomata4
		guiSubautomata4 = GuiSubautomata(4,10, self.automataGui)

		guiSubautomata4.newGuiNode(12, 0, 232, 178, 1, 'empty1')
		guiSubautomata4.newGuiNode(13, 0, 445, 217, 0, 'empty2')

		guiSubautomata4.newGuiTransition((232, 178), (445, 217), (357, 129), 4, 12, 13)
		guiSubautomata4.newGuiTransition((445, 217), (232, 178), (316, 271), 5, 13, 12)
		guiSubautomataList.append(guiSubautomata4)

		# Creating subAutomata5
		guiSubautomata5 = GuiSubautomata(5,14, self.automataGui)

		guiSubautomata5.newGuiNode(17, 6, 465, 388, 1, 'DoingRigthSide')
		guiSubautomata5.newGuiNode(18, 8, 461, 140, 0, 'DoingTopSide')
		guiSubautomata5.newGuiNode(19, 9, 134, 130, 0, 'DoingLeftSide')
		guiSubautomata5.newGuiNode(20, 10, 141, 389, 0, 'DoingBottomSide')

		guiSubautomata5.newGuiTransition((465, 388), (461, 140), (463, 264), 9, 17, 18)
		guiSubautomata5.newGuiTransition((461, 140), (134, 130), (298, 136), 10, 18, 19)
		guiSubautomata5.newGuiTransition((134, 130), (141, 389), (137, 260), 11, 19, 20)
		guiSubautomata5.newGuiTransition((141, 389), (465, 388), (303, 388), 12, 20, 17)
		guiSubautomataList.append(guiSubautomata5)

		# Creating subAutomata6
		guiSubautomata6 = GuiSubautomata(6,17, self.automataGui)

		guiSubautomata6.newGuiNode(21, 0, 352, 452, 1, 'GoingToTop')
		guiSubautomata6.newGuiNode(22, 0, 280, 218, 0, 'StabilizingTop')
		guiSubautomata6.newGuiNode(23, 0, 448, 105, 0, 'GoneToTop')

		guiSubautomata6.newGuiTransition((352, 452), (280, 218), (212, 319), 13, 21, 22)
		guiSubautomata6.newGuiTransition((280, 218), (448, 105), (350, 132), 14, 22, 23)
		guiSubautomata6.newGuiTransition((448, 105), (352, 452), (400, 279), 16, 23, 21)
		guiSubautomataList.append(guiSubautomata6)

		# Creating subAutomata8
		guiSubautomata8 = GuiSubautomata(8,18, self.automataGui)

		guiSubautomata8.newGuiNode(24, 0, 135, 397, 1, 'GoingToLeft')
		guiSubautomata8.newGuiNode(25, 0, 213, 194, 0, 'StabilizingLeft')
		guiSubautomata8.newGuiNode(26, 0, 416, 260, 0, 'GoneToLeft')

		guiSubautomata8.newGuiTransition((135, 397), (213, 194), (174, 295), 17, 24, 25)
		guiSubautomata8.newGuiTransition((213, 194), (416, 260), (314, 227), 18, 25, 26)
		guiSubautomata8.newGuiTransition((416, 260), (135, 397), (275, 328), 19, 26, 24)
		guiSubautomataList.append(guiSubautomata8)

		# Creating subAutomata9
		guiSubautomata9 = GuiSubautomata(9,19, self.automataGui)

		guiSubautomata9.newGuiNode(27, 0, 169, 408, 1, 'GoingToBottom')
		guiSubautomata9.newGuiNode(28, 0, 243, 164, 0, 'StabilizingBottom')
		guiSubautomata9.newGuiNode(29, 0, 418, 355, 0, 'GoneToBottom')

		guiSubautomata9.newGuiTransition((169, 408), (243, 164), (206, 286), 20, 27, 28)
		guiSubautomata9.newGuiTransition((243, 164), (418, 355), (330, 259), 21, 28, 29)
		guiSubautomata9.newGuiTransition((418, 355), (169, 408), (293, 381), 22, 29, 27)
		guiSubautomataList.append(guiSubautomata9)

		# Creating subAutomata10
		guiSubautomata10 = GuiSubautomata(10,20, self.automataGui)

		guiSubautomata10.newGuiNode(31, 0, 121, 361, 1, 'GoingToRigth')
		guiSubautomata10.newGuiNode(32, 0, 231, 110, 0, 'StabilizingRigth')
		guiSubautomata10.newGuiNode(33, 0, 371, 331, 0, 'GoneToRigth')

		guiSubautomata10.newGuiTransition((121, 361), (231, 110), (176, 235), 23, 31, 32)
		guiSubautomata10.newGuiTransition((231, 110), (371, 331), (300, 220), 24, 32, 33)
		guiSubautomata10.newGuiTransition((371, 331), (121, 361), (246, 346), 25, 33, 31)
		guiSubautomataList.append(guiSubautomata10)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run2 = False
		self.run3 = False
		self.run4 = False
		self.run5 = False
		self.run6 = False
		self.run8 = False
		self.run9 = False
		self.run10 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		self.goneFront = False
		self.squareDone = False
		self.goneBack = False

		while(run):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3.5):
						self.sub1 = "GoFront"
						t_activated = False
						print "From TakeOff to GoFront"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('GoFront')

			elif(self.sub1 == "GoFront"):
				if(self.goneFront):
					self.sub1 = "DoSquare"
					self.goneFront = False
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('DoSquare')

			elif(self.sub1 == "DoSquare"):
				if(self.squareDone):
					self.sub1 = "Landing"
					self.squareDone = False
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('Landing')

			elif(self.sub1 == "Landing"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "END"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('END')

			elif(self.sub1 == "END"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "TakeOff"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('TakeOff')


			# Actuation if
			if(self.sub1 == "Landing"):
				print "landing"
				self.lock.acquire()
				self.extraPrx.land()
				self.lock.release()
			elif(self.sub1 == "END"):
				print "END"

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata2(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_TakingOff_max = 2
		t_Stabilizing1_max = 1.7


		while(run):
			totala = time.time() * 1000000

			if(self.sub1 == "TakeOff"):
				if ((self.sub2 == "TakingOff_ghost") or (self.sub2 == "Stabilizing1_ghost")):
					ghostStateIndex = self.StatesSub2.index(self.sub2)
					self.sub2 = self.StatesSub2[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub2 == "TakingOff"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_TakingOff_max):
							self.sub2 = "Stabilizing1"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('Stabilizing1')
							t_TakingOff_max = 2

				elif(self.sub2 == "Stabilizing1"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_Stabilizing1_max):
							self.sub2 = "TakingOff"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('TakingOff')
							t_Stabilizing1_max = 1.7


				# Actuation if
				if(self.sub2 == "TakingOff"):
					print "Taking Off"
					self.lock.acquire()
					self.extraPrx.takeoff()
					self.lock.release()
			else:
				if(self.sub2 == "TakingOff"):
					t_TakingOff_max = 2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]
				elif(self.sub2 == "Stabilizing1"):
					t_Stabilizing1_max = 1.7 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_Stabilizing2_max = 3
		t_goneFront_max = 0.2

		initPos = 0
		pos = 0

		while(run):
			totala = time.time() * 1000000

			if(self.sub1 == "GoFront"):
				if ((self.sub3 == "GoingFront_ghost") or (self.sub3 == "Stabilizing2_ghost") or (self.sub3 == "goneFront_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "GoingFront"):
					if(pos - initPos > 300):
						self.sub3 = "Stabilizing2"
						print "300m reached"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Stabilizing2')

				elif(self.sub3 == "Stabilizing2"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_Stabilizing2_max):
							self.sub3 = "goneFront"
							t_activated = False
							print "from stabilizing2 to GoneFront"
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('goneFront')
							t_Stabilizing2_max = 3

				elif(self.sub3 == "goneFront"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_goneFront_max):
							self.sub3 = "GoingFront"
							t_activated = False
							initPos = 0
							pos = 0
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoingFront')
							t_goneFront_max = 0.2


				# Actuation if
				if(self.sub3 == "GoingFront"):
					pos = self.pose3dPrx.getPose3DData().x * 100
					
					if initPos == 0:
						initPos = self.pose3dPrx.getPose3DData().x * 100
					
					print "pos:", pos, "init:", initPos
					print "distancia", pos - initPos
					
					cmd = jderobot.CMDVelData()
					cmd.linearX = 1
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub3 == "Stabilizing2"):
					cmd.linearX = 0
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub3 == "goneFront"):
					self.goneFront = True
			else:
				if(self.sub3 == "GoingFront"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "Stabilizing2"):
					t_Stabilizing2_max = 3 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "goneFront"):
					t_goneFront_max = 0.2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata4(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_empty1_max = 0.8
		t_empty2_max = 0.8


		while(run):
			totala = time.time() * 1000000

			if(self.sub3 == "Stabilizing2"):
				if ((self.sub4 == "empty1_ghost") or (self.sub4 == "empty2_ghost")):
					ghostStateIndex = self.StatesSub4.index(self.sub4)
					self.sub4 = self.StatesSub4[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub4 == "empty1"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_empty1_max):
							self.sub4 = "empty2"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('empty2')
							t_empty1_max = 0.8

				elif(self.sub4 == "empty2"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_empty2_max):
							self.sub4 = "empty1"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('empty1')
							t_empty2_max = 0.8


				# Actuation if
			else:
				if(self.sub4 == "empty1"):
					t_empty1_max = 0.8 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]
				elif(self.sub4 == "empty2"):
					t_empty2_max = 0.8 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata5(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0


		self.goneToTop = False
		self.goneToLeft = False
		self.goneToBottom = False
		self.goneToRigth = False

		while(run):
			totala = time.time() * 1000000

			if(self.sub1 == "DoSquare"):
				if ((self.sub5 == "DoingRigthSide_ghost") or (self.sub5 == "DoingTopSide_ghost") or (self.sub5 == "DoingLeftSide_ghost") or (self.sub5 == "DoingBottomSide_ghost")):
					ghostStateIndex = self.StatesSub5.index(self.sub5)
					self.sub5 = self.StatesSub5[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub5 == "DoingRigthSide"):
					if(self.goneToTop):
						self.sub5 = "DoingTopSide"
						print "Rigth Done"
						self.goneToTop = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('DoingTopSide')

				elif(self.sub5 == "DoingTopSide"):
					if(self.goneToLeft):
						self.sub5 = "DoingLeftSide"
						print "Left done"
						self.goneToLeft = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('DoingLeftSide')

				elif(self.sub5 == "DoingLeftSide"):
					if(self.goneToBottom):
						self.sub5 = "DoingBottomSide"
						print "Left done"
						self.goneToBottom = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('DoingBottomSide')

				elif(self.sub5 == "DoingBottomSide"):
					if(self.goneToRigth):
						self.sub5 = "DoingRigthSide"
						
						
						self.squareDone = True
						print "Bottom done"
						self.goneToRigth = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('DoingRigthSide')


				# Actuation if
				if(self.sub5 == "DoingRigthSide"):
					print "Doing Rigth Side"
				elif(self.sub5 == "DoingTopSide"):
					print "Doing Top Side"
				elif(self.sub5 == "DoingLeftSide"):
					print "Doing Left Side"
				elif(self.sub5 == "DoingBottomSide"):
					print "Doing Bottom Side"
			else:
				if(self.sub5 == "DoingRigthSide"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "DoingTopSide"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "DoingLeftSide"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "DoingBottomSide"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata6(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_GoingToTop_max = 3
		t_StabilizingTop_max = 2
		t_GoneToTop_max = 0.1


		while(run):
			totala = time.time() * 1000000

			if(self.sub5 == "DoingRigthSide"):
				if ((self.sub6 == "GoingToTop_ghost") or (self.sub6 == "StabilizingTop_ghost") or (self.sub6 == "GoneToTop_ghost")):
					ghostStateIndex = self.StatesSub6.index(self.sub6)
					self.sub6 = self.StatesSub6[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub6 == "GoingToTop"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoingToTop_max):
							self.sub6 = "StabilizingTop"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('StabilizingTop')
							t_GoingToTop_max = 3

				elif(self.sub6 == "StabilizingTop"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_StabilizingTop_max):
							self.sub6 = "GoneToTop"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoneToTop')
							t_StabilizingTop_max = 2

				elif(self.sub6 == "GoneToTop"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoneToTop_max):
							self.sub6 = "GoingToTop"
							t_activated = False
							self.GoneToTop = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoingToTop')
							t_GoneToTop_max = 0.1


				# Actuation if
				if(self.sub6 == "GoingToTop"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0.75
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub6 == "StabilizingTop"):
					cmd.linearX = 0
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub6 == "GoneToTop"):
					self.goneToTop = True
			else:
				if(self.sub6 == "GoingToTop"):
					t_GoingToTop_max = 3 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]
				elif(self.sub6 == "StabilizingTop"):
					t_StabilizingTop_max = 2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]
				elif(self.sub6 == "GoneToTop"):
					t_GoneToTop_max = 0.1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata8(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_GoingToLeft_max = 3
		t_StabilizingLeft_max = 2
		t_GoneToLeft_max = 0.1


		while(run):
			totala = time.time() * 1000000

			if(self.sub5 == "DoingTopSide"):
				if ((self.sub8 == "GoingToLeft_ghost") or (self.sub8 == "StabilizingLeft_ghost") or (self.sub8 == "GoneToLeft_ghost")):
					ghostStateIndex = self.StatesSub8.index(self.sub8)
					self.sub8 = self.StatesSub8[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub8 == "GoingToLeft"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoingToLeft_max):
							self.sub8 = "StabilizingLeft"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('StabilizingLeft')
							t_GoingToLeft_max = 3

				elif(self.sub8 == "StabilizingLeft"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_StabilizingLeft_max):
							self.sub8 = "GoneToLeft"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoneToLeft')
							t_StabilizingLeft_max = 2

				elif(self.sub8 == "GoneToLeft"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoneToLeft_max):
							self.sub8 = "GoingToLeft"
							t_activated = False
							self.goneToLeft = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoingToLeft')
							t_GoneToLeft_max = 0.1


				# Actuation if
				if(self.sub8 == "GoingToLeft"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0
					cmd.linearY = 0.75
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub8 == "StabilizingLeft"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub8 == "GoneToLeft"):
					self.goneToLeft = True
			else:
				if(self.sub8 == "GoingToLeft"):
					t_GoingToLeft_max = 3 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
					self.sub8 = self.StatesSub8[ghostStateIndex]
				elif(self.sub8 == "StabilizingLeft"):
					t_StabilizingLeft_max = 2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
					self.sub8 = self.StatesSub8[ghostStateIndex]
				elif(self.sub8 == "GoneToLeft"):
					t_GoneToLeft_max = 0.1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
					self.sub8 = self.StatesSub8[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata9(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_GoingToBottom_max = 3
		t_StabilizingBottom_max = 2
		t_GoneToBottom_max = 0.1


		while(run):
			totala = time.time() * 1000000

			if(self.sub5 == "DoingLeftSide"):
				if ((self.sub9 == "GoingToBottom_ghost") or (self.sub9 == "StabilizingBottom_ghost") or (self.sub9 == "GoneToBottom_ghost")):
					ghostStateIndex = self.StatesSub9.index(self.sub9)
					self.sub9 = self.StatesSub9[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub9 == "GoingToBottom"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoingToBottom_max):
							self.sub9 = "StabilizingBottom"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('StabilizingBottom')
							t_GoingToBottom_max = 3

				elif(self.sub9 == "StabilizingBottom"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_StabilizingBottom_max):
							self.sub9 = "GoneToBottom"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoneToBottom')
							t_StabilizingBottom_max = 2

				elif(self.sub9 == "GoneToBottom"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoneToBottom_max):
							self.sub9 = "GoingToBottom"
							t_activated = False
							self.goneToBottom = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoingToBottom')
							t_GoneToBottom_max = 0.1


				# Actuation if
				if(self.sub9 == "GoingToBottom"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = -0.75
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub9 == "StabilizingBottom"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub9 == "GoneToBottom"):
					self.goneToBottom = True
			else:
				if(self.sub9 == "GoingToBottom"):
					t_GoingToBottom_max = 3 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
					self.sub9 = self.StatesSub9[ghostStateIndex]
				elif(self.sub9 == "StabilizingBottom"):
					t_StabilizingBottom_max = 2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
					self.sub9 = self.StatesSub9[ghostStateIndex]
				elif(self.sub9 == "GoneToBottom"):
					t_GoneToBottom_max = 0.1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
					self.sub9 = self.StatesSub9[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata10(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_GoingToRigth_max = 3
		t_StabilizingRigth_max = 2
		t_GoneToRigth_max = 0.1


		while(run):
			totala = time.time() * 1000000

			if(self.sub5 == "DoingBottomSide"):
				if ((self.sub10 == "GoingToRigth_ghost") or (self.sub10 == "StabilizingRigth_ghost") or (self.sub10 == "GoneToRigth_ghost")):
					ghostStateIndex = self.StatesSub10.index(self.sub10)
					self.sub10 = self.StatesSub10[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub10 == "GoingToRigth"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoingToRigth_max):
							self.sub10 = "StabilizingRigth"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('StabilizingRigth')
							t_GoingToRigth_max = 3

				elif(self.sub10 == "StabilizingRigth"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_StabilizingRigth_max):
							self.sub10 = "GoneToRigth"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoneToRigth')
							t_StabilizingRigth_max = 2

				elif(self.sub10 == "GoneToRigth"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_GoneToRigth_max):
							self.sub10 = "GoingToRigth"
							t_activated = False
							self.goneToRigth = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('GoingToRigth')
							t_GoneToRigth_max = 0.1


				# Actuation if
				if(self.sub10 == "GoingToRigth"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0
					cmd.linearY = -0.75
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub10 == "StabilizingRigth"):
					cmd = jderobot.CMDVelData()
					cmd.linearX = 0
					cmd.linearY = 0
					cmd.linearZ = 0
					self.lock.acquire()
					self.cmdVelPrx.setCMDVelData(cmd)
					self.lock.release()
				elif(self.sub10 == "GoneToRigth"):
					self.goneToRigth = True
			else:
				if(self.sub10 == "GoingToRigth"):
					t_GoingToRigth_max = 3 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
					self.sub10 = self.StatesSub10[ghostStateIndex]
				elif(self.sub10 == "StabilizingRigth"):
					t_StabilizingRigth_max = 2 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
					self.sub10 = self.StatesSub10[ghostStateIndex]
				elif(self.sub10 == "GoneToRigth"):
					t_GoneToRigth_max = 0.1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
					self.sub10 = self.StatesSub10[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to pose3d
		pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not pose3d):
			raise Exception('could not create proxy with pose3d')
		self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
		if(not self.pose3dPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'pose3d connected'

		# Contact to cmdVel
		cmdVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not cmdVel):
			raise Exception('could not create proxy with cmdVel')
		self.cmdVelPrx = CMDVelPrx.checkedCast(cmdVel)
		if(not self.cmdVelPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'cmdVel connected'

		# Contact to extra
		extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not extra):
			raise Exception('could not create proxy with extra')
		self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
		if(not self.extraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'extra connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t2.join()
		self.t3.join()
		self.t4.join()
		self.t5.join()
		self.t6.join()
		self.t8.join()
		self.t9.join()
		self.t10.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#4
0
文件: colors.py 项目: reysam93/TFG
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"TakeOff",
			"FollowGreenLookBlue",
			"FollowBlueLookGreen",
			"FollowGreenLookRed",
			"FollowRed",
		]

		self.StatesSub3 = [
			"WaitGreen",
			"WaitGreen_ghost",
			"FollowGreen",
			"FollowGreen_ghost",
		]

		self.StatesSub4 = [
			"WaitBlue",
			"WaitBlue_ghost",
			"FollowBlue",
			"FollowBlue_ghost",
		]

		self.StatesSub5 = [
			"WaitGreen2",
			"WaitGreen2_ghost",
			"FollowGreen2",
			"FollowGreen2_ghost",
		]

		self.StatesSub6 = [
			"WaitRed",
			"WaitRed_ghost",
			"FollowingRed",
			"FollowingRed_ghost",
		]

		self.sub1 = "TakeOff"
		self.run1 = True
		self.sub3 = "WaitGreen_ghost"
		self.run3 = True
		self.sub4 = "WaitBlue_ghost"
		self.run4 = True
		self.sub5 = "WaitGreen2_ghost"
		self.run5 = True
		self.sub6 = "WaitRed_ghost"
		self.run6 = True

	def getImage(self):
		img = self.CameraPrx.getImageData("RGB8")
		height = img.description.height
		width=img.description.width
		
		if self.targetX == None and self.targetY == None:
			self.targetX = width/2
			self.targetY = height/2
			print "targetX:", self.targetX, "targetY:,", self.targetY
	
		imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
		imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
		imgPixels.shape = height, width, 3
		return imgPixels
	
	
	def getContours(self, img, minValues, maxValues):
		hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
		thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
		contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
		return contours, hierarchy	
	
	
	def getVelocities(self, contours):
		contour = max(contours, key=cv2.contourArea)
		(x, y), radius = cv2.minEnclosingCircle(contour)
		xError = self.targetX - x
		yError = self.targetY - y
	
		xVel = self.xPID.process(xError)
		yVel = self.yPID.process(yError)
		return xVel, yVel
	
	
	def setVelocity(self, vx, vy, vz, yaw):
		cmd = jderobot.CMDVelData()
		cmd.linearX = vy
		cmd.linearY = vx
		cmd.linearZ = vz
		cmd.angularZ = yaw
		cmd.angularX = cmd.angularY = 1.0
		self.CMDVelPrx.setCMDVelData(cmd)
	
	
	class PID:
			def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
				self.Ep = Epsilon
				self.Kp = Kp
				self.Ki = Ki
				self.Kd = Kd
		
				self.Imax = Imax
				self.Imin = Imin
				self.lastError = 0
				self.cumulativeError = 0
		
			def updateCumulativeError(self, error):
				self.cumulativeError += error
				if self.cumulativeError > self.Imax:
					self.cumulativeError = self.Imax
				elif self.cumulativeError < self.Imin:
					self.cumulativeError = self.Imin
		
			def process(self, error, derivative=None, integral=None):
				if -self.Ep < error < self.Ep:
					return 0
		
				P = self.Kp * error
				self.updateCumulativeError(error)
				if integral != None:
					I = self.Ki * integral
				else:
					I = self.Ki * self.cumulativeError
	
				if derivative != None:
					D = self.Kd * derivative
				else:
					D = self.Kd * (error - self.lastError)
				self.lastError = error
				speed = P + I + D
	
				#if speed > 0.4:
				#	speed = 0.4
				#if speed < -0.4:
				#	speed = -0.4
	
				return speed
	
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()
		self.t4 = threading.Thread(target=self.subautomata4)
		self.t4.start()
		self.t5 = threading.Thread(target=self.subautomata5)
		self.t5.start()
		self.t6 = threading.Thread(target=self.subautomata6)
		self.t6.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 117, 139, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(2, 3, 303, 150, 0, 'FollowGreenLookBlue')
		guiSubautomata1.newGuiNode(5, 4, 421, 244, 0, 'FollowBlueLookGreen')
		guiSubautomata1.newGuiNode(6, 5, 325, 312, 0, 'FollowGreenLookRed')
		guiSubautomata1.newGuiNode(7, 6, 142, 326, 0, 'FollowRed')

		guiSubautomata1.newGuiTransition((303, 150), (421, 244), (421, 157), 4, 2, 5)
		guiSubautomata1.newGuiTransition((421, 244), (325, 312), (436, 325), 5, 5, 6)
		guiSubautomata1.newGuiTransition((325, 312), (142, 326), (232, 331), 6, 6, 7)
		guiSubautomata1.newGuiTransition((117, 139), (303, 150), (194, 174), 15, 1, 2)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)

		guiSubautomata3.newGuiNode(3, 0, 67, 147, 1, 'WaitGreen')
		guiSubautomata3.newGuiNode(4, 0, 269, 152, 0, 'FollowGreen')

		guiSubautomata3.newGuiTransition((67, 147), (269, 152), (173, 40), 2, 3, 4)
		guiSubautomata3.newGuiTransition((269, 152), (67, 147), (170, 221), 3, 4, 3)
		guiSubautomataList.append(guiSubautomata3)

		# Creating subAutomata4
		guiSubautomata4 = GuiSubautomata(4,5, self.automataGui)

		guiSubautomata4.newGuiNode(8, 0, 111, 129, 1, 'WaitBlue')
		guiSubautomata4.newGuiNode(9, 0, 296, 141, 0, 'FollowBlue')

		guiSubautomata4.newGuiTransition((111, 129), (296, 141), (200, 60), 7, 8, 9)
		guiSubautomata4.newGuiTransition((296, 141), (111, 129), (191, 198), 8, 9, 8)
		guiSubautomataList.append(guiSubautomata4)

		# Creating subAutomata5
		guiSubautomata5 = GuiSubautomata(5,6, self.automataGui)

		guiSubautomata5.newGuiNode(10, 0, 79, 120, 1, 'WaitGreen2')
		guiSubautomata5.newGuiNode(11, 0, 336, 152, 0, 'FollowGreen2')

		guiSubautomata5.newGuiTransition((336, 152), (79, 120), (196, 220), 9, 11, 10)
		guiSubautomata5.newGuiTransition((79, 120), (336, 152), (217, 56), 10, 10, 11)
		guiSubautomataList.append(guiSubautomata5)

		# Creating subAutomata6
		guiSubautomata6 = GuiSubautomata(6,7, self.automataGui)

		guiSubautomata6.newGuiNode(12, 0, 104, 151, 1, 'WaitRed')
		guiSubautomata6.newGuiNode(13, 0, 341, 163, 0, 'FollowingRed')

		guiSubautomata6.newGuiTransition((104, 151), (341, 163), (221, 56), 11, 12, 13)
		guiSubautomata6.newGuiTransition((341, 163), (104, 151), (225, 219), 12, 13, 12)
		guiSubautomataList.append(guiSubautomata6)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run3 = False
		self.run4 = False
		self.run5 = False
		self.run6 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		#ControlValues
		minError = 10
		self.targetX = None
		self.targetY = None
		self.hIters = 0
		#maxHIters = 35	#For gazebo
		maxHIters = 20	#For real
		refracIters = 0
		refracTime = 35
		
		#Contours
		self.greenConts = []
		self.blueConts = []
		self.redConts = []
		
		#Filter values for simulator. Order: [H, S, V] 
		minGValues = numpy.array([30,  129, 33])
		maxGValues = numpy.array([70, 255 , 190])
		minBValues = numpy.array([0, 140, 37])
		maxBValues = numpy.array([16, 255, 200])
		minRValues = numpy.array([100, 209, 82])
		maxRValues =  numpy.array([153, 255, 200])
		
		#Filter values for real. Order: [H, S, V] 
		#minGValues = numpy.array([31,0,120])
		#maxGValues = numpy.array([70, 46 , 255])
		#minBValues = numpy.array([9,126,175])
		#maxBValues = numpy.array([58,209,255])
		#minRValues = numpy.array([93,195,154])
		#maxRValues =  numpy.array([155,255,255])
		
		#PID control
		self.xPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
		self.yPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
		

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(self.hIters >= maxHIters):
					self.sub1 = "FollowGreenLookBlue"
					self.setVelocity(0,0,0,0)
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('FollowGreenLookBlue')

			elif(self.sub1 == "FollowGreenLookBlue"):
				if(self.blueConts):
					self.sub1 = "FollowBlueLookGreen"
					print "Reseting Green Conts"
					self.greenConts = []
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('FollowBlueLookGreen')

			elif(self.sub1 == "FollowBlueLookGreen"):
				if(self.greenConts):
					self.sub1 = "FollowGreenLookRed"
					self.blueConts = []
					refracIters = 0
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('FollowGreenLookRed')

			elif(self.sub1 == "FollowGreenLookRed"):
				if(self.redConts):
					self.sub1 = "FollowRed"
					self.greenConts = []
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('FollowRed')


			# Actuation if
			if(self.sub1 == "TakeOff"):
				if self.hIters == 0:
					self.ExtraPrx.takeoff()
					print "taking off"
				else:
					#self.setVelocity(0,0,0.5,0)
					self.setVelocity(0,0,1,0)
				
				self.hIters += 1
				print "ITERS: ", self.hIters
			elif(self.sub1 == "FollowGreenLookBlue"):
				inImg = self.getImage()
				softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
				self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
				self.blueConts,  hierarchy = self.getContours(softenedImg, minBValues, maxBValues) 
				
								
			elif(self.sub1 == "FollowBlueLookGreen"):
				inImg = self.getImage()
				softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
				self.blueConts,  hierarchy = self.getContours(softenedImg, minBValues, maxBValues) 
				
				if refracIters >= refracTime:
					self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
					print "inside refrac time"
				else:
					print "refrac Iters:", refracIters
				
				refracIters += 1
			elif(self.sub1 == "FollowGreenLookRed"):
				inImg = self.getImage()
				softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
				self.greenConts, hierarchy = self.getContours(softenedImg, minGValues, maxGValues)
				self.redConts,  hierarchy = self.getContours(softenedImg, minRValues, maxRValues)
				print "RED:", self.redConts
			elif(self.sub1 == "FollowRed"):
				inImg = self.getImage()
				softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
				self.redConts, hierarchy = self.getContours(softenedImg, minRValues, maxRValues)

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		self.run3 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run3):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowGreenLookBlue"):
				if ((self.sub3 == "WaitGreen_ghost") or (self.sub3 == "FollowGreen_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "WaitGreen"):
					if(self.greenConts):
						self.sub3 = "FollowGreen"
						print "Green Finded"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowGreen')

				elif(self.sub3 == "FollowGreen"):
					if(not self.greenConts):
						self.sub3 = "WaitGreen"
						print "Green Lost"
						self.setVelocity(0, 0, 0, 0)
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('WaitGreen')


				# Actuation if
				if(self.sub3 == "WaitGreen"):
					self.setVelocity(0, 0, 0, 0)
				elif(self.sub3 == "FollowGreen"):
					xVel, yVel = self.getVelocities(self.greenConts)
					self.setVelocity(xVel, yVel, 0, 0)
			else:
				if(self.sub3 == "WaitGreen"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "FollowGreen"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata4(self):
		self.run4 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run4):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowBlueLookGreen"):
				if ((self.sub4 == "WaitBlue_ghost") or (self.sub4 == "FollowBlue_ghost")):
					ghostStateIndex = self.StatesSub4.index(self.sub4)
					self.sub4 = self.StatesSub4[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub4 == "WaitBlue"):
					if(self.blueConts):
						self.sub4 = "FollowBlue"
						print "Blue Finded"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowBlue')

				elif(self.sub4 == "FollowBlue"):
					if(not self.blueConts):
						self.sub4 = "WaitBlue"
						print "Blue Lost"
						self.setVelocity(0, 0, 0, 0)
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('WaitBlue')


				# Actuation if
				if(self.sub4 == "WaitBlue"):
					self.setVelocity(0, 0, 0, 0)
				elif(self.sub4 == "FollowBlue"):
					xVel, yVel = self.getVelocities(self.blueConts)
					self.setVelocity(xVel, yVel, 0, 0)
			else:
				if(self.sub4 == "WaitBlue"):
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]
				elif(self.sub4 == "FollowBlue"):
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata5(self):
		self.run5 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run5):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowGreenLookRed"):
				if ((self.sub5 == "WaitGreen2_ghost") or (self.sub5 == "FollowGreen2_ghost")):
					ghostStateIndex = self.StatesSub5.index(self.sub5)
					self.sub5 = self.StatesSub5[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub5 == "WaitGreen2"):
					if(self.greenConts):
						self.sub5 = "FollowGreen2"
						print "Green2 finded"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowGreen2')

				elif(self.sub5 == "FollowGreen2"):
					if(not self.greenConts):
						self.sub5 = "WaitGreen2"
						print "Green2 Lost"
						self.setVelocity(0, 0, 0, 0)
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('WaitGreen2')


				# Actuation if
				if(self.sub5 == "WaitGreen2"):
					self.setVelocity(0, 0, 0, 0)
				elif(self.sub5 == "FollowGreen2"):
					xVel, yVel = self.getVelocities(self.greenConts)
					self.setVelocity(xVel, yVel, 0, 0)
			else:
				if(self.sub5 == "WaitGreen2"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "FollowGreen2"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata6(self):
		self.run6 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run6):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowRed"):
				if ((self.sub6 == "WaitRed_ghost") or (self.sub6 == "FollowingRed_ghost")):
					ghostStateIndex = self.StatesSub6.index(self.sub6)
					self.sub6 = self.StatesSub6[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub6 == "WaitRed"):
					if(self.redConts):
						self.sub6 = "FollowingRed"
						print "Red FInded"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowingRed')

				elif(self.sub6 == "FollowingRed"):
					if(not self.redConts):
						self.sub6 = "WaitRed"
						print "Red Lost"
						self.setVelocity(0, 0, 0, 0)
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('WaitRed')


				# Actuation if
				if(self.sub6 == "WaitRed"):
					self.setVelocity(0, 0, 0, 0)
				elif(self.sub6 == "FollowingRed"):
					xVel, yVel = self.getVelocities(self.redConts)
					self.setVelocity(xVel, yVel, 0, 0)
			else:
				if(self.sub6 == "WaitRed"):
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]
				elif(self.sub6 == "FollowingRed"):
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to Extra
		Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not Extra):
			raise Exception('could not create proxy with Extra')
		self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
		if(not self.ExtraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'Extra connected'

		# Contact to CMDVel
		CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not CMDVel):
			raise Exception('could not create proxy with CMDVel')
		self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
		if(not self.CMDVelPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'CMDVel connected'

		# Contact to Camera
		Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
		if(not Camera):
			raise Exception('could not create proxy with Camera')
		self.CameraPrx = CameraPrx.checkedCast(Camera)
		if(not self.CameraPrx):
			raise Exception('invalid proxy automata.Camera.Proxy')
		print 'Camera connected'

		# Contact to Pose3D
		Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not Pose3D):
			raise Exception('could not create proxy with Pose3D')
		self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
		if(not self.Pose3DPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'Pose3D connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t3.join()
		self.t4.join()
		self.t5.join()
		self.t6.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#5
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "goforward",
        ]

        self.sub1 = "goforward"
        self.run1 = True

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 106, 166, 1, 'goforward')

        guiSubautomataList.append(guiSubautomata1)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if

            # Actuation if
            if (self.sub1 == "goforward"):
                self.my_motors.sendV(0.2)

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = EasyIce.initialize(sys.argv)
        self.ic, self.node = comm.init(self.ic)

        # Contact to my_motors
        self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
        if (not self.my_motors):
            raise Exception('could not create client with my_motors')
        print('my_motors connected')

    def destroyIc(self):
        self.my_motors.stop()
        comm.destroy(self.ic, self.node)

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print('runtime gui enabled')
                else:
                    self.displayGui = False
                    print('runtime gui disabled')
示例#6
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "GoStraight",
            "TurnRight",
            "MoveLeft",
            "MoveRight",
        ]

        self.sub1 = "GoStraight"
        self.run1 = True

    def get_min_distance(self):
        laser_data = self.laser_sensor.getLaserData()
        min_dist = 100000
        for i in range(laser_data.numLaser):
            if i < 5:
                continue
            avg_dist = 0
            for j in range(5):
                avg_dist += laser_data.distanceData[i - j]
            avg_dist = avg_dist / 5.0
            if avg_dist < min_dist:
                min_dist = avg_dist

        print('min_dist:' + str(min_dist))
        return min_dist

    def get_left_distance(self):
        laser_data = self.laser_sensor.getLaserData()
        avg_dist = 0
        for i in range(10):
            avg_dist += laser_data.distanceData[laser_data.numLaser - i]
        avg_dist = avg_dist / 10.0

        print('avg_dist:' + str(avg_dist))
        return avg_dist

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 107, 124, 1, 'GoStraight')
        guiSubautomata1.newGuiNode(2, 0, 300, 128, 0, 'TurnRight')
        guiSubautomata1.newGuiNode(3, 0, 457, 131, 0, 'MoveLeft')
        guiSubautomata1.newGuiNode(4, 0, 464, 290, 0, 'MoveRight')

        guiSubautomata1.newGuiTransition((107, 124), (300, 128), (190, 59), 1,
                                         1, 2)
        guiSubautomata1.newGuiTransition((300, 128), (457, 131), (369, 59), 2,
                                         2, 3)
        guiSubautomata1.newGuiTransition((457, 131), (464, 290), (553, 205), 4,
                                         3, 4)
        guiSubautomata1.newGuiTransition((464, 290), (457, 131), (390, 209), 5,
                                         4, 3)
        guiSubautomataList.append(guiSubautomata1)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "GoStraight"):
                if (self.get_min_distance() < 700):
                    self.sub1 = "TurnRight"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('TurnRight')

            elif (self.sub1 == "TurnRight"):
                if (self.get_left_distance() < 750):
                    self.sub1 = "MoveLeft"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('MoveLeft')

            elif (self.sub1 == "MoveLeft"):
                if (self.get_left_distance() < 700):
                    self.sub1 = "MoveRight"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('MoveRight')

            elif (self.sub1 == "MoveRight"):
                if (self.get_left_distance() > 700):
                    self.sub1 = "MoveLeft"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('MoveLeft')

            # Actuation if
            if (self.sub1 == "GoStraight"):
                self.my_motors.setV(0.2)
                self.my_motors.setW(0.0)
            elif (self.sub1 == "TurnRight"):
                self.my_motors.setV(0.0)
                self.my_motors.setW(-0.2)
            elif (self.sub1 == "MoveLeft"):
                self.my_motors.setV(0.15)
                self.my_motors.setW(0.15)
            elif (self.sub1 == "MoveRight"):
                self.my_motors.setV(0.15)
                self.my_motors.setW(-0.15)

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = EasyIce.initialize(sys.argv)

        # Contact to laser_sensor
        laser_sensor = self.ic.propertyToProxy('automata.laser_sensor.Proxy')
        if (not laser_sensor):
            raise Exception('could not create proxy with laser_sensor')
        self.laser_sensor = LaserPrx.checkedCast(laser_sensor)
        if (not self.laser_sensor):
            raise Exception('invalid proxy automata.laser_sensor.Proxy')
        print('laser_sensor connected')

        # Contact to my_motors
        my_motors = self.ic.propertyToProxy('automata.my_motors.Proxy')
        if (not my_motors):
            raise Exception('could not create proxy with my_motors')
        self.my_motors = MotorsPrx.checkedCast(my_motors)
        if (not self.my_motors):
            raise Exception('invalid proxy automata.my_motors.Proxy')
        print('my_motors connected')

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print('runtime gui enabled')
                else:
                    self.displayGui = False
                    print('runtime gui disabled')
示例#7
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.StatesSub1 = [
            "TakeOff",
            "Stabilizing1",
            "GoFront",
            "Stoping",
            "Landing",
            "Stabilizing2",
        ]

        self.StatesSub3 = [
            "A",
            "A_ghost",
            "B",
            "B_ghost",
        ]

        self.StatesSub5 = [
            "C",
            "C_ghost",
        ]

        self.sub1 = "TakeOff"
        self.run1 = True
        self.sub3 = "A_ghost"
        self.run3 = True
        self.sub5 = "C_ghost"
        self.run5 = True

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 3, 61, 101, 1, 'TakeOff')
        guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
        guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
        guiSubautomata1.newGuiNode(4, 0, 489, 320, 0, 'Stoping')
        guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
        guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')

        guiSubautomata1.newGuiTransition((61, 101), (283, 75), (9, 212), 1, 1,
                                         2)
        guiSubautomata1.newGuiTransition((283, 75), (497, 130), (413, 79), 2,
                                         2, 3)
        guiSubautomata1.newGuiTransition((497, 130), (489, 320), (570, 239), 3,
                                         3, 4)
        guiSubautomata1.newGuiTransition((489, 320), (250, 408), (410, 411), 4,
                                         4, 5)
        guiSubautomata1.newGuiTransition((250, 408), (66, 283), (152, 338), 6,
                                         5, 6)
        guiSubautomata1.newGuiTransition((66, 283), (61, 101), (9, 212), 1, 6,
                                         1)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 1, self.automataGui)

        guiSubautomata3.newGuiNode(7, 0, 175, 146, 1, 'A')
        guiSubautomata3.newGuiNode(8, 5, 468, 201, 0, 'B')

        guiSubautomata3.newGuiTransition((175, 146), (468, 201), (348, 79), 1,
                                         7, 8)
        guiSubautomata3.newGuiTransition((468, 201), (175, 146), (303, 215), 2,
                                         8, 7)
        guiSubautomataList.append(guiSubautomata3)

        # Creating subAutomata5
        guiSubautomata5 = GuiSubautomata(5, 3, self.automataGui)

        guiSubautomata5.newGuiNode(9, 0, 70, 146, 1, 'C')

        guiSubautomata5.newGuiTransition((70, 146), (70, 146), (70, 186), 3, 9,
                                         9)
        guiSubautomataList.append(guiSubautomata5)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run3 = False
        self.run5 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        run = True
        cycle = 100
        t_activated = False

        hasTakenOff = False
        hasLanded = False

        while (run):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "TakeOff"):
                if (hasTakenOff):
                    self.sub1 = "Stabilizing1"
                    print "Going to Stabilizing"
                self.automataGui.notifySetNodeAsActive('Stabilizing1')

            elif (self.sub1 == "Stabilizing1"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 1.5):
                        self.sub1 = "GoFront"
                        t_activated = False
                        print "going to GoFront"
                        self.automataGui.notifySetNodeAsActive('GoFront')

            elif (self.sub1 == "GoFront"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3):
                        self.sub1 = "Stoping"
                        t_activated = False
                        print "going to Stoping"
                        self.automataGui.notifySetNodeAsActive('Stoping')

            elif (self.sub1 == "Stoping"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3.5):
                        self.sub1 = "Landing"
                        t_activated = False
                        print "going to Land"
                        self.automataGui.notifySetNodeAsActive('Landing')

            elif (self.sub1 == "Landing"):
                if (hasLanded):
                    self.sub1 = "Stabilizing2"

                self.automataGui.notifySetNodeAsActive('Stabilizing2')

            elif (self.sub1 == "Stabilizing2"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3):
                        self.sub1 = "TakeOff"
                        t_activated = False
                        print "Restarting"
                        self.automataGui.notifySetNodeAsActive('TakeOff')

            # Actuation if
            if (self.sub1 == "TakeOff"):
                print "Taking Off"
                self.lock.acquire()
                self.extraPrx.takeoff()
                self.lock.release()
                hasTakenOff = True
            elif (self.sub1 == "Stabilizing1"):
                print "Stabilizing"
            elif (self.sub1 == "GoFront"):
                cmd = jderobot.CMDVelData()
                cmd.linearX = 1
                cmd.linearY = 0
                cmd.linearZ = 0
                self.lock.acquire()
                self.cmdPrx.setCMDVelData(cmd)
                self.lock.release()
            elif (self.sub1 == "Stoping"):
                cmd = jderobot.CMDVelData()
                cmd.linearX = 0
                cmd.linearY = 0
                cmd.linearZ = 0
                self.lock.acquire()
                self.cmdPrx.setCMDVelData(cmd)
                self.lock.release()
                print "Stoping"
            elif (self.sub1 == "Landing"):
                print "Landing"
                self.lock.acquire()
                self.extraPrx.land()
                self.lock.release()
                hasLanded = True
            elif (self.sub1 == "Stabilizing2"):
                finished()

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_A_max = 0.023
        t_B_max = 0.055

        while (run):
            totala = time.time() * 1000000

            if (self.sub1 == "TakeOff"):
                if ((self.sub3 == "A_ghost") or (self.sub3 == "B_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "A"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_A_max):
                            self.sub3 = "B"
                            t_activated = False
                            self.automataGui.notifySetNodeAsActive('B')
                            t_A_max = 0.023

                elif (self.sub3 == "B"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_B_max):
                            self.sub3 = "A"
                            t_activated = False
                            self.automataGui.notifySetNodeAsActive('A')
                            t_B_max = 0.055

                # Actuation if
                if (self.sub3 == "A"):
                    print "A"
                elif (self.sub3 == "B"):
                    print "B"
            else:
                if (self.sub3 == "A"):
                    t_A_max = 0.023 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "B"):
                    t_B_max = 0.055 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata5(self):
        run = True
        cycle = 100
        t_activated = False

        t_C_max = 0.567

        while (run):
            totala = time.time() * 1000000

            if (self.sub3 == "B"):
                if ((self.sub5 == "C_ghost")):
                    ghostStateIndex = self.StatesSub5.index(self.sub5)
                    self.sub5 = self.StatesSub5[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub5 == "C"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_C_max):
                            self.sub5 = "C"
                            t_activated = False
                            self.automataGui.notifySetNodeAsActive('C')
                            t_C_max = 0.567

                # Actuation if
                if (self.sub5 == "C"):
                    print "C"
            else:
                if (self.sub5 == "C"):
                    t_C_max = 0.567 - (t_fin - t_ini)
                    ghostStateIndex = self.StateSub5.index(self.sub5) + 1
                    sub5 = self.StatesSub5[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to camera
        camera = self.ic.propertyToProxy('automata.Camera.Proxy')
        if (not camera):
            raise Exception('could not create proxy with camera')
        self.cameraPrx = CameraPrx.checkedCast(camera)
        if (not self.cameraPrx):
            raise Exception('invalid proxy automata.Camera.Proxy')
        print 'camera connected'

        # Contact to pose3d
        pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
        if (not pose3d):
            raise Exception('could not create proxy with pose3d')
        self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
        if (not self.pose3dPrx):
            raise Exception('invalid proxy automata.Pose3D.Proxy')
        print 'pose3d connected'

        # Contact to cmd
        cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
        if (not cmd):
            raise Exception('could not create proxy with cmd')
        self.cmdPrx = CMDVelPrx.checkedCast(cmd)
        if (not self.cmdPrx):
            raise Exception('invalid proxy automata.CMDVel.Proxy')
        print 'cmd connected'

        # Contact to extra
        extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
        if (not extra):
            raise Exception('could not create proxy with extra')
        self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
        if (not self.extraPrx):
            raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
        print 'extra connected'

        # Contact to navdata
        navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
        if (not navdata):
            raise Exception('could not create proxy with navdata')
        self.navdataPrx = NavdataPrx.checkedCast(navdata)
        if (not self.navdataPrx):
            raise Exception('invalid proxy automata.Navdata.Proxy')
        print 'navdata connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        self.guiThread = threading.Thread(target=self.runGui)
        self.guiThread.start()
        time.sleep(1)

        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()
        self.t5 = threading.Thread(target=self.subautomata5)
        self.t5.start()

    def join(self):
        self.guiThread.join()
        self.t1.join()
        self.t3.join()
        self.t5.join()
示例#8
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"TakeOff",
			"FollowTurtle",
			"Land",
		]

		self.StatesSub2 = [
			"FindTurtle",
			"FindTurtle_ghost",
			"FollowTurtle",
			"FollowTurtle_ghost",
		]

		self.StatesSub3 = [
			"LoseTurtle",
			"LoseTurtle_ghost",
			"Landing",
			"Landing_ghost",
		]

		self.sub1 = "TakeOff"
		self.run1 = True
		self.sub2 = "FindTurtle_ghost"
		self.run2 = True
		self.sub3 = "LoseTurtle_ghost"
		self.run3 = True

	def setVelocity(self, vx, vy, vz, yaw):
		cmd = jderobot.CMDVelData()
		cmd.linearX = vy
		cmd.linearY = vx
		cmd.linearZ = vz
		cmd.angularZ = yaw
		cmd.angularX = cmd.angularY = 1.0
		self.CMDVelPrx.setCMDVelData(cmd)
	
	def getImage(self):
		img = self.CameraPrx.getImageData("RGB8")
		height = img.description.height
		width = img.description.width
		imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
		imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
		imgPixels.shape = height, width, 3
		return imgPixels
	
	def getContours(self):
		img = self.getImage()
		img = cv2.GaussianBlur(img, (5, 5), 0)
		hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
	
		maxValues = numpy.array(self.hmax, self.vmax, self.smax)
		minValues = numpy.array(self.hmin, self.vmin, self.smin)
		
		thresoldImg = cv2.inRange(hsvImg, minValues, maxValues)
		conts, hierarchy = cv2.findContours(thresoldImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
		return conts, hierarchy	
	
		
	class PID:
		def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
			self.Ep = Epsilon		
			self.Kp = Kp
			self.Ki = Ki
			self.Kd = Kd
			self.Imax = Imax
			self.Imin = Imin
			self.lastError = 0
			self.cumulativeError = 0
	
		def updateCumulativeError(self, error):
			self.cumulativeError += error
			if self.cumulativeError > self.Imax:
				self.cumulativeError = self.Imax
			if self.cumulativeError < self.Imin:
				self.cumulativeError = self.Imin
	
		def process(self, error):
			if -self.Ep < error < self.Ep:
				return 0
			P = self.Kp * error
	
			self.updateCumulativeError(error)
			I = self.Ki * self.cumulativeError
	
			D = self.Kd * (error -  self.lastError)
			self.lastError = error
	
			vel = P + I + D
			return vel
			
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t2 = threading.Thread(target=self.subautomata2)
		self.t2.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 111, 195, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(2, 2, 323, 206, 0, 'FollowTurtle')
		guiSubautomata1.newGuiNode(3, 3, 531, 229, 0, 'Land')

		guiSubautomata1.newGuiTransition((111, 195), (323, 206), (191, 244), 1, 1, 2)
		guiSubautomata1.newGuiTransition((323, 206), (531, 229), (419, 267), 2, 2, 3)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata2
		guiSubautomata2 = GuiSubautomata(2,2, self.automataGui)

		guiSubautomata2.newGuiNode(4, 0, 118, 156, 1, 'FindTurtle')
		guiSubautomata2.newGuiNode(5, 0, 410, 171, 0, 'FollowTurtle')

		guiSubautomata2.newGuiTransition((118, 156), (410, 171), (276, 101), 3, 4, 5)
		guiSubautomata2.newGuiTransition((410, 171), (118, 156), (258, 221), 4, 5, 4)
		guiSubautomataList.append(guiSubautomata2)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,3, self.automataGui)

		guiSubautomata3.newGuiNode(7, 0, 117, 188, 1, 'LoseTurtle')
		guiSubautomata3.newGuiNode(8, 0, 378, 213, 0, 'Landing')

		guiSubautomata3.newGuiTransition((378, 213), (117, 188), (271, 109), 6, 8, 7)
		guiSubautomata3.newGuiTransition((117, 188), (378, 213), (250, 258), 7, 7, 8)
		guiSubautomataList.append(guiSubautomata3)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run2 = False
		self.run3 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		self.heighIters = 0
		self.maxIters = 50
		self.contours = []
		
		
		#Filter Values
		self.hmin = 50
		self.hmax = 70
		self.vmin = 20
		self.vmax = 235
		self.smin = 10
		self.smax = 245

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2.5):
						self.sub1 = "FollowTurtle"
						t_activated = False
						print "Iters:", self.heighIters
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowTurtle')

			elif(self.sub1 == "FollowTurtle"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 90):
						self.sub1 = "Land"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Land')


			# Actuation if
			if(self.sub1 == "TakeOff"):
				self.ExtraPrx.takeoff()
				
				#Get some heigh
				self.setVelocity(0, 0, 1, 0)
				
				self.heighIters += 1
			elif(self.sub1 == "FollowTurtle"):
				self.contours, hierarchy = self.getContours()
			elif(self.sub1 == "Land"):
				self.contours, hierarchy = self.getContours()

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata2(self):
		self.run2 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		targetX = 125
		targetY = 125
		
		minError = 8
		
		xPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
		yPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)

		while(self.run2):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowTurtle"):
				if ((self.sub2 == "FindTurtle_ghost") or (self.sub2 == "FollowTurtle_ghost")):
					ghostStateIndex = self.StatesSub2.index(self.sub2)
					self.sub2 = self.StatesSub2[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub2 == "FindTurtle"):
					if(self.contours):
						self.sub2 = "FollowTurtle"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowTurtle')

				elif(self.sub2 == "FollowTurtle"):
					if(not self.contours):
						self.sub2 = "FindTurtle"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FindTurtle')


				# Actuation if
				if(self.sub2 == "FindTurtle"):
					if self.heighIters <= self.maxIters:
						self.setVelocity(0, 0, 1, 0)
						self.heighIters += 1
						print "iters:", self.heighIters
					else:
						print "max hight reached"
				elif(self.sub2 == "FollowTurtle"):
					#We assume there is only one green target on the screen
					cnt = self.contours[0]
					
					#Locate the minimal circle enclosing the contour
					(x, y), radius = cv2.minEnclosingCircle(cnt)
					center = (int(x), int(y))
					radius = int(radius)
					
					xError = targetX - center[0]
					yError = targetY - center[1]
					
					xVel = xPid.process(xError)
					yVel = yPidprocess(yError)
					
					#Control Heigh#
					
					self.setVelocity(xVel, yVel, 0, 0)
					print "xError:", xError, "yError:", yError
					print "xVel:", xVel, "yVel:", yVel
			else:
				if(self.sub2 == "FindTurtle"):
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]
				elif(self.sub2 == "FollowTurtle"):
					ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
					self.sub2 = self.StatesSub2[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		self.run3 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run3):
			totala = time.time() * 1000000

			if(self.sub1 == "Land"):
				if ((self.sub3 == "LoseTurtle_ghost") or (self.sub3 == "Landing_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "LoseTurtle"):
					if(not self.contours):
						self.sub3 = "Landing"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Landing')

				elif(self.sub3 == "Landing"):
					if(self.contours):
						self.sub3 = "LoseTurtle"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('LoseTurtle')


				# Actuation if
				if(self.sub3 == "LoseTurtle"):
					self.setVelocity(1, 0, 0, 0)
				elif(self.sub3 == "Landing"):
					self.ExtraPrx.land()
			else:
				if(self.sub3 == "LoseTurtle"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "Landing"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to Extra
		Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not Extra):
			raise Exception('could not create proxy with Extra')
		self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
		if(not self.ExtraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'Extra connected'

		# Contact to CMDVel
		CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not CMDVel):
			raise Exception('could not create proxy with CMDVel')
		self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
		if(not self.CMDVelPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'CMDVel connected'

		# Contact to Camera
		Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
		if(not Camera):
			raise Exception('could not create proxy with Camera')
		self.CameraPrx = CameraPrx.checkedCast(Camera)
		if(not self.CameraPrx):
			raise Exception('invalid proxy automata.Camera.Proxy')
		print 'Camera connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t2.join()
		self.t3.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#9
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "TakeOff",
            "FollowGreenLookBlue",
            "FollowBlueLookGreen",
            "FollowGreenLookRed",
            "FollowRed",
        ]

        self.StatesSub3 = [
            "WaitGreen",
            "WaitGreen_ghost",
            "FollowGreen",
            "FollowGreen_ghost",
        ]

        self.StatesSub4 = [
            "WaitBlue",
            "WaitBlue_ghost",
            "FollowBlue",
            "FollowBlue_ghost",
        ]

        self.StatesSub5 = [
            "WaitGreen2",
            "WaitGreen2_ghost",
            "FollowGreen2",
            "FollowGreen2_ghost",
        ]

        self.StatesSub6 = [
            "WaitRed",
            "WaitRed_ghost",
            "FollowingRed",
            "FollowingRed_ghost",
        ]

        self.sub1 = "TakeOff"
        self.run1 = True
        self.sub3 = "WaitGreen_ghost"
        self.run3 = True
        self.sub4 = "WaitBlue_ghost"
        self.run4 = True
        self.sub5 = "WaitGreen2_ghost"
        self.run5 = True
        self.sub6 = "WaitRed_ghost"
        self.run6 = True

    def getImage(self):
        img = self.CameraPrx.getImageData("RGB8")
        height = img.description.height
        width = img.description.width

        if self.targetX == None and self.targetY == None:
            self.targetX = width / 2
            self.targetY = height / 2
            print "targetX:", self.targetX, "targetY:,", self.targetY

        imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
        imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
        imgPixels.shape = height, width, 3
        return imgPixels

    def getContours(self, img, minValues, maxValues):
        hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)
        thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
        contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL,
                                               cv2.CHAIN_APPROX_SIMPLE)
        return contours, hierarchy

    def getVelocities(self, contours):
        contour = max(contours, key=cv2.contourArea)
        (x, y), radius = cv2.minEnclosingCircle(contour)
        xError = self.targetX - x
        yError = self.targetY - y

        xVel = self.xPID.process(xError)
        yVel = self.yPID.process(yError)
        return xVel, yVel

    def setVelocity(self, vx, vy, vz, yaw):
        cmd = jderobot.CMDVelData()
        cmd.linearX = vy
        cmd.linearY = vx
        cmd.linearZ = vz
        cmd.angularZ = yaw
        cmd.angularX = cmd.angularY = 1.0
        self.CMDVelPrx.setCMDVelData(cmd)

    class PID:
        def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
            self.Ep = Epsilon
            self.Kp = Kp
            self.Ki = Ki
            self.Kd = Kd

            self.Imax = Imax
            self.Imin = Imin
            self.lastError = 0
            self.cumulativeError = 0

        def updateCumulativeError(self, error):
            self.cumulativeError += error
            if self.cumulativeError > self.Imax:
                self.cumulativeError = self.Imax
            elif self.cumulativeError < self.Imin:
                self.cumulativeError = self.Imin

        def process(self, error, derivative=None, integral=None):
            if -self.Ep < error < self.Ep:
                return 0

            P = self.Kp * error
            self.updateCumulativeError(error)
            if integral != None:
                I = self.Ki * integral
            else:
                I = self.Ki * self.cumulativeError

            if derivative != None:
                D = self.Kd * derivative
            else:
                D = self.Kd * (error - self.lastError)
            self.lastError = error
            speed = P + I + D

            #if speed > 0.4:
            #	speed = 0.4
            #if speed < -0.4:
            #	speed = -0.4

            return speed

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()
        self.t4 = threading.Thread(target=self.subautomata4)
        self.t4.start()
        self.t5 = threading.Thread(target=self.subautomata5)
        self.t5.start()
        self.t6 = threading.Thread(target=self.subautomata6)
        self.t6.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 117, 139, 1, 'TakeOff')
        guiSubautomata1.newGuiNode(2, 3, 303, 150, 0, 'FollowGreenLookBlue')
        guiSubautomata1.newGuiNode(5, 4, 421, 244, 0, 'FollowBlueLookGreen')
        guiSubautomata1.newGuiNode(6, 5, 325, 312, 0, 'FollowGreenLookRed')
        guiSubautomata1.newGuiNode(7, 6, 142, 326, 0, 'FollowRed')

        guiSubautomata1.newGuiTransition((303, 150), (421, 244), (421, 157), 4,
                                         2, 5)
        guiSubautomata1.newGuiTransition((421, 244), (325, 312), (436, 325), 5,
                                         5, 6)
        guiSubautomata1.newGuiTransition((325, 312), (142, 326), (232, 331), 6,
                                         6, 7)
        guiSubautomata1.newGuiTransition((117, 139), (303, 150), (194, 174),
                                         15, 1, 2)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 2, self.automataGui)

        guiSubautomata3.newGuiNode(3, 0, 67, 147, 1, 'WaitGreen')
        guiSubautomata3.newGuiNode(4, 0, 269, 152, 0, 'FollowGreen')

        guiSubautomata3.newGuiTransition((67, 147), (269, 152), (173, 40), 2,
                                         3, 4)
        guiSubautomata3.newGuiTransition((269, 152), (67, 147), (170, 221), 3,
                                         4, 3)
        guiSubautomataList.append(guiSubautomata3)

        # Creating subAutomata4
        guiSubautomata4 = GuiSubautomata(4, 5, self.automataGui)

        guiSubautomata4.newGuiNode(8, 0, 111, 129, 1, 'WaitBlue')
        guiSubautomata4.newGuiNode(9, 0, 296, 141, 0, 'FollowBlue')

        guiSubautomata4.newGuiTransition((111, 129), (296, 141), (200, 60), 7,
                                         8, 9)
        guiSubautomata4.newGuiTransition((296, 141), (111, 129), (191, 198), 8,
                                         9, 8)
        guiSubautomataList.append(guiSubautomata4)

        # Creating subAutomata5
        guiSubautomata5 = GuiSubautomata(5, 6, self.automataGui)

        guiSubautomata5.newGuiNode(10, 0, 79, 120, 1, 'WaitGreen2')
        guiSubautomata5.newGuiNode(11, 0, 336, 152, 0, 'FollowGreen2')

        guiSubautomata5.newGuiTransition((336, 152), (79, 120), (196, 220), 9,
                                         11, 10)
        guiSubautomata5.newGuiTransition((79, 120), (336, 152), (217, 56), 10,
                                         10, 11)
        guiSubautomataList.append(guiSubautomata5)

        # Creating subAutomata6
        guiSubautomata6 = GuiSubautomata(6, 7, self.automataGui)

        guiSubautomata6.newGuiNode(12, 0, 104, 151, 1, 'WaitRed')
        guiSubautomata6.newGuiNode(13, 0, 341, 163, 0, 'FollowingRed')

        guiSubautomata6.newGuiTransition((104, 151), (341, 163), (221, 56), 11,
                                         12, 13)
        guiSubautomata6.newGuiTransition((341, 163), (104, 151), (225, 219),
                                         12, 13, 12)
        guiSubautomataList.append(guiSubautomata6)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run3 = False
        self.run4 = False
        self.run5 = False
        self.run6 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        #ControlValues
        minError = 10
        self.targetX = None
        self.targetY = None
        self.hIters = 0
        #maxHIters = 35	#For gazebo
        maxHIters = 20  #For real
        refracIters = 0
        refracTime = 35

        #Contours
        self.greenConts = []
        self.blueConts = []
        self.redConts = []

        #Filter values for simulator. Order: [H, S, V]
        minGValues = numpy.array([30, 129, 33])
        maxGValues = numpy.array([70, 255, 190])
        minBValues = numpy.array([0, 140, 37])
        maxBValues = numpy.array([16, 255, 200])
        minRValues = numpy.array([100, 209, 82])
        maxRValues = numpy.array([153, 255, 200])

        #Filter values for real. Order: [H, S, V]
        #minGValues = numpy.array([31,0,120])
        #maxGValues = numpy.array([70, 46 , 255])
        #minBValues = numpy.array([9,126,175])
        #maxBValues = numpy.array([58,209,255])
        #minRValues = numpy.array([93,195,154])
        #maxRValues =  numpy.array([155,255,255])

        #PID control
        self.xPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)
        self.yPID = self.PID(minError, 0.01, 0.01, 0.02, 5, -5)

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "TakeOff"):
                if (self.hIters >= maxHIters):
                    self.sub1 = "FollowGreenLookBlue"
                    self.setVelocity(0, 0, 0, 0)
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive(
                            'FollowGreenLookBlue')

            elif (self.sub1 == "FollowGreenLookBlue"):
                if (self.blueConts):
                    self.sub1 = "FollowBlueLookGreen"
                    print "Reseting Green Conts"
                    self.greenConts = []
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive(
                            'FollowBlueLookGreen')

            elif (self.sub1 == "FollowBlueLookGreen"):
                if (self.greenConts):
                    self.sub1 = "FollowGreenLookRed"
                    self.blueConts = []
                    refracIters = 0
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive(
                            'FollowGreenLookRed')

            elif (self.sub1 == "FollowGreenLookRed"):
                if (self.redConts):
                    self.sub1 = "FollowRed"
                    self.greenConts = []
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('FollowRed')

            # Actuation if
            if (self.sub1 == "TakeOff"):
                if self.hIters == 0:
                    self.ExtraPrx.takeoff()
                    print "taking off"
                else:
                    #self.setVelocity(0,0,0.5,0)
                    self.setVelocity(0, 0, 1, 0)

                self.hIters += 1
                print "ITERS: ", self.hIters
            elif (self.sub1 == "FollowGreenLookBlue"):
                inImg = self.getImage()
                softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
                self.greenConts, hierarchy = self.getContours(
                    softenedImg, minGValues, maxGValues)
                self.blueConts, hierarchy = self.getContours(
                    softenedImg, minBValues, maxBValues)

            elif (self.sub1 == "FollowBlueLookGreen"):
                inImg = self.getImage()
                softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
                self.blueConts, hierarchy = self.getContours(
                    softenedImg, minBValues, maxBValues)

                if refracIters >= refracTime:
                    self.greenConts, hierarchy = self.getContours(
                        softenedImg, minGValues, maxGValues)
                    print "inside refrac time"
                else:
                    print "refrac Iters:", refracIters

                refracIters += 1
            elif (self.sub1 == "FollowGreenLookRed"):
                inImg = self.getImage()
                softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
                self.greenConts, hierarchy = self.getContours(
                    softenedImg, minGValues, maxGValues)
                self.redConts, hierarchy = self.getContours(
                    softenedImg, minRValues, maxRValues)
                print "RED:", self.redConts
            elif (self.sub1 == "FollowRed"):
                inImg = self.getImage()
                softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
                self.redConts, hierarchy = self.getContours(
                    softenedImg, minRValues, maxRValues)

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        self.run3 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run3):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowGreenLookBlue"):
                if ((self.sub3 == "WaitGreen_ghost")
                        or (self.sub3 == "FollowGreen_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "WaitGreen"):
                    if (self.greenConts):
                        self.sub3 = "FollowGreen"
                        print "Green Finded"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowGreen')

                elif (self.sub3 == "FollowGreen"):
                    if (not self.greenConts):
                        self.sub3 = "WaitGreen"
                        print "Green Lost"
                        self.setVelocity(0, 0, 0, 0)
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('WaitGreen')

                # Actuation if
                if (self.sub3 == "WaitGreen"):
                    self.setVelocity(0, 0, 0, 0)
                elif (self.sub3 == "FollowGreen"):
                    xVel, yVel = self.getVelocities(self.greenConts)
                    self.setVelocity(xVel, yVel, 0, 0)
            else:
                if (self.sub3 == "WaitGreen"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "FollowGreen"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata4(self):
        self.run4 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run4):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowBlueLookGreen"):
                if ((self.sub4 == "WaitBlue_ghost")
                        or (self.sub4 == "FollowBlue_ghost")):
                    ghostStateIndex = self.StatesSub4.index(self.sub4)
                    self.sub4 = self.StatesSub4[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub4 == "WaitBlue"):
                    if (self.blueConts):
                        self.sub4 = "FollowBlue"
                        print "Blue Finded"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowBlue')

                elif (self.sub4 == "FollowBlue"):
                    if (not self.blueConts):
                        self.sub4 = "WaitBlue"
                        print "Blue Lost"
                        self.setVelocity(0, 0, 0, 0)
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('WaitBlue')

                # Actuation if
                if (self.sub4 == "WaitBlue"):
                    self.setVelocity(0, 0, 0, 0)
                elif (self.sub4 == "FollowBlue"):
                    xVel, yVel = self.getVelocities(self.blueConts)
                    self.setVelocity(xVel, yVel, 0, 0)
            else:
                if (self.sub4 == "WaitBlue"):
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]
                elif (self.sub4 == "FollowBlue"):
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata5(self):
        self.run5 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run5):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowGreenLookRed"):
                if ((self.sub5 == "WaitGreen2_ghost")
                        or (self.sub5 == "FollowGreen2_ghost")):
                    ghostStateIndex = self.StatesSub5.index(self.sub5)
                    self.sub5 = self.StatesSub5[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub5 == "WaitGreen2"):
                    if (self.greenConts):
                        self.sub5 = "FollowGreen2"
                        print "Green2 finded"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowGreen2')

                elif (self.sub5 == "FollowGreen2"):
                    if (not self.greenConts):
                        self.sub5 = "WaitGreen2"
                        print "Green2 Lost"
                        self.setVelocity(0, 0, 0, 0)
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'WaitGreen2')

                # Actuation if
                if (self.sub5 == "WaitGreen2"):
                    self.setVelocity(0, 0, 0, 0)
                elif (self.sub5 == "FollowGreen2"):
                    xVel, yVel = self.getVelocities(self.greenConts)
                    self.setVelocity(xVel, yVel, 0, 0)
            else:
                if (self.sub5 == "WaitGreen2"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "FollowGreen2"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata6(self):
        self.run6 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run6):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowRed"):
                if ((self.sub6 == "WaitRed_ghost")
                        or (self.sub6 == "FollowingRed_ghost")):
                    ghostStateIndex = self.StatesSub6.index(self.sub6)
                    self.sub6 = self.StatesSub6[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub6 == "WaitRed"):
                    if (self.redConts):
                        self.sub6 = "FollowingRed"
                        print "Red FInded"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowingRed')

                elif (self.sub6 == "FollowingRed"):
                    if (not self.redConts):
                        self.sub6 = "WaitRed"
                        print "Red Lost"
                        self.setVelocity(0, 0, 0, 0)
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('WaitRed')

                # Actuation if
                if (self.sub6 == "WaitRed"):
                    self.setVelocity(0, 0, 0, 0)
                elif (self.sub6 == "FollowingRed"):
                    xVel, yVel = self.getVelocities(self.redConts)
                    self.setVelocity(xVel, yVel, 0, 0)
            else:
                if (self.sub6 == "WaitRed"):
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]
                elif (self.sub6 == "FollowingRed"):
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to Extra
        Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
        if (not Extra):
            raise Exception('could not create proxy with Extra')
        self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
        if (not self.ExtraPrx):
            raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
        print 'Extra connected'

        # Contact to CMDVel
        CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
        if (not CMDVel):
            raise Exception('could not create proxy with CMDVel')
        self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
        if (not self.CMDVelPrx):
            raise Exception('invalid proxy automata.CMDVel.Proxy')
        print 'CMDVel connected'

        # Contact to Camera
        Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
        if (not Camera):
            raise Exception('could not create proxy with Camera')
        self.CameraPrx = CameraPrx.checkedCast(Camera)
        if (not self.CameraPrx):
            raise Exception('invalid proxy automata.Camera.Proxy')
        print 'Camera connected'

        # Contact to Pose3D
        Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
        if (not Pose3D):
            raise Exception('could not create proxy with Pose3D')
        self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
        if (not self.Pose3DPrx):
            raise Exception('invalid proxy automata.Pose3D.Proxy')
        print 'Pose3D connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()
        self.t3.join()
        self.t4.join()
        self.t5.join()
        self.t6.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'
示例#10
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"TakeOff",
			"FollowRoad",
			"MonitorArea",
			"TurnAround",
			"Landing",
			"END",
			"HeighControl",
		]

		self.StatesSub3 = [
			"FindRoad",
			"FindRoad_ghost",
			"FollowingRoad",
			"FollowingRoad_ghost",
		]

		self.StatesSub5 = [
			"ToFirstPos",
			"ToFirstPos_ghost",
			"ToSecondPos",
			"ToSecondPos_ghost",
			"ToThirdPos",
			"ToThirdPos_ghost",
			"Return",
			"Return_ghost",
		]

		self.StatesSub6 = [
			"Descending",
			"Descending_ghost",
			"Land",
			"Land_ghost",
		]

		self.sub1 = "TakeOff"
		self.run1 = True
		self.sub3 = "FindRoad_ghost"
		self.run3 = True
		self.sub5 = "ToFirstPos_ghost"
		self.run5 = True
		self.sub6 = "Descending_ghost"
		self.run6 = True

	def getImage(self):
		img = self.CameraPrx.getImageData("RGB8")
		height = img.description.height
		width=img.description.width
		imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
		imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
		imgPixels.shape = height, width, 3
		return imgPixels
	
	def setVelocity(self, vx, vy, vz, yaw):
		cmd = jderobot.CMDVelData()
		cmd.linearX = vy
		cmd.linearY = vx
		cmd.linearZ = vz
		cmd.angularZ = yaw
		cmd.angularX = cmd.angularY = 1.0
		self.CMDVelPrx.setCMDVelData(cmd)
	
	#The factor indicate the margin of the error multipliyin the error for this factor
	def droneInPosition(self, pos,factor=1 ):
		return (abs(pos[0] - self.xPos) < self.minDist*factor) and (abs(pos[1] - self.yPos) < self.minDist*factor)
	
	class PID:
		def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
			self.Ep = Epsilon
			self.Kp = Kp
			self.Ki = Ki
			self.Kd = Kd
	
			self.Imax = Imax
			self.Imin = Imin
			self.lastError = 0
			self.cumulativeError = 0
	
		def updateCumulativeError(self, error):
			self.cumulativeError += error
			if self.cumulativeError > self.Imax:
				self.cumulativeError = self.Imax
			elif self.cumulativeError < self.Imin:
				self.cumulativeError = self.Imin
	
		def process(self, error, derivative=None, integral=None):
			if -self.Ep < error < self.Ep:
				return 0
	
			P = self.Kp * error
			self.updateCumulativeError(error)
			if integral != None:
				I = self.Ki * integral
			else:
				I = self.Ki * self.cumulativeError
	
			if derivative != None:
				D = self.Kd * derivative
			else:
				D = self.Kd * (error - self.lastError)
			self.lastError = error
			speed = P + I + D
			if speed > 3:
				speed = 3
			elif speed < -3:
				speed = -3
			return speed
	
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()
		self.t5 = threading.Thread(target=self.subautomata5)
		self.t5.start()
		self.t6 = threading.Thread(target=self.subautomata6)
		self.t6.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 62, 66, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(2, 3, 189, 116, 0, 'FollowRoad')
		guiSubautomata1.newGuiNode(6, 5, 309, 268, 0, 'MonitorArea')
		guiSubautomata1.newGuiNode(11, 0, 263, 428, 0, 'TurnAround')
		guiSubautomata1.newGuiNode(12, 6, 53, 239, 0, 'Landing')
		guiSubautomata1.newGuiNode(14, 0, 41, 427, 0, 'END')
		guiSubautomata1.newGuiNode(17, 0, 481, 139, 0, 'HeighControl')

		guiSubautomata1.newGuiTransition((189, 116), (309, 268), (274, 172), 7, 2, 6)
		guiSubautomata1.newGuiTransition((309, 268), (263, 428), (349, 362), 13, 6, 11)
		guiSubautomata1.newGuiTransition((263, 428), (189, 116), (164, 318), 14, 11, 2)
		guiSubautomata1.newGuiTransition((189, 116), (53, 239), (82, 154), 19, 2, 12)
		guiSubautomata1.newGuiTransition((53, 239), (41, 427), (43, 326), 22, 12, 14)
		guiSubautomata1.newGuiTransition((481, 139), (189, 116), (328, 116), 26, 17, 2)
		guiSubautomata1.newGuiTransition((62, 66), (481, 139), (430, 27), 27, 1, 17)
		guiSubautomata1.newGuiTransition((189, 116), (481, 139), (315, 70), 28, 2, 17)
		guiSubautomata1.newGuiTransition((309, 268), (481, 139), (378, 195), 29, 6, 17)
		guiSubautomata1.newGuiTransition((481, 139), (309, 268), (412, 279), 30, 17, 6)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)

		guiSubautomata3.newGuiNode(3, 0, 156, 228, 1, 'FindRoad')
		guiSubautomata3.newGuiNode(4, 0, 427, 255, 0, 'FollowingRoad')

		guiSubautomata3.newGuiTransition((156, 228), (427, 255), (297, 157), 2, 3, 4)
		guiSubautomata3.newGuiTransition((427, 255), (156, 228), (265, 320), 3, 4, 3)
		guiSubautomataList.append(guiSubautomata3)

		# Creating subAutomata5
		guiSubautomata5 = GuiSubautomata(5,6, self.automataGui)

		guiSubautomata5.newGuiNode(7, 0, 86, 275, 1, 'ToFirstPos')
		guiSubautomata5.newGuiNode(8, 0, 247, 92, 0, 'ToSecondPos')
		guiSubautomata5.newGuiNode(9, 0, 491, 303, 0, 'ToThirdPos')
		guiSubautomata5.newGuiNode(10, 0, 281, 455, 0, 'Return')

		guiSubautomata5.newGuiTransition((86, 275), (247, 92), (166, 184), 9, 7, 8)
		guiSubautomata5.newGuiTransition((247, 92), (491, 303), (369, 198), 10, 8, 9)
		guiSubautomata5.newGuiTransition((491, 303), (281, 455), (386, 379), 11, 9, 10)
		guiSubautomata5.newGuiTransition((281, 455), (86, 275), (184, 365), 12, 10, 7)
		guiSubautomataList.append(guiSubautomata5)

		# Creating subAutomata6
		guiSubautomata6 = GuiSubautomata(6,12, self.automataGui)

		guiSubautomata6.newGuiNode(15, 0, 126, 185, 1, 'Descending')
		guiSubautomata6.newGuiNode(16, 0, 350, 190, 0, 'Land')

		guiSubautomata6.newGuiTransition((126, 185), (350, 190), (232, 220), 24, 15, 16)
		guiSubautomataList.append(guiSubautomata6)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run3 = False
		self.run5 = False
		self.run6 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		self.targetX = 125
		self.targetY = 125
		self.targetZ = 2.5
		self.initPos = (-1, -8.5)
		landingPose = (self.Pose3DPrx.getPose3DData().x, self.Pose3DPrx.getPose3DData().y)
		
		#Minimun errors
		self.minError = 10
		self.minActit = 0.5
		self.minAltit = 0.01
		self.minDist = 0.01
		heighMargin = 1
		
		#Control Variables
		self.heigh = 0
		self.contours = []
		self.monitorComplet = False
		self.xPos = landingPose[0]
		self.yPos = landingPose[1]
		initAngle = None
		self.hasLanded = False
		currentState = "FollowRoad"
		takenOff = False
		
		#Filter Values
		self.hmin = 90
		self.hmax = 97
		self.vmin = 0
		self.vmax = 50
		self.smin = 45
		self.smax = 80
		
		#Control PID
		self.xPid = self.PID(Epsilon=self.minError, Kp=0.01, Ki=0.04, Kd=0.003, Imax=5, Imin=-5)
		self.zPid = self.PID(Epsilon=self.minAltit, Kp=1, Ki=0.02, Kd=0, Imax=5, Imin=-5)
		self.aPid = self.PID(Epsilon=self.minActit, Kp=0.01, Ki=0.04, Kd=0.003, Imax=5, Imin=-5)

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(takenOff):
					self.sub1 = "HeighControl"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('HeighControl')

			elif(self.sub1 == "FollowRoad"):
				if((self.droneInPosition(self.initPos, 200)) and (not self.monitorComplet)):
					self.sub1 = "MonitorArea"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('MonitorArea')

				elif((self.droneInPosition(landingPose, 75)) and (self.monitorComplet)):
					self.sub1 = "Landing"
					self.setVelocity(0,0,0,0)
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('Landing')

				elif(abs(self.targetZ - self.heigh) > heighMargin):
					self.sub1 = "HeighControl"
					currentState = "FollowRoad"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('HeighControl')

			elif(self.sub1 == "MonitorArea"):
				if(self.monitorComplet):
					self.sub1 = "TurnAround"
					self.setVelocity(0, 0, 0, 0)
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('TurnAround')

				elif(abs(self.targetZ - self.heigh) > heighMargin):
					self.sub1 = "HeighControl"
					currentState = "MonitorArea"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('HeighControl')

			elif(self.sub1 == "TurnAround"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2.5):
						self.sub1 = "FollowRoad"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowRoad')

			elif(self.sub1 == "Landing"):
				if(self.hasLanded):
					self.sub1 = "END"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('END')

			elif(self.sub1 == "HeighControl"):
				if((abs(self.targetZ - self.heigh) < self.minAltit) and (currentState == "FollowRoad")):
					self.sub1 = "FollowRoad"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('FollowRoad')

				elif((abs(self.targetZ - self.heigh) < self.minAltit) and (currentState == "MonitorArea")):
					self.sub1 = "MonitorArea"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('MonitorArea')


			# Actuation if
			if(self.sub1 == "TakeOff"):
				#Taking Off
				self.ExtraPrx.takeoff()
				takenOff = True
			elif(self.sub1 == "FollowRoad"):
				lastState = "FollowRoad"
				
				#Get and prepare image
				inImg = self.getImage()
				softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
				hsvImg = cv2.cvtColor(softenedImg, cv2.COLOR_BGR2HSV)
				
				#Get threshold image
				minValues = numpy.array([self.hmin, self.vmin, self.smin])
				maxValues = numpy.array([self.hmax, self.vmax, self.smax])
				thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)
				
				#Get contours
				self.contours, hierarchy = cv2.findContours(thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
				
				#Update Heigh
				self.heigh = self.Pose3DPrx.getPose3DData().z
			elif(self.sub1 == "TurnAround"):
				self.setVelocity(0, 0, 0, 1)
				pose= self.Pose3DPrx.getPose3DData()
				print "q0", pose.q0, "q1",pose.q1, "q2",pose.q2, "q3",pose.q3
			elif(self.sub1 == "END"):
				print "SHUT DOWN"
				self.shutDown()
			elif(self.sub1 == "HeighControl"):
				#Controling heigh
				self.heigh = self.Pose3DPrx.getPose3DData().z
				zVel = self.zPid.process(self.targetZ - self.heigh)
				self.setVelocity(0, 0, zVel, 0)
				print "Heigh:", self.heigh
				print "Vel:", zVel

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		self.run3 = True
		cycle = 100
		t_activated = False
		t_fin = 0



		while(self.run3):
			totala = time.time() * 1000000

			if(self.sub1 == "FollowRoad"):
				if ((self.sub3 == "FindRoad_ghost") or (self.sub3 == "FollowingRoad_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "FindRoad"):
					if(self.contours):
						self.sub3 = "FollowingRoad"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FollowingRoad')

				elif(self.sub3 == "FollowingRoad"):
					if(not self.contours):
						self.sub3 = "FindRoad"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('FindRoad')


				# Actuation if
				if(self.sub3 == "FindRoad"):
					self.setVelocity(0, 0, 0.75, 0)
					print "Road lost"
				elif(self.sub3 == "FollowingRoad"):
					contour = max(self.contours, key=cv2.contourArea)
					try:
						(x, y), radius = cv2.minEnclosingCircle(contour)
						center = (int(x), int(y))
						ellipse = cv2.fitEllipse(contour)
						inclination = ellipse[2]
					
						if inclination < 90:
							yAngle = -inclination
						else:
							yAngle = 180 - inclination
					
						avel = self.aPid.process(yAngle)
						xError = self.targetX - center[0]
						xvel = self.xPid.process(xError)
						speed = max((self.targetX - pow(1.09, abs(xError)))/125.,0)
						
						self.setVelocity(xvel, speed, 0, avel)
					except:
						print "EXCEPTION FOUND"
						self.contours = []
					
					self.xPos = self.Pose3DPrx.getPose3DData().x
					self.yPos = self.Pose3DPrx.getPose3DData().y
					print "Xpos:", self.xPos, "Ypos:", self.yPos
			else:
				if(self.sub3 == "FindRoad"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "FollowingRoad"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata5(self):
		self.run5 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		firstPos = (10, -20)
		secondPos = (5, -30)
		thirdPos = (-10, -20)
		
		self.minDist = 0.02
		
		#Control PID
		xDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
		yDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)

		while(self.run5):
			totala = time.time() * 1000000

			if(self.sub1 == "MonitorArea"):
				if ((self.sub5 == "ToFirstPos_ghost") or (self.sub5 == "ToSecondPos_ghost") or (self.sub5 == "ToThirdPos_ghost") or (self.sub5 == "Return_ghost")):
					ghostStateIndex = self.StatesSub5.index(self.sub5)
					self.sub5 = self.StatesSub5[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub5 == "ToFirstPos"):
					if(self.droneInPosition(firstPos)):
						self.sub5 = "ToSecondPos"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('ToSecondPos')

				elif(self.sub5 == "ToSecondPos"):
					if(self.droneInPosition(secondPos)):
						self.sub5 = "ToThirdPos"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('ToThirdPos')

				elif(self.sub5 == "ToThirdPos"):
					if(self.droneInPosition(thirdPos)):
						self.sub5 = "Return"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Return')

				elif(self.sub5 == "Return"):
					if(self.droneInPosition(self.initPos)):
						self.sub5 = "ToFirstPos"
						self.monitorComplet = True
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('ToFirstPos')


				# Actuation if
				if(self.sub5 == "ToFirstPos"):
					self.xPos = self.Pose3DPrx.getPose3DData().x
					self.yPos = self.Pose3DPrx.getPose3DData().y
					
					xError = firstPos[0] - self.xPos
					xVel = xDistPid.process(xError)
					yError = firstPos[1] - self.yPos
					yVel = yDistPid.process(yError)
					
					print "Xerror:", xError, "Yerror:", yError
					print "Xvel:", xVel, "Yvel:", -yVel
					
					self.setVelocity(xVel, -yVel, 0, 0)
				elif(self.sub5 == "ToSecondPos"):
					self.xPos = self.Pose3DPrx.getPose3DData().x
					self.yPos = self.Pose3DPrx.getPose3DData().y
					
					xError = secondPos[0] - self.xPos
					xVel = xDistPid.process(xError)
					yError = secondPos[1] - self.yPos
					yVel = yDistPid.process(yError)
					
					print "xError:",xError, "Yerror:", yError
					print "Xvel:", xVel, "Yvel:", -yVel
					
					self.setVelocity(xVel, -yVel, 0, 0)
				elif(self.sub5 == "ToThirdPos"):
					self.xPos = self.Pose3DPrx.getPose3DData().x
					self.yPos = self.Pose3DPrx.getPose3DData().y
					
					xError = thirdPos[0] - self.xPos
					xVel = xDistPid.process(xError)
					yError = thirdPos[1] - self.yPos
					yVel = yDistPid.process(yError)
					
					print "Xerror:", xError, "Yerror:", yError
					print "Xvel:", xVel, "Yvel:", yVel
					print "Xvel:", xVel, "Yvel:", -yVel
					
					self.setVelocity(xVel, -yVel, 0, 0)
				elif(self.sub5 == "Return"):
					self.xPos = self.Pose3DPrx.getPose3DData().x
					self.yPos = self.Pose3DPrx.getPose3DData().y
					
					xError = self.initPos[0] - self.xPos
					xVel = xDistPid.process(xError)
					yError = self.initPos[1] - self.yPos
					yVel = yDistPid.process(yError)
					
					print "Xerror:", xError, "Yerror:", yError
					print "Xvel:", xVel, "Yvel:", -yVel
					
					self.setVelocity(xVel, -yVel, 0, 0)
			else:
				if(self.sub5 == "ToFirstPos"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "ToSecondPos"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "ToThirdPos"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "Return"):
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata6(self):
		self.run6 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		targetZDescend = 0.5

		while(self.run6):
			totala = time.time() * 1000000

			if(self.sub1 == "Landing"):
				if ((self.sub6 == "Descending_ghost") or (self.sub6 == "Land_ghost")):
					ghostStateIndex = self.StatesSub6.index(self.sub6)
					self.sub6 = self.StatesSub6[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub6 == "Descending"):
					if(abs(targetZDescend - self.heigh) < self.minAltit):
						self.sub6 = "Land"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Land')


				# Actuation if
				if(self.sub6 == "Descending"):
					#Controling heigh
					self.heigh = self.Pose3DPrx.getPose3DData().z
					zVel = self.zPid.process(targetZDescend - self.heigh)
					self.setVelocity(0, 0, zVel, 0)
					print "Heigh:", self.heigh
				elif(self.sub6 == "Land"):
					self.setVelocity(0,0,0,0)
					if not self.hasLanded:
						self.ExtraPrx.land()
						self.hasLanded =True
			else:
				if(self.sub6 == "Descending"):
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]
				elif(self.sub6 == "Land"):
					ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
					self.sub6 = self.StatesSub6[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to Extra
		Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not Extra):
			raise Exception('could not create proxy with Extra')
		self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
		if(not self.ExtraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'Extra connected'

		# Contact to CMDVel
		CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not CMDVel):
			raise Exception('could not create proxy with CMDVel')
		self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
		if(not self.CMDVelPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'CMDVel connected'

		# Contact to Pose3D
		Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not Pose3D):
			raise Exception('could not create proxy with Pose3D')
		self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
		if(not self.Pose3DPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'Pose3D connected'

		# Contact to Camera
		Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
		if(not Camera):
			raise Exception('could not create proxy with Camera')
		self.CameraPrx = CameraPrx.checkedCast(Camera)
		if(not self.CameraPrx):
			raise Exception('invalid proxy automata.Camera.Proxy')
		print 'Camera connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t3.join()
		self.t5.join()
		self.t6.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#11
0
文件: try2.py 项目: reysam93/TFG
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"PingPong",
			"Numbers",
		]

		self.StatesSub3 = [
			"Ping",
			"Ping_ghost",
			"Pong",
			"Pong_ghost",
		]

		self.StatesSub4 = [
			"1",
			"1_ghost",
			"2",
			"2_ghost",
			"3",
			"3_ghost",
		]

		self.StatesSub5 = [
			"wait2",
			"wait2_ghost",
			"wait1",
			"wait1_ghost",
		]

		self.sub1 = "Numbers"
		self.run1 = True
		self.sub3 = "Ping_ghost"
		self.run3 = True
		self.sub4 = "1_ghost"
		self.run4 = True
		self.sub5 = "wait1_ghost"
		self.run5 = True

	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()
		self.t4 = threading.Thread(target=self.subautomata4)
		self.t4.start()
		self.t5 = threading.Thread(target=self.subautomata5)
		self.t5.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 3, 113, 192, 0, 'PingPong')
		guiSubautomata1.newGuiNode(2, 4, 512, 223, 1, 'Numbers')

		guiSubautomata1.newGuiTransition((512, 223), (113, 192), (302, 378), 1, 2, 1)
		guiSubautomata1.newGuiTransition((113, 192), (512, 223), (322, 127), 2, 1, 2)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,1, self.automataGui)

		guiSubautomata3.newGuiNode(4, 0, 65, 154, 1, 'Ping')
		guiSubautomata3.newGuiNode(5, 5, 313, 162, 0, 'Pong')

		guiSubautomata3.newGuiTransition((65, 154), (313, 162), (192, 74), 3, 4, 5)
		guiSubautomata3.newGuiTransition((313, 162), (65, 154), (187, 212), 4, 5, 4)
		guiSubautomataList.append(guiSubautomata3)

		# Creating subAutomata4
		guiSubautomata4 = GuiSubautomata(4,2, self.automataGui)

		guiSubautomata4.newGuiNode(6, 0, 161, 158, 1, '1')
		guiSubautomata4.newGuiNode(7, 0, 493, 246, 0, '2')
		guiSubautomata4.newGuiNode(8, 0, 207, 358, 0, '3')

		guiSubautomata4.newGuiTransition((161, 158), (493, 246), (327, 202), 5, 6, 7)
		guiSubautomata4.newGuiTransition((493, 246), (207, 358), (350, 302), 6, 7, 8)
		guiSubautomata4.newGuiTransition((207, 358), (161, 158), (184, 258), 7, 8, 6)
		guiSubautomataList.append(guiSubautomata4)

		# Creating subAutomata5
		guiSubautomata5 = GuiSubautomata(5,5, self.automataGui)

		guiSubautomata5.newGuiNode(9, 0, 257, 221, 0, 'wait2')
		guiSubautomata5.newGuiNode(10, 0, 130, 105, 1, 'wait1')

		guiSubautomata5.newGuiTransition((130, 105), (257, 221), (260, 99), 1, 10, 9)
		guiSubautomata5.newGuiTransition((257, 221), (130, 105), (118, 214), 2, 9, 10)
		guiSubautomataList.append(guiSubautomata5)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run3 = False
		self.run4 = False
		self.run5 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		self.numbersFinished = False

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "PingPong"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3.5):
						self.sub1 = "Numbers"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Numbers')

			elif(self.sub1 == "Numbers"):
				if(self.numbersFinished):
					self.sub1 = "PingPong"
					self.numbersFinished = False
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('PingPong')


			# Actuation if
			if(self.sub1 == "PingPong"):
				print "PingPong"
				time.sleep(4)
			elif(self.sub1 == "Numbers"):
				print "Numbers"
				time.sleep(5)

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		self.run3 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		self.ping = False
		self.ping = False

		while(self.run3):
			totala = time.time() * 1000000

			if(self.sub1 == "PingPong"):
				if ((self.sub3 == "Ping_ghost") or (self.sub3 == "Pong_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "Ping"):
					if(self.ping):
						self.sub3 = "Pong"
						self.ping = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Pong')

				elif(self.sub3 == "Pong"):
					if(self.pong):
						self.sub3 = "Ping"
						self.pong = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Ping')


				# Actuation if
				if(self.sub3 == "Ping"):
					print "    PING"
					time.sleep(0.5)
					self.ping= True
				elif(self.sub3 == "Pong"):
					print "            PONG"
					time.sleep(0.5)
					self.pong = True
			else:
				if(self.sub3 == "Ping"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "Pong"):
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					self.sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata4(self):
		self.run4 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_1_max = 0.001
		t_2_max = 0.001
		t_3_max = 0.001


		while(self.run4):
			totala = time.time() * 1000000

			if(self.sub1 == "Numbers"):
				if ((self.sub4 == "1_ghost") or (self.sub4 == "2_ghost") or (self.sub4 == "3_ghost")):
					ghostStateIndex = self.StatesSub4.index(self.sub4)
					self.sub4 = self.StatesSub4[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub4 == "1"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_1_max):
							self.sub4 = "2"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('2')
							t_1_max = 0.001

				elif(self.sub4 == "2"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_2_max):
							self.sub4 = "3"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('3')
							t_2_max = 0.001

				elif(self.sub4 == "3"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_3_max):
							self.sub4 = "1"
							t_activated = False
							self.numbersFinished= True
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('1')
							t_3_max = 0.001


				# Actuation if
				if(self.sub4 == "1"):
					print "    1"
					time.sleep(1)
				elif(self.sub4 == "2"):
					print "        2"
					time.sleep(1)
				elif(self.sub4 == "3"):
					print "            3"
					time.sleep(1)
			else:
				if(self.sub4 == "1"):
					t_1_max = 0.001 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]
				elif(self.sub4 == "2"):
					t_2_max = 0.001 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]
				elif(self.sub4 == "3"):
					t_3_max = 0.001 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
					self.sub4 = self.StatesSub4[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata5(self):
		self.run5 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_wait1_max = 1
		t_wait2_max = 1


		while(self.run5):
			totala = time.time() * 1000000

			if(self.sub3 == "Pong"):
				if ((self.sub5 == "wait2_ghost") or (self.sub5 == "wait1_ghost")):
					ghostStateIndex = self.StatesSub5.index(self.sub5)
					self.sub5 = self.StatesSub5[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub5 == "wait2"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_wait2_max):
							self.sub5 = "wait1"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('wait1')
							t_wait2_max = 1

				elif(self.sub5 == "wait1"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_wait1_max):
							self.sub5 = "wait2"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('wait2')
							t_wait1_max = 1


				# Actuation if
				if(self.sub5 == "wait2"):
					print ""
				elif(self.sub5 == "wait1"):
					print ""
			else:
				if(self.sub5 == "wait2"):
					t_wait2_max = 1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]
				elif(self.sub5 == "wait1"):
					t_wait1_max = 1 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
					self.sub5 = self.StatesSub5[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()
		self.t3.join()
		self.t4.join()
		self.t5.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#12
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.StatesSub1 = [
			"TakeOff",
			"Stabilizing1",
			"GoFront",
			"Stoping",
			"Landing",
			"Stabilizing2",
		]

		self.sub1 = "TakeOff"
		self.run1 = True

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = automatagui.GuiSubautomata(1,0)

		guiSubautomata1.newGuiNode(1, 0, 61, 101, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
		guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
		guiSubautomata1.newGuiNode(4, 0, 488, 320, 0, 'Stoping')
		guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
		guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')

		orig11 = automatagui.Point(61, 101)
		dest11 = automatagui.Point(283, 75)
		mid11 = automatagui.Point(38, 179)
		guiSubautomata1.newGuiTransition(orig11, dest11, mid11, 1, 1, 2)

		orig12 = automatagui.Point(283, 75)
		dest12 = automatagui.Point(497, 130)
		mid12 = automatagui.Point(413, 79)
		guiSubautomata1.newGuiTransition(orig12, dest12, mid12, 2, 2, 3)

		orig13 = automatagui.Point(497, 130)
		dest13 = automatagui.Point(488, 320)
		mid13 = automatagui.Point(570, 239)
		guiSubautomata1.newGuiTransition(orig13, dest13, mid13, 3, 3, 4)

		orig14 = automatagui.Point(488, 320)
		dest14 = automatagui.Point(250, 408)
		mid14 = automatagui.Point(410, 411)
		guiSubautomata1.newGuiTransition(orig14, dest14, mid14, 4, 4, 5)

		orig16 = automatagui.Point(250, 408)
		dest16 = automatagui.Point(66, 283)
		mid16 = automatagui.Point(152, 338)
		guiSubautomata1.newGuiTransition(orig16, dest16, mid16, 6, 5, 6)

		orig11 = automatagui.Point(66, 283)
		dest11 = automatagui.Point(61, 101)
		mid11 = automatagui.Point(38, 179)
		guiSubautomata1.newGuiTransition(orig11, dest11, mid11, 1, 6, 1)

		guiSubautomataList.append(guiSubautomata1);

		return guiSubautomataList

	def shutDown(self):
		self.run1 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		run = True
		cycle = 100
		t_activated = False

		hasTakenOff = False
		hasLanded = False

		while(run):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(hasTakenOff):
					self.sub1 = "Stabilizing1"
					print "Going to Stabilizing"

			elif(self.sub1 == "Stabilizing1"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 1.5):
						self.sub1 = "GoFront"
						t_activated = False
						print "going to GoFront"

			elif(self.sub1 == "GoFront"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "Stoping"
						t_activated = False
						print "going to Stoping"

			elif(self.sub1 == "Stoping"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3.5):
						self.sub1 = "Landing"
						t_activated = False
						print "going to Land"

			elif(self.sub1 == "Landing"):
				if(hasLanded):
					self.sub1 = "Stabilizing2"


			elif(self.sub1 == "Stabilizing2"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "TakeOff"
						t_activated = False
						print "Restarting"


			# Actuation if
			if(self.sub1 == "TakeOff"):
				print "Taking Off"
				self.lock.acquire()
				#self.extraPrx.takeoff()
				self.lock.release()
				hasTakenOff = True
			elif(self.sub1 == "Stabilizing1"):
				print "Stabilizing"
			elif(self.sub1 == "GoFront"):
				cmd = jderobot.CMDVelData()
				cmd.linearX = 1
				cmd.linearY = 0
				cmd.linearZ = 0
				self.lock.acquire()
				self.cmdPrx.setCMDVelData(cmd)
				self.lock.release()
			elif(self.sub1 == "Stoping"):
				cmd = jderobot.CMDVelData()
				cmd.linearX = 0
				cmd.linearY = 0
				cmd.linearZ = 0
				self.lock.acquire()
				self.cmdPrx.setCMDVelData(cmd)
				self.lock.release()
				print "Stoping"
			elif(self.sub1 == "Landing"):
				print "Landing"
				self.lock.acquire()
				#self.extraPrx.land()
				self.lock.release()
				hasLanded = True
			elif(self.sub1 == "Stabilizing2"):
				finished()

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to camera
		camera = self.ic.propertyToProxy('automata.Camera.Proxy')
		if(not camera):
			raise Exception('could not create proxy with camera')
		self.cameraPrx = CameraPrx.checkedCast(camera)
		if(not self.cameraPrx):
			raise Exception('invalid proxy automata.Camera.Proxy')
		print 'camera connected'

		# Contact to pose3d
		pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not pose3d):
			raise Exception('could not create proxy with pose3d')
		self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
		if(not self.pose3dPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'pose3d connected'

		# Contact to cmd
		cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not cmd):
			raise Exception('could not create proxy with cmd')
		self.cmdPrx = CMDVelPrx.checkedCast(cmd)
		if(not self.cmdPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'cmd connected'

		# Contact to extra
		extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not extra):
			raise Exception('could not create proxy with extra')
		self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
		if(not self.extraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'extra connected'

		# Contact to navdata
		navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
		if(not navdata):
			raise Exception('could not create proxy with navdata')
		self.navdataPrx = NavdataPrx.checkedCast(navdata)
		if(not self.navdataPrx):
			raise Exception('invalid proxy automata.Navdata.Proxy')
		print 'navdata connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		self.guiThread = threading.Thread(target=self.runGui)
		self.guiThread.start()

		self.t1 = threading.Thread(target=self.subautomata1)
		#self.t1.start()


	def join(self):
		self.guiThread.join()
		self.t1.join()
示例#13
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.StatesSub1 = [
			"GoFront",
			"GoBack",
			"Turn",
			"Wait",
		]

		self.sub1 = "GoFront"
		self.run1 = True

	def shutDown(self):
		self.run1 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(createAutomata())
		self.automataGui.loadAutomata()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		run = True
		cycle = 100
		t_activated = False

		jderobot::EncodersDataPtr encoders = Encodersprx->getEncodersData();
		float thetapos = 0;
		float angle = 0;

		while(run):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "GoFront"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "GoBack"
						t_activated = False

			elif(self.sub1 == "GoBack"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 2):
						self.sub1 = "Turn"
						t_activated = False

			elif(self.sub1 == "Turn"):
				if(angle > 45):
					self.sub1 = "Wait"


			elif(self.sub1 == "Wait"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 1):
						self.sub1 = "GoFront"
						t_activated = False


			# Actuation if
			if(self.sub1 == "GoFront"):
				Motorsprx->setV(100.0);
				Motorsprx->setW(0.0);
				
				encoders = Encodersprx->getEncodersData();
				thetapos = encoders->robottheta;
			elif(self.sub1 == "GoBack"):
				Motorsprx->setV(-100.0);
				Motorsprx->setW(0.0);
				
				encoders = Encodersprx->getEncodersData();
				thetapos = encoders->robottheta;
			elif(self.sub1 == "Turn"):
				Motorsprx->setV(0.0);
				Motorsprx->setW(-3.0);
				
				encoders = Encodersprx->getEncodersData();
				if (encoders->robottheta > thetapos + 1)
					angle = thetapos - encoders->robottheta + 360;
				else
					angle = thetapos - encoders->robottheta;
				
				std::cout << "angle: " << angle << std::endl;
			elif(self.sub1 == "Wait"):
				Motorsprx->setV(0.0);
				Motorsprx->setW(0.0);
				
				encoders = Encodersprx->getEncodersData();
				thetapos = encoders->robottheta;

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);
示例#14
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"Go",
			"GoBack",
			"Rotate",
		]

		self.sub1 = "Go"
		self.run1 = True

	def getRobotTheta(self):
	        d = self.EncodersPrx.getPose3DData()
	        theta = math.atan2(2*(d.q0*d.q3 + d.q1*d.q2), 1-2*(d.q2*d.q2 + d.q3*d.q3))
	        return theta
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 112, 126, 1, 'Go')
		guiSubautomata1.newGuiNode(2, 0, 372, 136, 0, 'GoBack')
		guiSubautomata1.newGuiNode(3, 0, 238, 308, 0, 'Rotate')

		guiSubautomata1.newGuiTransition((112, 126), (372, 136), (237, 114), 1, 1, 2)
		guiSubautomata1.newGuiTransition((372, 136), (238, 308), (305, 222), 2, 2, 3)
		guiSubautomata1.newGuiTransition((238, 308), (112, 126), (140, 216), 3, 3, 1)
		guiSubautomataList.append(guiSubautomata1)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0

		laserData = self.LasersPrx.getLaserData()
		minDistance = 1.5
		dist = None
		destinyAngle = None
		error = 0
		
		print "numero de lasers (grados):", laserData.numLaser
		

		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "Go"):
				if(dist != None and dist < minDistance):
					self.sub1 = "GoBack"
					self.MotorsPrx.setV(0)
					print "stopping"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('GoBack')

			elif(self.sub1 == "GoBack"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 1.5):
						self.sub1 = "Rotate"
						t_activated = False
						angle = self.getRobotTheta()
						print "my Angle:", angle
						self.MotorsPrx.setV(0)
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('Rotate')

			elif(self.sub1 == "Rotate"):
				if(error <= 0.1 and error >= -0.1):
					self.sub1 = "Go"
					destinyAngle =None
					self.MotorsPrx.setW(0)
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('Go')


			# Actuation if
			if(self.sub1 == "Go"):
				self.MotorsPrx.setV(1)
				laserData = self.LasersPrx.getLaserData()
				
				dist = None
				for i in range(60, 120):
					dist = laserData.distanceData[i]/1000
					if dist == None or laserData.distanceData[i]/1000 < dist:
						dist = laserData.distanceData[i]/1000
				
				print "dist:", dist
			elif(self.sub1 == "GoBack"):
				self.MotorsPrx.setV(-0.1)
				print "back"
			elif(self.sub1 == "Rotate"):
				if destinyAngle == None:
					turn = random.uniform(-math.pi, math.pi)
					destinyAngle = (angle + turn) % (math.pi)
					print "DestinyAngle:", destinyAngle
				
				angle =  self.getRobotTheta()
				error = (destinyAngle - angle)*0.75
				
				if error > 0 and error < 0.1:
					error = 0.1
				elif error < 0 and error > -0.1:
					error = -0.1
				
				print "angle:", angle, "destiny:", destinyAngle, "speed:", error
				self.MotorsPrx.setW(error)

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to Motors
		Motors = self.ic.propertyToProxy('automata.Motors.Proxy')
		if(not Motors):
			raise Exception('could not create proxy with Motors')
		self.MotorsPrx = MotorsPrx.checkedCast(Motors)
		if(not self.MotorsPrx):
			raise Exception('invalid proxy automata.Motors.Proxy')
		print 'Motors connected'

		# Contact to Pose3D
		Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not Pose3D):
			raise Exception('could not create proxy with Pose3D')
		self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
		if(not self.Pose3DPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'Pose3D connected'

		# Contact to Laser
		Laser = self.ic.propertyToProxy('automata.Laser.Proxy')
		if(not Laser):
			raise Exception('could not create proxy with Laser')
		self.LaserPrx = LaserPrx.checkedCast(Laser)
		if(not self.LaserPrx):
			raise Exception('invalid proxy automata.Laser.Proxy')
		print 'Laser connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print 'runtime gui enabled'
				else:
					self.displayGui = False
					print 'runtime gui disabled'
示例#15
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"square",
			"wait",
		]

		self.StatesSub2 = [
			"go_up",
			"go_up_ghost",
			"turn_rigth",
			"turn_rigth_ghost",
			"go_rigth",
			"go_rigth_ghost",
			"turn_down",
			"turn_down_ghost",
			"go_down",
			"go_down_ghost",
			"turn_left",
			"turn_left_ghost",
			"go_left",
			"go_left_ghost",
			"turn_up",
			"turn_up_ghost",
		]

		self.StatesSub3 = [
			"wait1",
			"wait1_ghost",
			"wait2",
			"wait2_ghost",
		]

		self.sub1 = "square"
		self.run1 = True
		self.sub2 = "go_up_ghost"
		self.run2 = True
		self.sub3 = "wait1_ghost"
		self.run3 = True

	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t2 = threading.Thread(target=self.subautomata2)
		self.t2.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 2, 134, 346, 1, 'square')
		guiSubautomata1.newGuiNode(2, 3, 534, 349, 0, 'wait')

		guiSubautomata1.newGuiTransition((134, 346), (534, 349), (337, 230), 1, 1, 2)
		guiSubautomata1.newGuiTransition((534, 349), (134, 346), (335, 457), 4, 2, 1)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata2
		guiSubautomata2 = GuiSubautomata(2,1, self.automataGui)

		guiSubautomata2.newGuiNode(5, 0, 168, 559, 1, 'go_up')
		guiSubautomata2.newGuiNode(8, 0, 69, 396, 0, 'turn_rigth')
		guiSubautomata2.newGuiNode(9, 0, 185, 270, 0, 'go_rigth')
		guiSubautomata2.newGuiNode(10, 0, 343, 192, 0, 'turn_down')
		guiSubautomata2.newGuiNode(11, 0, 486, 283, 0, 'go_down')
		guiSubautomata2.newGuiNode(12, 0, 555, 426, 0, 'turn_left')
		guiSubautomata2.newGuiNode(13, 0, 505, 583, 0, 'go_left')
		guiSubautomata2.newGuiNode(14, 0, 354, 650, 0, 'turn_up')

		guiSubautomata2.newGuiTransition((168, 559), (69, 396), (118, 477), 10, 5, 8)
		guiSubautomata2.newGuiTransition((69, 396), (185, 270), (127, 333), 12, 8, 9)
		guiSubautomata2.newGuiTransition((185, 270), (343, 192), (264, 231), 13, 9, 10)
		guiSubautomata2.newGuiTransition((343, 192), (486, 283), (415, 237), 14, 10, 11)
		guiSubautomata2.newGuiTransition((486, 283), (555, 426), (525, 348), 15, 11, 12)
		guiSubautomata2.newGuiTransition((555, 426), (505, 583), (534, 498), 16, 12, 13)
		guiSubautomata2.newGuiTransition((505, 583), (354, 650), (430, 616), 17, 13, 14)
		guiSubautomata2.newGuiTransition((354, 650), (168, 559), (261, 604), 18, 14, 5)
		guiSubautomataList.append(guiSubautomata2)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,2, self.automataGui)

		guiSubautomata3.newGuiNode(15, 0, 166, 223, 1, 'wait1')
		guiSubautomata3.newGuiNode(16, 0, 446, 237, 0, 'wait2')

		guiSubautomata3.newGuiTransition((166, 223), (446, 237), (291, 127), 1, 15, 16)
		guiSubautomata3.newGuiTransition((446, 237), (166, 223), (291, 301), 3, 16, 15)
		guiSubautomataList.append(guiSubautomata3)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run2 = False
		self.run3 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0


		while(run):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "square"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 20):
						self.sub1 = "wait"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('wait')

			elif(self.sub1 == "wait"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 5):
						self.sub1 = "square"
						t_activated = False
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('square')


			# Actuation if
			if(self.sub1 == "wait"):
				
				
				Motorsprx->setV(0.0);
				Motorsprx->setW(0.0);

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata2(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_go_up_max = 2
		t_go_rigth_max = 2
		t_go_down_max = 2
		t_go_left_max = 2

		jderobot::EncodersDataPtr encoders = Encodersprx->getEncodersData();
		float thetapos = 0;
		float angle = 0;

		while(run):
			totala = time.time() * 1000000

			if(self.sub1 == "square"):
				if ((self.sub2 == "go_up_ghost") or (self.sub2 == "turn_rigth_ghost") or (self.sub2 == "go_rigth_ghost") or (self.sub2 == "turn_down_ghost") or (self.sub2 == "go_down_ghost") or (self.sub2 == "turn_left_ghost") or (self.sub2 == "go_left_ghost") or (self.sub2 == "turn_up_ghost")):
					ghostStateIndex = self.StatesSub2.index(self.sub2)
					self.sub2 = self.StatesSub2[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub2 == "go_up"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_go_up_max):
							self.sub2 = "turn_rigth"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('turn_rigth')
							t_go_up_max = 2

				elif(self.sub2 == "turn_rigth"):
					if(angle > 90):
						self.sub2 = "go_rigth"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('go_rigth')

				elif(self.sub2 == "go_rigth"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_go_rigth_max):
							self.sub2 = "turn_down"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('turn_down')
							t_go_rigth_max = 2

				elif(self.sub2 == "turn_down"):
					if(angle > 90):
						self.sub2 = "go_down"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('go_down')

				elif(self.sub2 == "go_down"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_go_down_max):
							self.sub2 = "turn_left"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('turn_left')
							t_go_down_max = 2

				elif(self.sub2 == "turn_left"):
					if(angle > 90):
						self.sub2 = "go_left"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('go_left')

				elif(self.sub2 == "go_left"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_go_left_max):
							self.sub2 = "turn_up"
							t_activated = False
							if self.displayGui:
								self.automataGui.notifySetNodeAsActive('turn_up')
							t_go_left_max = 2

				elif(self.sub2 == "turn_up"):
					if(angle > 90):
						self.sub2 = "go_up"
						if self.displayGui:
							self.automataGui.notifySetNodeAsActive('go_up')


				# Actuation if
				if(self.sub2 == "go_up"):
					
					
					Motorsprx->setV(100.0);
					Motorsprx->setW(0.0);
					
					encoders = Encodersprx->getEncodersData();
					thetapos = encoders->robottheta;
				elif(self.sub2 == "turn_rigth"):
					
					
					Motorsprx->setV(0.0);
					Motorsprx->setW(-3.0);
					
					encoders = Encodersprx->getEncodersData();
					if (encoders->robottheta > thetapos + 1)
						angle = thetapos - encoders->robottheta + 360;
					else
						angle = thetapos - encoders->robottheta;
					
					std::cout << "angle: " << angle << std::endl;
				elif(self.sub2 == "go_rigth"):
					
					
					Motorsprx->setV(100.0);
					Motorsprx->setW(0.0);
					
					encoders = Encodersprx->getEncodersData();
					thetapos = encoders->robottheta;
				elif(self.sub2 == "turn_down"):
					
					
					Motorsprx->setV(0.0);
					Motorsprx->setW(-3.0);
					
					encoders = Encodersprx->getEncodersData();
					if (encoders->robottheta > thetapos + 1)
						angle = thetapos - encoders->robottheta + 360;
					else
						angle = thetapos - encoders->robottheta;
					
					std::cout << "angle: " << angle << std::endl;
				elif(self.sub2 == "go_down"):
					
					
					Motorsprx->setV(100.0);
					Motorsprx->setW(0.0);
					
					encoders = Encodersprx->getEncodersData();
					thetapos = encoders->robottheta;
				elif(self.sub2 == "turn_left"):
					
					
					Motorsprx->setV(0.0);
					Motorsprx->setW(-3.0);
					
					encoders = Encodersprx->getEncodersData();
					if (encoders->robottheta > thetapos + 1)
						angle = thetapos - encoders->robottheta + 360;
示例#16
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"goforward",
		]

		self.sub1 = "goforward"
		self.run1 = True

	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 118, 121, 1, 'goforward')

		guiSubautomataList.append(guiSubautomata1)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if

			# Actuation if
			if(self.sub1 == "goforward"):
				self.my_motors.sendV(0.2)

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)
		self.ic,self.node = comm.init(self.ic)

		# Contact to my_motors
		self.my_motors = comm.getMotorsClient(self.ic, 'automata.my_motors')
		if(not self.my_motors):
			raise Exception('could not create client with my_motors')
		print('my_motors connected')


	def destroyIc(self):
		self.my_motors.stop()
		comm.destroy(self.ic, self.node)

	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print('runtime gui enabled')
				else:
					self.displayGui = False
					print('runtime gui disabled')
示例#17
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.displayGui = False
		self.StatesSub1 = [
			"GoForward",
			"GoBack",
		]

		self.sub1 = "GoForward"
		self.run1 = True

	def calculate_obstacle(self):
		self.laserData = self.KobukiLaser.getLaserData()
		min_dist = 1000
		for i in range(len(self.laserData.values)):
			if self.laserData.values[i] < min_dist:
				min_dist = self.laserData.values[i]
	
		if min_dist < 1.0:
			return True
		else:
			return False
	
	def startThreads(self):
		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 0, 69, 163, 1, 'GoForward')
		guiSubautomata1.newGuiNode(2, 0, 255, 117, 0, 'GoBack')

		guiSubautomata1.newGuiTransition((69, 163), (255, 117), (139, 78), 1, 1, 2)
		guiSubautomata1.newGuiTransition((255, 117), (69, 163), (189, 196), 2, 2, 1)
		guiSubautomataList.append(guiSubautomata1)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.startThreads()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		self.run1 = True
		cycle = 100
		t_activated = False
		t_fin = 0


		while(self.run1):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "GoForward"):
				if(self.calculate_obstacle()):
					self.sub1 = "GoBack"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('GoBack')

			elif(self.sub1 == "GoBack"):
				if(not self.calculate_obstacle()):
					self.sub1 = "GoForward"
					if self.displayGui:
						self.automataGui.notifySetNodeAsActive('GoForward')


			# Actuation if
			if(self.sub1 == "GoForward"):
				self.KobukiMotors.sendV(0.5)
				self.KobukiMotors.sendW(0.0)
			elif(self.sub1 == "GoBack"):
				self.KobukiMotors.sendV(-0.3)
				self.KobukiMotors.sendW(0.2)

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = EasyIce.initialize(sys.argv)
		self.ic,self.node = comm.init(self.ic)

		# Contact to KobukiMotors
		self.KobukiMotors = comm.getMotorsClient(self.ic, 'automata.KobukiMotors')
		if(not self.KobukiMotors):
			raise Exception('could not create client with KobukiMotors')
		print('KobukiMotors connected')

		# Contact to KobukiLaser
		self.KobukiLaser = comm.getLaserClient(self.ic, 'automata.KobukiLaser')
		if(not self.KobukiLaser):
			raise Exception('could not create client with KobukiLaser')
		print('KobukiLaser connected')


	def destroyIc(self):
		self.KobukiMotors.stop()
		self.KobukiLaser.stop()
		comm.destroy(self.ic, self.node)

	def start(self):
		if self.displayGui:
			self.guiThread = threading.Thread(target=self.runGui)
			self.guiThread.start()
		else:
			self.startThreads()



	def join(self):
		if self.displayGui:
			self.guiThread.join()
		self.t1.join()


	def readArgs(self):
		for arg in sys.argv:
			splitedArg = arg.split('=')
			if splitedArg[0] == '--displaygui':
				if splitedArg[1] == 'True' or splitedArg[1] == 'true':
					self.displayGui = True
					print('runtime gui enabled')
				else:
					self.displayGui = False
					print('runtime gui disabled')
示例#18
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "PingPong",
            "Numbers",
        ]

        self.StatesSub3 = [
            "Ping",
            "Ping_ghost",
            "Pong",
            "Pong_ghost",
        ]

        self.StatesSub4 = [
            "1",
            "1_ghost",
            "2",
            "2_ghost",
            "3",
            "3_ghost",
        ]

        self.StatesSub5 = [
            "wait2",
            "wait2_ghost",
            "wait1",
            "wait1_ghost",
        ]

        self.sub1 = "Numbers"
        self.run1 = True
        self.sub3 = "Ping_ghost"
        self.run3 = True
        self.sub4 = "1_ghost"
        self.run4 = True
        self.sub5 = "wait1_ghost"
        self.run5 = True

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()
        self.t4 = threading.Thread(target=self.subautomata4)
        self.t4.start()
        self.t5 = threading.Thread(target=self.subautomata5)
        self.t5.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 3, 113, 192, 0, 'PingPong')
        guiSubautomata1.newGuiNode(2, 4, 512, 223, 1, 'Numbers')

        guiSubautomata1.newGuiTransition((512, 223), (113, 192), (302, 378), 1,
                                         2, 1)
        guiSubautomata1.newGuiTransition((113, 192), (512, 223), (322, 127), 2,
                                         1, 2)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 1, self.automataGui)

        guiSubautomata3.newGuiNode(4, 0, 65, 154, 1, 'Ping')
        guiSubautomata3.newGuiNode(5, 5, 313, 162, 0, 'Pong')

        guiSubautomata3.newGuiTransition((65, 154), (313, 162), (192, 74), 3,
                                         4, 5)
        guiSubautomata3.newGuiTransition((313, 162), (65, 154), (187, 212), 4,
                                         5, 4)
        guiSubautomataList.append(guiSubautomata3)

        # Creating subAutomata4
        guiSubautomata4 = GuiSubautomata(4, 2, self.automataGui)

        guiSubautomata4.newGuiNode(6, 0, 161, 158, 1, '1')
        guiSubautomata4.newGuiNode(7, 0, 493, 246, 0, '2')
        guiSubautomata4.newGuiNode(8, 0, 207, 358, 0, '3')

        guiSubautomata4.newGuiTransition((161, 158), (493, 246), (327, 202), 5,
                                         6, 7)
        guiSubautomata4.newGuiTransition((493, 246), (207, 358), (350, 302), 6,
                                         7, 8)
        guiSubautomata4.newGuiTransition((207, 358), (161, 158), (184, 258), 7,
                                         8, 6)
        guiSubautomataList.append(guiSubautomata4)

        # Creating subAutomata5
        guiSubautomata5 = GuiSubautomata(5, 5, self.automataGui)

        guiSubautomata5.newGuiNode(9, 0, 257, 221, 0, 'wait2')
        guiSubautomata5.newGuiNode(10, 0, 130, 105, 1, 'wait1')

        guiSubautomata5.newGuiTransition((130, 105), (257, 221), (260, 99), 1,
                                         10, 9)
        guiSubautomata5.newGuiTransition((257, 221), (130, 105), (118, 214), 2,
                                         9, 10)
        guiSubautomataList.append(guiSubautomata5)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run3 = False
        self.run4 = False
        self.run5 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.numbersFinished = False

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "PingPong"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3.5):
                        self.sub1 = "Numbers"
                        t_activated = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Numbers')

            elif (self.sub1 == "Numbers"):
                if (self.numbersFinished):
                    self.sub1 = "PingPong"
                    self.numbersFinished = False
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('PingPong')

            # Actuation if
            if (self.sub1 == "PingPong"):
                print "PingPong"
                time.sleep(4)
            elif (self.sub1 == "Numbers"):
                print "Numbers"
                time.sleep(5)

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        self.run3 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.ping = False
        self.ping = False

        while (self.run3):
            totala = time.time() * 1000000

            if (self.sub1 == "PingPong"):
                if ((self.sub3 == "Ping_ghost")
                        or (self.sub3 == "Pong_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "Ping"):
                    if (self.ping):
                        self.sub3 = "Pong"
                        self.ping = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Pong')

                elif (self.sub3 == "Pong"):
                    if (self.pong):
                        self.sub3 = "Ping"
                        self.pong = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Ping')

                # Actuation if
                if (self.sub3 == "Ping"):
                    print "    PING"
                    time.sleep(0.5)
                    self.ping = True
                elif (self.sub3 == "Pong"):
                    print "            PONG"
                    time.sleep(0.5)
                    self.pong = True
            else:
                if (self.sub3 == "Ping"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "Pong"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata4(self):
        self.run4 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_1_max = 0.001
        t_2_max = 0.001
        t_3_max = 0.001

        while (self.run4):
            totala = time.time() * 1000000

            if (self.sub1 == "Numbers"):
                if ((self.sub4 == "1_ghost") or (self.sub4 == "2_ghost")
                        or (self.sub4 == "3_ghost")):
                    ghostStateIndex = self.StatesSub4.index(self.sub4)
                    self.sub4 = self.StatesSub4[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub4 == "1"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_1_max):
                            self.sub4 = "2"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive('2')
                            t_1_max = 0.001

                elif (self.sub4 == "2"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_2_max):
                            self.sub4 = "3"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive('3')
                            t_2_max = 0.001

                elif (self.sub4 == "3"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_3_max):
                            self.sub4 = "1"
                            t_activated = False
                            self.numbersFinished = True
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive('1')
                            t_3_max = 0.001

                # Actuation if
                if (self.sub4 == "1"):
                    print "    1"
                    time.sleep(1)
                elif (self.sub4 == "2"):
                    print "        2"
                    time.sleep(1)
                elif (self.sub4 == "3"):
                    print "            3"
                    time.sleep(1)
            else:
                if (self.sub4 == "1"):
                    t_1_max = 0.001 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]
                elif (self.sub4 == "2"):
                    t_2_max = 0.001 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]
                elif (self.sub4 == "3"):
                    t_3_max = 0.001 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata5(self):
        self.run5 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_wait1_max = 1
        t_wait2_max = 1

        while (self.run5):
            totala = time.time() * 1000000

            if (self.sub3 == "Pong"):
                if ((self.sub5 == "wait2_ghost")
                        or (self.sub5 == "wait1_ghost")):
                    ghostStateIndex = self.StatesSub5.index(self.sub5)
                    self.sub5 = self.StatesSub5[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub5 == "wait2"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_wait2_max):
                            self.sub5 = "wait1"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive('wait1')
                            t_wait2_max = 1

                elif (self.sub5 == "wait1"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_wait1_max):
                            self.sub5 = "wait2"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive('wait2')
                            t_wait1_max = 1

                # Actuation if
                if (self.sub5 == "wait2"):
                    print ""
                elif (self.sub5 == "wait1"):
                    print ""
            else:
                if (self.sub5 == "wait2"):
                    t_wait2_max = 1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "wait1"):
                    t_wait1_max = 1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()
        self.t3.join()
        self.t4.join()
        self.t5.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'
示例#19
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "Go",
            "GoBack",
            "Rotate",
        ]

        self.sub1 = "Go"
        self.run1 = True

    def getRobotTheta(self):
        d = self.EncodersPrx.getPose3DData()
        theta = math.atan2(2 * (d.q0 * d.q3 + d.q1 * d.q2),
                           1 - 2 * (d.q2 * d.q2 + d.q3 * d.q3))
        return theta

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 112, 126, 1, 'Go')
        guiSubautomata1.newGuiNode(2, 0, 372, 136, 0, 'GoBack')
        guiSubautomata1.newGuiNode(3, 0, 238, 308, 0, 'Rotate')

        guiSubautomata1.newGuiTransition((112, 126), (372, 136), (237, 114), 1,
                                         1, 2)
        guiSubautomata1.newGuiTransition((372, 136), (238, 308), (305, 222), 2,
                                         2, 3)
        guiSubautomata1.newGuiTransition((238, 308), (112, 126), (140, 216), 3,
                                         3, 1)
        guiSubautomataList.append(guiSubautomata1)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        laserData = self.LasersPrx.getLaserData()
        minDistance = 1.5
        dist = None
        destinyAngle = None
        error = 0

        print "numero de lasers (grados):", laserData.numLaser

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "Go"):
                if (dist != None and dist < minDistance):
                    self.sub1 = "GoBack"
                    self.MotorsPrx.setV(0)
                    print "stopping"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('GoBack')

            elif (self.sub1 == "GoBack"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 1.5):
                        self.sub1 = "Rotate"
                        t_activated = False
                        angle = self.getRobotTheta()
                        print "my Angle:", angle
                        self.MotorsPrx.setV(0)
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Rotate')

            elif (self.sub1 == "Rotate"):
                if (error <= 0.1 and error >= -0.1):
                    self.sub1 = "Go"
                    destinyAngle = None
                    self.MotorsPrx.setW(0)
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('Go')

            # Actuation if
            if (self.sub1 == "Go"):
                self.MotorsPrx.setV(1)
                laserData = self.LasersPrx.getLaserData()

                dist = None
                for i in range(60, 120):
                    dist = laserData.distanceData[i] / 1000
                    if dist == None or laserData.distanceData[i] / 1000 < dist:
                        dist = laserData.distanceData[i] / 1000

                print "dist:", dist
            elif (self.sub1 == "GoBack"):
                self.MotorsPrx.setV(-0.1)
                print "back"
            elif (self.sub1 == "Rotate"):
                if destinyAngle == None:
                    turn = random.uniform(-math.pi, math.pi)
                    destinyAngle = (angle + turn) % (math.pi)
                    print "DestinyAngle:", destinyAngle

                angle = self.getRobotTheta()
                error = (destinyAngle - angle) * 0.75

                if error > 0 and error < 0.1:
                    error = 0.1
                elif error < 0 and error > -0.1:
                    error = -0.1

                print "angle:", angle, "destiny:", destinyAngle, "speed:", error
                self.MotorsPrx.setW(error)

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to Motors
        Motors = self.ic.propertyToProxy('automata.Motors.Proxy')
        if (not Motors):
            raise Exception('could not create proxy with Motors')
        self.MotorsPrx = MotorsPrx.checkedCast(Motors)
        if (not self.MotorsPrx):
            raise Exception('invalid proxy automata.Motors.Proxy')
        print 'Motors connected'

        # Contact to Pose3D
        Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
        if (not Pose3D):
            raise Exception('could not create proxy with Pose3D')
        self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
        if (not self.Pose3DPrx):
            raise Exception('invalid proxy automata.Pose3D.Proxy')
        print 'Pose3D connected'

        # Contact to Laser
        Laser = self.ic.propertyToProxy('automata.Laser.Proxy')
        if (not Laser):
            raise Exception('could not create proxy with Laser')
        self.LaserPrx = LaserPrx.checkedCast(Laser)
        if (not self.LaserPrx):
            raise Exception('invalid proxy automata.Laser.Proxy')
        print 'Laser connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'
示例#20
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "TakeOff",
            "GoFront",
            "DoSquare",
            "Landing",
            "END",
        ]

        self.StatesSub2 = [
            "TakingOff",
            "TakingOff_ghost",
            "Stabilizing1",
            "Stabilizing1_ghost",
        ]

        self.StatesSub3 = [
            "GoingFront",
            "GoingFront_ghost",
            "Stabilizing2",
            "Stabilizing2_ghost",
            "goneFront",
            "goneFront_ghost",
        ]

        self.StatesSub4 = [
            "empty1",
            "empty1_ghost",
            "empty2",
            "empty2_ghost",
        ]

        self.StatesSub5 = [
            "DoingRigthSide",
            "DoingRigthSide_ghost",
            "DoingTopSide",
            "DoingTopSide_ghost",
            "DoingLeftSide",
            "DoingLeftSide_ghost",
            "DoingBottomSide",
            "DoingBottomSide_ghost",
        ]

        self.StatesSub6 = [
            "GoingToTop",
            "GoingToTop_ghost",
            "StabilizingTop",
            "StabilizingTop_ghost",
            "GoneToTop",
            "GoneToTop_ghost",
        ]

        self.StatesSub8 = [
            "GoingToLeft",
            "GoingToLeft_ghost",
            "StabilizingLeft",
            "StabilizingLeft_ghost",
            "GoneToLeft",
            "GoneToLeft_ghost",
        ]

        self.StatesSub9 = [
            "GoingToBottom",
            "GoingToBottom_ghost",
            "StabilizingBottom",
            "StabilizingBottom_ghost",
            "GoneToBottom",
            "GoneToBottom_ghost",
        ]

        self.StatesSub10 = [
            "GoingToRigth",
            "GoingToRigth_ghost",
            "StabilizingRigth",
            "StabilizingRigth_ghost",
            "GoneToRigth",
            "GoneToRigth_ghost",
        ]

        self.sub1 = "TakeOff"
        self.run1 = True
        self.sub2 = "TakingOff_ghost"
        self.run2 = True
        self.sub3 = "GoingFront_ghost"
        self.run3 = True
        self.sub4 = "empty1_ghost"
        self.run4 = True
        self.sub5 = "DoingRigthSide_ghost"
        self.run5 = True
        self.sub6 = "GoingToTop_ghost"
        self.run6 = True
        self.sub8 = "GoingToLeft_ghost"
        self.run8 = True
        self.sub9 = "GoingToBottom_ghost"
        self.run9 = True
        self.sub10 = "GoingToRigth_ghost"
        self.run10 = True

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t2 = threading.Thread(target=self.subautomata2)
        self.t2.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()
        self.t4 = threading.Thread(target=self.subautomata4)
        self.t4.start()
        self.t5 = threading.Thread(target=self.subautomata5)
        self.t5.start()
        self.t6 = threading.Thread(target=self.subautomata6)
        self.t6.start()
        self.t8 = threading.Thread(target=self.subautomata8)
        self.t8.start()
        self.t9 = threading.Thread(target=self.subautomata9)
        self.t9.start()
        self.t10 = threading.Thread(target=self.subautomata10)
        self.t10.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 2, 76, 158, 1, 'TakeOff')
        guiSubautomata1.newGuiNode(5, 3, 323, 136, 0, 'GoFront')
        guiSubautomata1.newGuiNode(14, 5, 587, 181, 0, 'DoSquare')
        guiSubautomata1.newGuiNode(15, 0, 395, 467, 0, 'Landing')
        guiSubautomata1.newGuiNode(16, 0, 89, 421, 0, 'END')

        guiSubautomata1.newGuiTransition((76, 158), (323, 136), (189, 145), 2,
                                         1, 5)
        guiSubautomata1.newGuiTransition((323, 136), (587, 181), (446, 132), 6,
                                         5, 14)
        guiSubautomata1.newGuiTransition((395, 467), (89, 421), (226, 464), 8,
                                         15, 16)
        guiSubautomata1.newGuiTransition((89, 421), (76, 158), (48, 290), 28,
                                         16, 1)
        guiSubautomata1.newGuiTransition((587, 181), (395, 467), (491, 324),
                                         37, 14, 15)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata2
        guiSubautomata2 = GuiSubautomata(2, 1, self.automataGui)

        guiSubautomata2.newGuiNode(2, 0, 106, 141, 1, 'TakingOff')
        guiSubautomata2.newGuiNode(3, 0, 283, 219, 0, 'Stabilizing1')

        guiSubautomata2.newGuiTransition((106, 141), (283, 219), (138, 243), 1,
                                         2, 3)
        guiSubautomata2.newGuiTransition((283, 219), (106, 141), (244, 116),
                                         26, 3, 2)
        guiSubautomataList.append(guiSubautomata2)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 5, self.automataGui)

        guiSubautomata3.newGuiNode(9, 0, 127, 107, 1, 'GoingFront')
        guiSubautomata3.newGuiNode(10, 4, 511, 137, 0, 'Stabilizing2')
        guiSubautomata3.newGuiNode(11, 0, 298, 291, 0, 'goneFront')

        guiSubautomata3.newGuiTransition((127, 107), (511, 137), (319, 123), 2,
                                         9, 10)
        guiSubautomata3.newGuiTransition((511, 137), (298, 291), (405, 214), 3,
                                         10, 11)
        guiSubautomata3.newGuiTransition((298, 291), (127, 107), (212, 199),
                                         27, 11, 9)
        guiSubautomataList.append(guiSubautomata3)

        # Creating subAutomata4
        guiSubautomata4 = GuiSubautomata(4, 10, self.automataGui)

        guiSubautomata4.newGuiNode(12, 0, 232, 178, 1, 'empty1')
        guiSubautomata4.newGuiNode(13, 0, 445, 217, 0, 'empty2')

        guiSubautomata4.newGuiTransition((232, 178), (445, 217), (357, 129), 4,
                                         12, 13)
        guiSubautomata4.newGuiTransition((445, 217), (232, 178), (316, 271), 5,
                                         13, 12)
        guiSubautomataList.append(guiSubautomata4)

        # Creating subAutomata5
        guiSubautomata5 = GuiSubautomata(5, 14, self.automataGui)

        guiSubautomata5.newGuiNode(17, 6, 465, 388, 1, 'DoingRigthSide')
        guiSubautomata5.newGuiNode(18, 8, 461, 140, 0, 'DoingTopSide')
        guiSubautomata5.newGuiNode(19, 9, 134, 130, 0, 'DoingLeftSide')
        guiSubautomata5.newGuiNode(20, 10, 141, 389, 0, 'DoingBottomSide')

        guiSubautomata5.newGuiTransition((465, 388), (461, 140), (463, 264), 9,
                                         17, 18)
        guiSubautomata5.newGuiTransition((461, 140), (134, 130), (298, 136),
                                         10, 18, 19)
        guiSubautomata5.newGuiTransition((134, 130), (141, 389), (137, 260),
                                         11, 19, 20)
        guiSubautomata5.newGuiTransition((141, 389), (465, 388), (303, 388),
                                         12, 20, 17)
        guiSubautomataList.append(guiSubautomata5)

        # Creating subAutomata6
        guiSubautomata6 = GuiSubautomata(6, 17, self.automataGui)

        guiSubautomata6.newGuiNode(21, 0, 352, 452, 1, 'GoingToTop')
        guiSubautomata6.newGuiNode(22, 0, 280, 218, 0, 'StabilizingTop')
        guiSubautomata6.newGuiNode(23, 0, 448, 105, 0, 'GoneToTop')

        guiSubautomata6.newGuiTransition((352, 452), (280, 218), (212, 319),
                                         13, 21, 22)
        guiSubautomata6.newGuiTransition((280, 218), (448, 105), (350, 132),
                                         14, 22, 23)
        guiSubautomata6.newGuiTransition((448, 105), (352, 452), (400, 279),
                                         16, 23, 21)
        guiSubautomataList.append(guiSubautomata6)

        # Creating subAutomata8
        guiSubautomata8 = GuiSubautomata(8, 18, self.automataGui)

        guiSubautomata8.newGuiNode(24, 0, 135, 397, 1, 'GoingToLeft')
        guiSubautomata8.newGuiNode(25, 0, 213, 194, 0, 'StabilizingLeft')
        guiSubautomata8.newGuiNode(26, 0, 416, 260, 0, 'GoneToLeft')

        guiSubautomata8.newGuiTransition((135, 397), (213, 194), (174, 295),
                                         17, 24, 25)
        guiSubautomata8.newGuiTransition((213, 194), (416, 260), (314, 227),
                                         18, 25, 26)
        guiSubautomata8.newGuiTransition((416, 260), (135, 397), (275, 328),
                                         19, 26, 24)
        guiSubautomataList.append(guiSubautomata8)

        # Creating subAutomata9
        guiSubautomata9 = GuiSubautomata(9, 19, self.automataGui)

        guiSubautomata9.newGuiNode(27, 0, 169, 408, 1, 'GoingToBottom')
        guiSubautomata9.newGuiNode(28, 0, 243, 164, 0, 'StabilizingBottom')
        guiSubautomata9.newGuiNode(29, 0, 418, 355, 0, 'GoneToBottom')

        guiSubautomata9.newGuiTransition((169, 408), (243, 164), (206, 286),
                                         20, 27, 28)
        guiSubautomata9.newGuiTransition((243, 164), (418, 355), (330, 259),
                                         21, 28, 29)
        guiSubautomata9.newGuiTransition((418, 355), (169, 408), (293, 381),
                                         22, 29, 27)
        guiSubautomataList.append(guiSubautomata9)

        # Creating subAutomata10
        guiSubautomata10 = GuiSubautomata(10, 20, self.automataGui)

        guiSubautomata10.newGuiNode(31, 0, 121, 361, 1, 'GoingToRigth')
        guiSubautomata10.newGuiNode(32, 0, 231, 110, 0, 'StabilizingRigth')
        guiSubautomata10.newGuiNode(33, 0, 371, 331, 0, 'GoneToRigth')

        guiSubautomata10.newGuiTransition((121, 361), (231, 110), (176, 235),
                                          23, 31, 32)
        guiSubautomata10.newGuiTransition((231, 110), (371, 331), (300, 220),
                                          24, 32, 33)
        guiSubautomata10.newGuiTransition((371, 331), (121, 361), (246, 346),
                                          25, 33, 31)
        guiSubautomataList.append(guiSubautomata10)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run2 = False
        self.run3 = False
        self.run4 = False
        self.run5 = False
        self.run6 = False
        self.run8 = False
        self.run9 = False
        self.run10 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.goneFront = False
        self.squareDone = False
        self.goneBack = False

        while (run):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "TakeOff"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3.5):
                        self.sub1 = "GoFront"
                        t_activated = False
                        print "From TakeOff to GoFront"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('GoFront')

            elif (self.sub1 == "GoFront"):
                if (self.goneFront):
                    self.sub1 = "DoSquare"
                    self.goneFront = False
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('DoSquare')

            elif (self.sub1 == "DoSquare"):
                if (self.squareDone):
                    self.sub1 = "Landing"
                    self.squareDone = False
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('Landing')

            elif (self.sub1 == "Landing"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 2):
                        self.sub1 = "END"
                        t_activated = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('END')

            elif (self.sub1 == "END"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 3):
                        self.sub1 = "TakeOff"
                        t_activated = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('TakeOff')

            # Actuation if
            if (self.sub1 == "Landing"):
                print "landing"
                self.lock.acquire()
                self.extraPrx.land()
                self.lock.release()
            elif (self.sub1 == "END"):
                print "END"

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata2(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_TakingOff_max = 2
        t_Stabilizing1_max = 1.7

        while (run):
            totala = time.time() * 1000000

            if (self.sub1 == "TakeOff"):
                if ((self.sub2 == "TakingOff_ghost")
                        or (self.sub2 == "Stabilizing1_ghost")):
                    ghostStateIndex = self.StatesSub2.index(self.sub2)
                    self.sub2 = self.StatesSub2[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub2 == "TakingOff"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_TakingOff_max):
                            self.sub2 = "Stabilizing1"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'Stabilizing1')
                            t_TakingOff_max = 2

                elif (self.sub2 == "Stabilizing1"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_Stabilizing1_max):
                            self.sub2 = "TakingOff"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'TakingOff')
                            t_Stabilizing1_max = 1.7

                # Actuation if
                if (self.sub2 == "TakingOff"):
                    print "Taking Off"
                    self.lock.acquire()
                    self.extraPrx.takeoff()
                    self.lock.release()
            else:
                if (self.sub2 == "TakingOff"):
                    t_TakingOff_max = 2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
                    self.sub2 = self.StatesSub2[ghostStateIndex]
                elif (self.sub2 == "Stabilizing1"):
                    t_Stabilizing1_max = 1.7 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
                    self.sub2 = self.StatesSub2[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_Stabilizing2_max = 3
        t_goneFront_max = 0.2

        initPos = 0
        pos = 0

        while (run):
            totala = time.time() * 1000000

            if (self.sub1 == "GoFront"):
                if ((self.sub3 == "GoingFront_ghost")
                        or (self.sub3 == "Stabilizing2_ghost")
                        or (self.sub3 == "goneFront_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "GoingFront"):
                    if (pos - initPos > 300):
                        self.sub3 = "Stabilizing2"
                        print "300m reached"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'Stabilizing2')

                elif (self.sub3 == "Stabilizing2"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_Stabilizing2_max):
                            self.sub3 = "goneFront"
                            t_activated = False
                            print "from stabilizing2 to GoneFront"
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'goneFront')
                            t_Stabilizing2_max = 3

                elif (self.sub3 == "goneFront"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_goneFront_max):
                            self.sub3 = "GoingFront"
                            t_activated = False
                            initPos = 0
                            pos = 0
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoingFront')
                            t_goneFront_max = 0.2

                # Actuation if
                if (self.sub3 == "GoingFront"):
                    pos = self.pose3dPrx.getPose3DData().x * 100

                    if initPos == 0:
                        initPos = self.pose3dPrx.getPose3DData().x * 100

                    print "pos:", pos, "init:", initPos
                    print "distancia", pos - initPos

                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 1
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub3 == "Stabilizing2"):
                    cmd.linearX = 0
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub3 == "goneFront"):
                    self.goneFront = True
            else:
                if (self.sub3 == "GoingFront"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "Stabilizing2"):
                    t_Stabilizing2_max = 3 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "goneFront"):
                    t_goneFront_max = 0.2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata4(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_empty1_max = 0.8
        t_empty2_max = 0.8

        while (run):
            totala = time.time() * 1000000

            if (self.sub3 == "Stabilizing2"):
                if ((self.sub4 == "empty1_ghost")
                        or (self.sub4 == "empty2_ghost")):
                    ghostStateIndex = self.StatesSub4.index(self.sub4)
                    self.sub4 = self.StatesSub4[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub4 == "empty1"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_empty1_max):
                            self.sub4 = "empty2"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'empty2')
                            t_empty1_max = 0.8

                elif (self.sub4 == "empty2"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_empty2_max):
                            self.sub4 = "empty1"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'empty1')
                            t_empty2_max = 0.8

                # Actuation if
            else:
                if (self.sub4 == "empty1"):
                    t_empty1_max = 0.8 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]
                elif (self.sub4 == "empty2"):
                    t_empty2_max = 0.8 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub4.index(self.sub4) + 1
                    self.sub4 = self.StatesSub4[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata5(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.goneToTop = False
        self.goneToLeft = False
        self.goneToBottom = False
        self.goneToRigth = False

        while (run):
            totala = time.time() * 1000000

            if (self.sub1 == "DoSquare"):
                if ((self.sub5 == "DoingRigthSide_ghost")
                        or (self.sub5 == "DoingTopSide_ghost")
                        or (self.sub5 == "DoingLeftSide_ghost")
                        or (self.sub5 == "DoingBottomSide_ghost")):
                    ghostStateIndex = self.StatesSub5.index(self.sub5)
                    self.sub5 = self.StatesSub5[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub5 == "DoingRigthSide"):
                    if (self.goneToTop):
                        self.sub5 = "DoingTopSide"
                        print "Rigth Done"
                        self.goneToTop = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'DoingTopSide')

                elif (self.sub5 == "DoingTopSide"):
                    if (self.goneToLeft):
                        self.sub5 = "DoingLeftSide"
                        print "Left done"
                        self.goneToLeft = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'DoingLeftSide')

                elif (self.sub5 == "DoingLeftSide"):
                    if (self.goneToBottom):
                        self.sub5 = "DoingBottomSide"
                        print "Left done"
                        self.goneToBottom = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'DoingBottomSide')

                elif (self.sub5 == "DoingBottomSide"):
                    if (self.goneToRigth):
                        self.sub5 = "DoingRigthSide"

                        self.squareDone = True
                        print "Bottom done"
                        self.goneToRigth = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'DoingRigthSide')

                # Actuation if
                if (self.sub5 == "DoingRigthSide"):
                    print "Doing Rigth Side"
                elif (self.sub5 == "DoingTopSide"):
                    print "Doing Top Side"
                elif (self.sub5 == "DoingLeftSide"):
                    print "Doing Left Side"
                elif (self.sub5 == "DoingBottomSide"):
                    print "Doing Bottom Side"
            else:
                if (self.sub5 == "DoingRigthSide"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "DoingTopSide"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "DoingLeftSide"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "DoingBottomSide"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata6(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_GoingToTop_max = 3
        t_StabilizingTop_max = 2
        t_GoneToTop_max = 0.1

        while (run):
            totala = time.time() * 1000000

            if (self.sub5 == "DoingRigthSide"):
                if ((self.sub6 == "GoingToTop_ghost")
                        or (self.sub6 == "StabilizingTop_ghost")
                        or (self.sub6 == "GoneToTop_ghost")):
                    ghostStateIndex = self.StatesSub6.index(self.sub6)
                    self.sub6 = self.StatesSub6[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub6 == "GoingToTop"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoingToTop_max):
                            self.sub6 = "StabilizingTop"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'StabilizingTop')
                            t_GoingToTop_max = 3

                elif (self.sub6 == "StabilizingTop"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_StabilizingTop_max):
                            self.sub6 = "GoneToTop"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoneToTop')
                            t_StabilizingTop_max = 2

                elif (self.sub6 == "GoneToTop"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoneToTop_max):
                            self.sub6 = "GoingToTop"
                            t_activated = False
                            self.GoneToTop = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoingToTop')
                            t_GoneToTop_max = 0.1

                # Actuation if
                if (self.sub6 == "GoingToTop"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0.75
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub6 == "StabilizingTop"):
                    cmd.linearX = 0
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub6 == "GoneToTop"):
                    self.goneToTop = True
            else:
                if (self.sub6 == "GoingToTop"):
                    t_GoingToTop_max = 3 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]
                elif (self.sub6 == "StabilizingTop"):
                    t_StabilizingTop_max = 2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]
                elif (self.sub6 == "GoneToTop"):
                    t_GoneToTop_max = 0.1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata8(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_GoingToLeft_max = 3
        t_StabilizingLeft_max = 2
        t_GoneToLeft_max = 0.1

        while (run):
            totala = time.time() * 1000000

            if (self.sub5 == "DoingTopSide"):
                if ((self.sub8 == "GoingToLeft_ghost")
                        or (self.sub8 == "StabilizingLeft_ghost")
                        or (self.sub8 == "GoneToLeft_ghost")):
                    ghostStateIndex = self.StatesSub8.index(self.sub8)
                    self.sub8 = self.StatesSub8[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub8 == "GoingToLeft"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoingToLeft_max):
                            self.sub8 = "StabilizingLeft"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'StabilizingLeft')
                            t_GoingToLeft_max = 3

                elif (self.sub8 == "StabilizingLeft"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_StabilizingLeft_max):
                            self.sub8 = "GoneToLeft"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoneToLeft')
                            t_StabilizingLeft_max = 2

                elif (self.sub8 == "GoneToLeft"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoneToLeft_max):
                            self.sub8 = "GoingToLeft"
                            t_activated = False
                            self.goneToLeft = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoingToLeft')
                            t_GoneToLeft_max = 0.1

                # Actuation if
                if (self.sub8 == "GoingToLeft"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0
                    cmd.linearY = 0.75
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub8 == "StabilizingLeft"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub8 == "GoneToLeft"):
                    self.goneToLeft = True
            else:
                if (self.sub8 == "GoingToLeft"):
                    t_GoingToLeft_max = 3 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
                    self.sub8 = self.StatesSub8[ghostStateIndex]
                elif (self.sub8 == "StabilizingLeft"):
                    t_StabilizingLeft_max = 2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
                    self.sub8 = self.StatesSub8[ghostStateIndex]
                elif (self.sub8 == "GoneToLeft"):
                    t_GoneToLeft_max = 0.1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub8.index(self.sub8) + 1
                    self.sub8 = self.StatesSub8[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata9(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_GoingToBottom_max = 3
        t_StabilizingBottom_max = 2
        t_GoneToBottom_max = 0.1

        while (run):
            totala = time.time() * 1000000

            if (self.sub5 == "DoingLeftSide"):
                if ((self.sub9 == "GoingToBottom_ghost")
                        or (self.sub9 == "StabilizingBottom_ghost")
                        or (self.sub9 == "GoneToBottom_ghost")):
                    ghostStateIndex = self.StatesSub9.index(self.sub9)
                    self.sub9 = self.StatesSub9[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub9 == "GoingToBottom"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoingToBottom_max):
                            self.sub9 = "StabilizingBottom"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'StabilizingBottom')
                            t_GoingToBottom_max = 3

                elif (self.sub9 == "StabilizingBottom"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_StabilizingBottom_max):
                            self.sub9 = "GoneToBottom"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoneToBottom')
                            t_StabilizingBottom_max = 2

                elif (self.sub9 == "GoneToBottom"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoneToBottom_max):
                            self.sub9 = "GoingToBottom"
                            t_activated = False
                            self.goneToBottom = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoingToBottom')
                            t_GoneToBottom_max = 0.1

                # Actuation if
                if (self.sub9 == "GoingToBottom"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = -0.75
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub9 == "StabilizingBottom"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub9 == "GoneToBottom"):
                    self.goneToBottom = True
            else:
                if (self.sub9 == "GoingToBottom"):
                    t_GoingToBottom_max = 3 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
                    self.sub9 = self.StatesSub9[ghostStateIndex]
                elif (self.sub9 == "StabilizingBottom"):
                    t_StabilizingBottom_max = 2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
                    self.sub9 = self.StatesSub9[ghostStateIndex]
                elif (self.sub9 == "GoneToBottom"):
                    t_GoneToBottom_max = 0.1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub9.index(self.sub9) + 1
                    self.sub9 = self.StatesSub9[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata10(self):
        run = True
        cycle = 100
        t_activated = False
        t_fin = 0

        t_GoingToRigth_max = 3
        t_StabilizingRigth_max = 2
        t_GoneToRigth_max = 0.1

        while (run):
            totala = time.time() * 1000000

            if (self.sub5 == "DoingBottomSide"):
                if ((self.sub10 == "GoingToRigth_ghost")
                        or (self.sub10 == "StabilizingRigth_ghost")
                        or (self.sub10 == "GoneToRigth_ghost")):
                    ghostStateIndex = self.StatesSub10.index(self.sub10)
                    self.sub10 = self.StatesSub10[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub10 == "GoingToRigth"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoingToRigth_max):
                            self.sub10 = "StabilizingRigth"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'StabilizingRigth')
                            t_GoingToRigth_max = 3

                elif (self.sub10 == "StabilizingRigth"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_StabilizingRigth_max):
                            self.sub10 = "GoneToRigth"
                            t_activated = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoneToRigth')
                            t_StabilizingRigth_max = 2

                elif (self.sub10 == "GoneToRigth"):
                    if (not t_activated):
                        t_ini = time.time()
                        t_activated = True
                    else:
                        t_fin = time.time()
                        secs = t_fin - t_ini
                        if (secs > t_GoneToRigth_max):
                            self.sub10 = "GoingToRigth"
                            t_activated = False
                            self.goneToRigth = False
                            if self.displayGui:
                                self.automataGui.notifySetNodeAsActive(
                                    'GoingToRigth')
                            t_GoneToRigth_max = 0.1

                # Actuation if
                if (self.sub10 == "GoingToRigth"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0
                    cmd.linearY = -0.75
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub10 == "StabilizingRigth"):
                    cmd = jderobot.CMDVelData()
                    cmd.linearX = 0
                    cmd.linearY = 0
                    cmd.linearZ = 0
                    self.lock.acquire()
                    self.cmdVelPrx.setCMDVelData(cmd)
                    self.lock.release()
                elif (self.sub10 == "GoneToRigth"):
                    self.goneToRigth = True
            else:
                if (self.sub10 == "GoingToRigth"):
                    t_GoingToRigth_max = 3 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
                    self.sub10 = self.StatesSub10[ghostStateIndex]
                elif (self.sub10 == "StabilizingRigth"):
                    t_StabilizingRigth_max = 2 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
                    self.sub10 = self.StatesSub10[ghostStateIndex]
                elif (self.sub10 == "GoneToRigth"):
                    t_GoneToRigth_max = 0.1 - (t_fin - t_ini)
                    ghostStateIndex = self.StatesSub10.index(self.sub10) + 1
                    self.sub10 = self.StatesSub10[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to pose3d
        pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
        if (not pose3d):
            raise Exception('could not create proxy with pose3d')
        self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
        if (not self.pose3dPrx):
            raise Exception('invalid proxy automata.Pose3D.Proxy')
        print 'pose3d connected'

        # Contact to cmdVel
        cmdVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
        if (not cmdVel):
            raise Exception('could not create proxy with cmdVel')
        self.cmdVelPrx = CMDVelPrx.checkedCast(cmdVel)
        if (not self.cmdVelPrx):
            raise Exception('invalid proxy automata.CMDVel.Proxy')
        print 'cmdVel connected'

        # Contact to extra
        extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
        if (not extra):
            raise Exception('could not create proxy with extra')
        self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
        if (not self.extraPrx):
            raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
        print 'extra connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()
        self.t2.join()
        self.t3.join()
        self.t4.join()
        self.t5.join()
        self.t6.join()
        self.t8.join()
        self.t9.join()
        self.t10.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'
示例#21
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "TakeOff",
            "FollowRoad",
            "MonitorArea",
            "TurnAround",
            "Landing",
            "END",
            "HeighControl",
        ]

        self.StatesSub3 = [
            "FindRoad",
            "FindRoad_ghost",
            "FollowingRoad",
            "FollowingRoad_ghost",
        ]

        self.StatesSub5 = [
            "ToFirstPos",
            "ToFirstPos_ghost",
            "ToSecondPos",
            "ToSecondPos_ghost",
            "ToThirdPos",
            "ToThirdPos_ghost",
            "Return",
            "Return_ghost",
        ]

        self.StatesSub6 = [
            "Descending",
            "Descending_ghost",
            "Land",
            "Land_ghost",
        ]

        self.sub1 = "TakeOff"
        self.run1 = True
        self.sub3 = "FindRoad_ghost"
        self.run3 = True
        self.sub5 = "ToFirstPos_ghost"
        self.run5 = True
        self.sub6 = "Descending_ghost"
        self.run6 = True

    def getImage(self):
        img = self.CameraPrx.getImageData("RGB8")
        height = img.description.height
        width = img.description.width
        imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
        imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
        imgPixels.shape = height, width, 3
        return imgPixels

    def setVelocity(self, vx, vy, vz, yaw):
        cmd = jderobot.CMDVelData()
        cmd.linearX = vy
        cmd.linearY = vx
        cmd.linearZ = vz
        cmd.angularZ = yaw
        cmd.angularX = cmd.angularY = 1.0
        self.CMDVelPrx.setCMDVelData(cmd)

    #The factor indicate the margin of the error multipliyin the error for this factor
    def droneInPosition(self, pos, factor=1):
        return (abs(pos[0] - self.xPos) < self.minDist * factor) and (
            abs(pos[1] - self.yPos) < self.minDist * factor)

    class PID:
        def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
            self.Ep = Epsilon
            self.Kp = Kp
            self.Ki = Ki
            self.Kd = Kd

            self.Imax = Imax
            self.Imin = Imin
            self.lastError = 0
            self.cumulativeError = 0

        def updateCumulativeError(self, error):
            self.cumulativeError += error
            if self.cumulativeError > self.Imax:
                self.cumulativeError = self.Imax
            elif self.cumulativeError < self.Imin:
                self.cumulativeError = self.Imin

        def process(self, error, derivative=None, integral=None):
            if -self.Ep < error < self.Ep:
                return 0

            P = self.Kp * error
            self.updateCumulativeError(error)
            if integral != None:
                I = self.Ki * integral
            else:
                I = self.Ki * self.cumulativeError

            if derivative != None:
                D = self.Kd * derivative
            else:
                D = self.Kd * (error - self.lastError)
            self.lastError = error
            speed = P + I + D
            if speed > 3:
                speed = 3
            elif speed < -3:
                speed = -3
            return speed

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()
        self.t5 = threading.Thread(target=self.subautomata5)
        self.t5.start()
        self.t6 = threading.Thread(target=self.subautomata6)
        self.t6.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 62, 66, 1, 'TakeOff')
        guiSubautomata1.newGuiNode(2, 3, 189, 116, 0, 'FollowRoad')
        guiSubautomata1.newGuiNode(6, 5, 309, 268, 0, 'MonitorArea')
        guiSubautomata1.newGuiNode(11, 0, 263, 428, 0, 'TurnAround')
        guiSubautomata1.newGuiNode(12, 6, 53, 239, 0, 'Landing')
        guiSubautomata1.newGuiNode(14, 0, 41, 427, 0, 'END')
        guiSubautomata1.newGuiNode(17, 0, 481, 139, 0, 'HeighControl')

        guiSubautomata1.newGuiTransition((189, 116), (309, 268), (274, 172), 7,
                                         2, 6)
        guiSubautomata1.newGuiTransition((309, 268), (263, 428), (349, 362),
                                         13, 6, 11)
        guiSubautomata1.newGuiTransition((263, 428), (189, 116), (164, 318),
                                         14, 11, 2)
        guiSubautomata1.newGuiTransition((189, 116), (53, 239), (82, 154), 19,
                                         2, 12)
        guiSubautomata1.newGuiTransition((53, 239), (41, 427), (43, 326), 22,
                                         12, 14)
        guiSubautomata1.newGuiTransition((481, 139), (189, 116), (328, 116),
                                         26, 17, 2)
        guiSubautomata1.newGuiTransition((62, 66), (481, 139), (430, 27), 27,
                                         1, 17)
        guiSubautomata1.newGuiTransition((189, 116), (481, 139), (315, 70), 28,
                                         2, 17)
        guiSubautomata1.newGuiTransition((309, 268), (481, 139), (378, 195),
                                         29, 6, 17)
        guiSubautomata1.newGuiTransition((481, 139), (309, 268), (412, 279),
                                         30, 17, 6)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 2, self.automataGui)

        guiSubautomata3.newGuiNode(3, 0, 156, 228, 1, 'FindRoad')
        guiSubautomata3.newGuiNode(4, 0, 427, 255, 0, 'FollowingRoad')

        guiSubautomata3.newGuiTransition((156, 228), (427, 255), (297, 157), 2,
                                         3, 4)
        guiSubautomata3.newGuiTransition((427, 255), (156, 228), (265, 320), 3,
                                         4, 3)
        guiSubautomataList.append(guiSubautomata3)

        # Creating subAutomata5
        guiSubautomata5 = GuiSubautomata(5, 6, self.automataGui)

        guiSubautomata5.newGuiNode(7, 0, 86, 275, 1, 'ToFirstPos')
        guiSubautomata5.newGuiNode(8, 0, 247, 92, 0, 'ToSecondPos')
        guiSubautomata5.newGuiNode(9, 0, 491, 303, 0, 'ToThirdPos')
        guiSubautomata5.newGuiNode(10, 0, 281, 455, 0, 'Return')

        guiSubautomata5.newGuiTransition((86, 275), (247, 92), (166, 184), 9,
                                         7, 8)
        guiSubautomata5.newGuiTransition((247, 92), (491, 303), (369, 198), 10,
                                         8, 9)
        guiSubautomata5.newGuiTransition((491, 303), (281, 455), (386, 379),
                                         11, 9, 10)
        guiSubautomata5.newGuiTransition((281, 455), (86, 275), (184, 365), 12,
                                         10, 7)
        guiSubautomataList.append(guiSubautomata5)

        # Creating subAutomata6
        guiSubautomata6 = GuiSubautomata(6, 12, self.automataGui)

        guiSubautomata6.newGuiNode(15, 0, 126, 185, 1, 'Descending')
        guiSubautomata6.newGuiNode(16, 0, 350, 190, 0, 'Land')

        guiSubautomata6.newGuiTransition((126, 185), (350, 190), (232, 220),
                                         24, 15, 16)
        guiSubautomataList.append(guiSubautomata6)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run3 = False
        self.run5 = False
        self.run6 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.targetX = 125
        self.targetY = 125
        self.targetZ = 2.5
        self.initPos = (-1, -8.5)
        landingPose = (self.Pose3DPrx.getPose3DData().x,
                       self.Pose3DPrx.getPose3DData().y)

        #Minimun errors
        self.minError = 10
        self.minActit = 0.5
        self.minAltit = 0.01
        self.minDist = 0.01
        heighMargin = 1

        #Control Variables
        self.heigh = 0
        self.contours = []
        self.monitorComplet = False
        self.xPos = landingPose[0]
        self.yPos = landingPose[1]
        initAngle = None
        self.hasLanded = False
        currentState = "FollowRoad"
        takenOff = False

        #Filter Values
        self.hmin = 90
        self.hmax = 97
        self.vmin = 0
        self.vmax = 50
        self.smin = 45
        self.smax = 80

        #Control PID
        self.xPid = self.PID(Epsilon=self.minError,
                             Kp=0.01,
                             Ki=0.04,
                             Kd=0.003,
                             Imax=5,
                             Imin=-5)
        self.zPid = self.PID(Epsilon=self.minAltit,
                             Kp=1,
                             Ki=0.02,
                             Kd=0,
                             Imax=5,
                             Imin=-5)
        self.aPid = self.PID(Epsilon=self.minActit,
                             Kp=0.01,
                             Ki=0.04,
                             Kd=0.003,
                             Imax=5,
                             Imin=-5)

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "TakeOff"):
                if (takenOff):
                    self.sub1 = "HeighControl"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('HeighControl')

            elif (self.sub1 == "FollowRoad"):
                if ((self.droneInPosition(self.initPos, 200))
                        and (not self.monitorComplet)):
                    self.sub1 = "MonitorArea"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('MonitorArea')

                elif ((self.droneInPosition(landingPose, 75))
                      and (self.monitorComplet)):
                    self.sub1 = "Landing"
                    self.setVelocity(0, 0, 0, 0)
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('Landing')

                elif (abs(self.targetZ - self.heigh) > heighMargin):
                    self.sub1 = "HeighControl"
                    currentState = "FollowRoad"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('HeighControl')

            elif (self.sub1 == "MonitorArea"):
                if (self.monitorComplet):
                    self.sub1 = "TurnAround"
                    self.setVelocity(0, 0, 0, 0)
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('TurnAround')

                elif (abs(self.targetZ - self.heigh) > heighMargin):
                    self.sub1 = "HeighControl"
                    currentState = "MonitorArea"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('HeighControl')

            elif (self.sub1 == "TurnAround"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 2.5):
                        self.sub1 = "FollowRoad"
                        t_activated = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowRoad')

            elif (self.sub1 == "Landing"):
                if (self.hasLanded):
                    self.sub1 = "END"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('END')

            elif (self.sub1 == "HeighControl"):
                if ((abs(self.targetZ - self.heigh) < self.minAltit)
                        and (currentState == "FollowRoad")):
                    self.sub1 = "FollowRoad"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('FollowRoad')

                elif ((abs(self.targetZ - self.heigh) < self.minAltit)
                      and (currentState == "MonitorArea")):
                    self.sub1 = "MonitorArea"
                    if self.displayGui:
                        self.automataGui.notifySetNodeAsActive('MonitorArea')

            # Actuation if
            if (self.sub1 == "TakeOff"):
                #Taking Off
                self.ExtraPrx.takeoff()
                takenOff = True
            elif (self.sub1 == "FollowRoad"):
                lastState = "FollowRoad"

                #Get and prepare image
                inImg = self.getImage()
                softenedImg = cv2.bilateralFilter(inImg, 9, 75, 75)
                hsvImg = cv2.cvtColor(softenedImg, cv2.COLOR_BGR2HSV)

                #Get threshold image
                minValues = numpy.array([self.hmin, self.vmin, self.smin])
                maxValues = numpy.array([self.hmax, self.vmax, self.smax])
                thresholdImg = cv2.inRange(hsvImg, minValues, maxValues)

                #Get contours
                self.contours, hierarchy = cv2.findContours(
                    thresholdImg, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

                #Update Heigh
                self.heigh = self.Pose3DPrx.getPose3DData().z
            elif (self.sub1 == "TurnAround"):
                self.setVelocity(0, 0, 0, 1)
                pose = self.Pose3DPrx.getPose3DData()
                print "q0", pose.q0, "q1", pose.q1, "q2", pose.q2, "q3", pose.q3
            elif (self.sub1 == "END"):
                print "SHUT DOWN"
                self.shutDown()
            elif (self.sub1 == "HeighControl"):
                #Controling heigh
                self.heigh = self.Pose3DPrx.getPose3DData().z
                zVel = self.zPid.process(self.targetZ - self.heigh)
                self.setVelocity(0, 0, zVel, 0)
                print "Heigh:", self.heigh
                print "Vel:", zVel

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        self.run3 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run3):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowRoad"):
                if ((self.sub3 == "FindRoad_ghost")
                        or (self.sub3 == "FollowingRoad_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "FindRoad"):
                    if (self.contours):
                        self.sub3 = "FollowingRoad"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowingRoad')

                elif (self.sub3 == "FollowingRoad"):
                    if (not self.contours):
                        self.sub3 = "FindRoad"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('FindRoad')

                # Actuation if
                if (self.sub3 == "FindRoad"):
                    self.setVelocity(0, 0, 0.75, 0)
                    print "Road lost"
                elif (self.sub3 == "FollowingRoad"):
                    contour = max(self.contours, key=cv2.contourArea)
                    try:
                        (x, y), radius = cv2.minEnclosingCircle(contour)
                        center = (int(x), int(y))
                        ellipse = cv2.fitEllipse(contour)
                        inclination = ellipse[2]

                        if inclination < 90:
                            yAngle = -inclination
                        else:
                            yAngle = 180 - inclination

                        avel = self.aPid.process(yAngle)
                        xError = self.targetX - center[0]
                        xvel = self.xPid.process(xError)
                        speed = max(
                            (self.targetX - pow(1.09, abs(xError))) / 125., 0)

                        self.setVelocity(xvel, speed, 0, avel)
                    except:
                        print "EXCEPTION FOUND"
                        self.contours = []

                    self.xPos = self.Pose3DPrx.getPose3DData().x
                    self.yPos = self.Pose3DPrx.getPose3DData().y
                    print "Xpos:", self.xPos, "Ypos:", self.yPos
            else:
                if (self.sub3 == "FindRoad"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "FollowingRoad"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata5(self):
        self.run5 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        firstPos = (10, -20)
        secondPos = (5, -30)
        thirdPos = (-10, -20)

        self.minDist = 0.02

        #Control PID
        xDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)
        yDistPid = self.PID(self.minDist, 1, 0.005, 0.003, 5, -5)

        while (self.run5):
            totala = time.time() * 1000000

            if (self.sub1 == "MonitorArea"):
                if ((self.sub5 == "ToFirstPos_ghost")
                        or (self.sub5 == "ToSecondPos_ghost")
                        or (self.sub5 == "ToThirdPos_ghost")
                        or (self.sub5 == "Return_ghost")):
                    ghostStateIndex = self.StatesSub5.index(self.sub5)
                    self.sub5 = self.StatesSub5[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub5 == "ToFirstPos"):
                    if (self.droneInPosition(firstPos)):
                        self.sub5 = "ToSecondPos"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'ToSecondPos')

                elif (self.sub5 == "ToSecondPos"):
                    if (self.droneInPosition(secondPos)):
                        self.sub5 = "ToThirdPos"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'ToThirdPos')

                elif (self.sub5 == "ToThirdPos"):
                    if (self.droneInPosition(thirdPos)):
                        self.sub5 = "Return"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Return')

                elif (self.sub5 == "Return"):
                    if (self.droneInPosition(self.initPos)):
                        self.sub5 = "ToFirstPos"
                        self.monitorComplet = True
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'ToFirstPos')

                # Actuation if
                if (self.sub5 == "ToFirstPos"):
                    self.xPos = self.Pose3DPrx.getPose3DData().x
                    self.yPos = self.Pose3DPrx.getPose3DData().y

                    xError = firstPos[0] - self.xPos
                    xVel = xDistPid.process(xError)
                    yError = firstPos[1] - self.yPos
                    yVel = yDistPid.process(yError)

                    print "Xerror:", xError, "Yerror:", yError
                    print "Xvel:", xVel, "Yvel:", -yVel

                    self.setVelocity(xVel, -yVel, 0, 0)
                elif (self.sub5 == "ToSecondPos"):
                    self.xPos = self.Pose3DPrx.getPose3DData().x
                    self.yPos = self.Pose3DPrx.getPose3DData().y

                    xError = secondPos[0] - self.xPos
                    xVel = xDistPid.process(xError)
                    yError = secondPos[1] - self.yPos
                    yVel = yDistPid.process(yError)

                    print "xError:", xError, "Yerror:", yError
                    print "Xvel:", xVel, "Yvel:", -yVel

                    self.setVelocity(xVel, -yVel, 0, 0)
                elif (self.sub5 == "ToThirdPos"):
                    self.xPos = self.Pose3DPrx.getPose3DData().x
                    self.yPos = self.Pose3DPrx.getPose3DData().y

                    xError = thirdPos[0] - self.xPos
                    xVel = xDistPid.process(xError)
                    yError = thirdPos[1] - self.yPos
                    yVel = yDistPid.process(yError)

                    print "Xerror:", xError, "Yerror:", yError
                    print "Xvel:", xVel, "Yvel:", yVel
                    print "Xvel:", xVel, "Yvel:", -yVel

                    self.setVelocity(xVel, -yVel, 0, 0)
                elif (self.sub5 == "Return"):
                    self.xPos = self.Pose3DPrx.getPose3DData().x
                    self.yPos = self.Pose3DPrx.getPose3DData().y

                    xError = self.initPos[0] - self.xPos
                    xVel = xDistPid.process(xError)
                    yError = self.initPos[1] - self.yPos
                    yVel = yDistPid.process(yError)

                    print "Xerror:", xError, "Yerror:", yError
                    print "Xvel:", xVel, "Yvel:", -yVel

                    self.setVelocity(xVel, -yVel, 0, 0)
            else:
                if (self.sub5 == "ToFirstPos"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "ToSecondPos"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "ToThirdPos"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]
                elif (self.sub5 == "Return"):
                    ghostStateIndex = self.StatesSub5.index(self.sub5) + 1
                    self.sub5 = self.StatesSub5[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata6(self):
        self.run6 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        targetZDescend = 0.5

        while (self.run6):
            totala = time.time() * 1000000

            if (self.sub1 == "Landing"):
                if ((self.sub6 == "Descending_ghost")
                        or (self.sub6 == "Land_ghost")):
                    ghostStateIndex = self.StatesSub6.index(self.sub6)
                    self.sub6 = self.StatesSub6[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub6 == "Descending"):
                    if (abs(targetZDescend - self.heigh) < self.minAltit):
                        self.sub6 = "Land"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Land')

                # Actuation if
                if (self.sub6 == "Descending"):
                    #Controling heigh
                    self.heigh = self.Pose3DPrx.getPose3DData().z
                    zVel = self.zPid.process(targetZDescend - self.heigh)
                    self.setVelocity(0, 0, zVel, 0)
                    print "Heigh:", self.heigh
                elif (self.sub6 == "Land"):
                    self.setVelocity(0, 0, 0, 0)
                    if not self.hasLanded:
                        self.ExtraPrx.land()
                        self.hasLanded = True
            else:
                if (self.sub6 == "Descending"):
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]
                elif (self.sub6 == "Land"):
                    ghostStateIndex = self.StatesSub6.index(self.sub6) + 1
                    self.sub6 = self.StatesSub6[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to Extra
        Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
        if (not Extra):
            raise Exception('could not create proxy with Extra')
        self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
        if (not self.ExtraPrx):
            raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
        print 'Extra connected'

        # Contact to CMDVel
        CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
        if (not CMDVel):
            raise Exception('could not create proxy with CMDVel')
        self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
        if (not self.CMDVelPrx):
            raise Exception('invalid proxy automata.CMDVel.Proxy')
        print 'CMDVel connected'

        # Contact to Pose3D
        Pose3D = self.ic.propertyToProxy('automata.Pose3D.Proxy')
        if (not Pose3D):
            raise Exception('could not create proxy with Pose3D')
        self.Pose3DPrx = Pose3DPrx.checkedCast(Pose3D)
        if (not self.Pose3DPrx):
            raise Exception('invalid proxy automata.Pose3D.Proxy')
        print 'Pose3D connected'

        # Contact to Camera
        Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
        if (not Camera):
            raise Exception('could not create proxy with Camera')
        self.CameraPrx = CameraPrx.checkedCast(Camera)
        if (not self.CameraPrx):
            raise Exception('invalid proxy automata.Camera.Proxy')
        print 'Camera connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()
        self.t3.join()
        self.t5.join()
        self.t6.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'
示例#22
0
class Automata():

	def __init__(self):
		self.lock = threading.Lock()
		self.StatesSub1 = [
			"TakeOff",
			"Stabilizing1",
			"GoFront",
			"Stoping",
			"Landing",
			"Stabilizing2",
		]

		self.StatesSub3 = [
			"A",
			"A_ghost",
			"B",
			"B_ghost",
		]

		self.StatesSub5 = [
			"C",
			"C_ghost",
		]

		self.sub1 = "TakeOff"
		self.run1 = True
		self.sub3 = "A_ghost"
		self.run3 = True
		self.sub5 = "C_ghost"
		self.run5 = True

	def createAutomata(self):
		guiSubautomataList = []

		# Creating subAutomata1
		guiSubautomata1 = GuiSubautomata(1,0, self.automataGui)

		guiSubautomata1.newGuiNode(1, 3, 61, 101, 1, 'TakeOff')
		guiSubautomata1.newGuiNode(2, 0, 283, 75, 0, 'Stabilizing1')
		guiSubautomata1.newGuiNode(3, 0, 497, 130, 0, 'GoFront')
		guiSubautomata1.newGuiNode(4, 0, 489, 320, 0, 'Stoping')
		guiSubautomata1.newGuiNode(5, 0, 250, 408, 0, 'Landing')
		guiSubautomata1.newGuiNode(6, 0, 66, 283, 0, 'Stabilizing2')

		guiSubautomata1.newGuiTransition((61, 101), (283, 75), (9, 212), 1, 1, 2)
		guiSubautomata1.newGuiTransition((283, 75), (497, 130), (413, 79), 2, 2, 3)
		guiSubautomata1.newGuiTransition((497, 130), (489, 320), (570, 239), 3, 3, 4)
		guiSubautomata1.newGuiTransition((489, 320), (250, 408), (410, 411), 4, 4, 5)
		guiSubautomata1.newGuiTransition((250, 408), (66, 283), (152, 338), 6, 5, 6)
		guiSubautomata1.newGuiTransition((66, 283), (61, 101), (9, 212), 1, 6, 1)
		guiSubautomataList.append(guiSubautomata1)

		# Creating subAutomata3
		guiSubautomata3 = GuiSubautomata(3,1, self.automataGui)

		guiSubautomata3.newGuiNode(7, 0, 175, 146, 1, 'A')
		guiSubautomata3.newGuiNode(8, 5, 468, 201, 0, 'B')

		guiSubautomata3.newGuiTransition((175, 146), (468, 201), (348, 79), 1, 7, 8)
		guiSubautomata3.newGuiTransition((468, 201), (175, 146), (303, 215), 2, 8, 7)
		guiSubautomataList.append(guiSubautomata3)

		# Creating subAutomata5
		guiSubautomata5 = GuiSubautomata(5,3, self.automataGui)

		guiSubautomata5.newGuiNode(9, 0, 70, 146, 1, 'C')

		guiSubautomata5.newGuiTransition((70, 146), (70, 146), (70, 186), 3, 9, 9)
		guiSubautomataList.append(guiSubautomata5)


		return guiSubautomataList

	def shutDown(self):
		self.run1 = False
		self.run3 = False
		self.run5 = False

	def runGui(self):
		app = QtGui.QApplication(sys.argv)
		self.automataGui = AutomataGui()
		self.automataGui.setAutomata(self.createAutomata())
		self.automataGui.loadAutomata()
		self.automataGui.show()
		app.exec_()

	def subautomata1(self):
		run = True
		cycle = 100
		t_activated = False

		hasTakenOff = False
		hasLanded = False

		while(run):
			totala = time.time() * 1000000

			# Evaluation if
			if(self.sub1 == "TakeOff"):
				if(hasTakenOff):
					self.sub1 = "Stabilizing1"
					print "Going to Stabilizing"
				self.automataGui.notifySetNodeAsActive('Stabilizing1')

			elif(self.sub1 == "Stabilizing1"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 1.5):
						self.sub1 = "GoFront"
						t_activated = False
						print "going to GoFront"
						self.automataGui.notifySetNodeAsActive('GoFront')

			elif(self.sub1 == "GoFront"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "Stoping"
						t_activated = False
						print "going to Stoping"
						self.automataGui.notifySetNodeAsActive('Stoping')

			elif(self.sub1 == "Stoping"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3.5):
						self.sub1 = "Landing"
						t_activated = False
						print "going to Land"
						self.automataGui.notifySetNodeAsActive('Landing')

			elif(self.sub1 == "Landing"):
				if(hasLanded):
					self.sub1 = "Stabilizing2"

				self.automataGui.notifySetNodeAsActive('Stabilizing2')

			elif(self.sub1 == "Stabilizing2"):
				if(not t_activated):
					t_ini = time.time()
					t_activated = True
				else:
					t_fin = time.time()
					secs = t_fin - t_ini
					if(secs > 3):
						self.sub1 = "TakeOff"
						t_activated = False
						print "Restarting"
						self.automataGui.notifySetNodeAsActive('TakeOff')


			# Actuation if
			if(self.sub1 == "TakeOff"):
				print "Taking Off"
				self.lock.acquire()
				self.extraPrx.takeoff()
				self.lock.release()
				hasTakenOff = True
			elif(self.sub1 == "Stabilizing1"):
				print "Stabilizing"
			elif(self.sub1 == "GoFront"):
				cmd = jderobot.CMDVelData()
				cmd.linearX = 1
				cmd.linearY = 0
				cmd.linearZ = 0
				self.lock.acquire()
				self.cmdPrx.setCMDVelData(cmd)
				self.lock.release()
			elif(self.sub1 == "Stoping"):
				cmd = jderobot.CMDVelData()
				cmd.linearX = 0
				cmd.linearY = 0
				cmd.linearZ = 0
				self.lock.acquire()
				self.cmdPrx.setCMDVelData(cmd)
				self.lock.release()
				print "Stoping"
			elif(self.sub1 == "Landing"):
				print "Landing"
				self.lock.acquire()
				self.extraPrx.land()
				self.lock.release()
				hasLanded = True
			elif(self.sub1 == "Stabilizing2"):
				finished()

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata3(self):
		run = True
		cycle = 100
		t_activated = False
		t_fin = 0

		t_A_max = 0.023
		t_B_max = 0.055


		while(run):
			totala = time.time() * 1000000

			if(self.sub1 == "TakeOff"):
				if ((self.sub3 == "A_ghost") or (self.sub3 == "B_ghost")):
					ghostStateIndex = self.StatesSub3.index(self.sub3)
					self.sub3 = self.StatesSub3[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub3 == "A"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_A_max):
							self.sub3 = "B"
							t_activated = False
							self.automataGui.notifySetNodeAsActive('B')
							t_A_max = 0.023

				elif(self.sub3 == "B"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_B_max):
							self.sub3 = "A"
							t_activated = False
							self.automataGui.notifySetNodeAsActive('A')
							t_B_max = 0.055


				# Actuation if
				if(self.sub3 == "A"):
					print "A"
				elif(self.sub3 == "B"):
					print "B"
			else:
				if(self.sub3 == "A"):
					t_A_max = 0.023 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					sub3 = self.StatesSub3[ghostStateIndex]
				elif(self.sub3 == "B"):
					t_B_max = 0.055 - (t_fin - t_ini)
					ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
					sub3 = self.StatesSub3[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def subautomata5(self):
		run = True
		cycle = 100
		t_activated = False

		t_C_max = 0.567


		while(run):
			totala = time.time() * 1000000

			if(self.sub3 == "B"):
				if ((self.sub5 == "C_ghost")):
					ghostStateIndex = self.StatesSub5.index(self.sub5)
					self.sub5 = self.StatesSub5[ghostStateIndex - 1]
					t_ini = time.time()

				# Evaluation if
				if(self.sub5 == "C"):
					if(not t_activated):
						t_ini = time.time()
						t_activated = True
					else:
						t_fin = time.time()
						secs = t_fin - t_ini
						if(secs > t_C_max):
							self.sub5 = "C"
							t_activated = False
							self.automataGui.notifySetNodeAsActive('C')
							t_C_max = 0.567


				# Actuation if
				if(self.sub5 == "C"):
					print "C"
			else:
				if(self.sub5 == "C"):
					t_C_max = 0.567 - (t_fin - t_ini)
					ghostStateIndex = self.StateSub5.index(self.sub5) + 1
					sub5 = self.StatesSub5[ghostStateIndex]

			totalb = time.time() * 1000000
			msecs = (totalb - totala) / 1000;
			if(msecs < 0 or msecs > cycle):
				msecs = cycle
			else:
				msecs = cycle - msecs

			time.sleep(msecs / 1000)
			if(msecs < 33 ):
				time.sleep(33 / 1000);


	def connectToProxys(self):
		self.ic = Ice.initialize(sys.argv)

		# Contact to camera
		camera = self.ic.propertyToProxy('automata.Camera.Proxy')
		if(not camera):
			raise Exception('could not create proxy with camera')
		self.cameraPrx = CameraPrx.checkedCast(camera)
		if(not self.cameraPrx):
			raise Exception('invalid proxy automata.Camera.Proxy')
		print 'camera connected'

		# Contact to pose3d
		pose3d = self.ic.propertyToProxy('automata.Pose3D.Proxy')
		if(not pose3d):
			raise Exception('could not create proxy with pose3d')
		self.pose3dPrx = Pose3DPrx.checkedCast(pose3d)
		if(not self.pose3dPrx):
			raise Exception('invalid proxy automata.Pose3D.Proxy')
		print 'pose3d connected'

		# Contact to cmd
		cmd = self.ic.propertyToProxy('automata.CMDVel.Proxy')
		if(not cmd):
			raise Exception('could not create proxy with cmd')
		self.cmdPrx = CMDVelPrx.checkedCast(cmd)
		if(not self.cmdPrx):
			raise Exception('invalid proxy automata.CMDVel.Proxy')
		print 'cmd connected'

		# Contact to extra
		extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
		if(not extra):
			raise Exception('could not create proxy with extra')
		self.extraPrx = ArDroneExtraPrx.checkedCast(extra)
		if(not self.extraPrx):
			raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
		print 'extra connected'

		# Contact to navdata
		navdata = self.ic.propertyToProxy('automata.Navdata.Proxy')
		if(not navdata):
			raise Exception('could not create proxy with navdata')
		self.navdataPrx = NavdataPrx.checkedCast(navdata)
		if(not self.navdataPrx):
			raise Exception('invalid proxy automata.Navdata.Proxy')
		print 'navdata connected'


	def destroyIc(self):
		if(self.ic):
			self.ic.destroy()


	def start(self):
		self.guiThread = threading.Thread(target=self.runGui)
		self.guiThread.start()
		time.sleep(1)


		self.t1 = threading.Thread(target=self.subautomata1)
		self.t1.start()
		self.t3 = threading.Thread(target=self.subautomata3)
		self.t3.start()
		self.t5 = threading.Thread(target=self.subautomata5)
		self.t5.start()


	def join(self):
		self.guiThread.join()
		self.t1.join()
		self.t3.join()
		self.t5.join()
示例#23
0
class Automata():
    def __init__(self):
        self.lock = threading.Lock()
        self.displayGui = False
        self.StatesSub1 = [
            "TakeOff",
            "FollowTurtle",
            "Land",
        ]

        self.StatesSub2 = [
            "FindTurtle",
            "FindTurtle_ghost",
            "FollowTurtle",
            "FollowTurtle_ghost",
        ]

        self.StatesSub3 = [
            "LoseTurtle",
            "LoseTurtle_ghost",
            "Landing",
            "Landing_ghost",
        ]

        self.sub1 = "TakeOff"
        self.run1 = True
        self.sub2 = "FindTurtle_ghost"
        self.run2 = True
        self.sub3 = "LoseTurtle_ghost"
        self.run3 = True

    def setVelocity(self, vx, vy, vz, yaw):
        cmd = jderobot.CMDVelData()
        cmd.linearX = vy
        cmd.linearY = vx
        cmd.linearZ = vz
        cmd.angularZ = yaw
        cmd.angularX = cmd.angularY = 1.0
        self.CMDVelPrx.setCMDVelData(cmd)

    def getImage(self):
        img = self.CameraPrx.getImageData("RGB8")
        height = img.description.height
        width = img.description.width
        imgPixels = numpy.zeros((height, width, 3), numpy.uint8)
        imgPixels = numpy.frombuffer(img.pixelData, dtype=numpy.uint8)
        imgPixels.shape = height, width, 3
        return imgPixels

    def getContours(self):
        img = self.getImage()
        img = cv2.GaussianBlur(img, (5, 5), 0)
        hsvImg = cv2.cvtColor(img, cv2.COLOR_BGR2HSV)

        maxValues = numpy.array(self.hmax, self.vmax, self.smax)
        minValues = numpy.array(self.hmin, self.vmin, self.smin)

        thresoldImg = cv2.inRange(hsvImg, minValues, maxValues)
        conts, hierarchy = cv2.findContours(thresoldImg, cv2.RETR_EXTERNAL,
                                            cv2.CHAIN_APPROX_SIMPLE)
        return conts, hierarchy

    class PID:
        def __init__(self, Epsilon=.01, Kp=1, Ki=1, Kd=1, Imax=300, Imin=-300):
            self.Ep = Epsilon
            self.Kp = Kp
            self.Ki = Ki
            self.Kd = Kd
            self.Imax = Imax
            self.Imin = Imin
            self.lastError = 0
            self.cumulativeError = 0

        def updateCumulativeError(self, error):
            self.cumulativeError += error
            if self.cumulativeError > self.Imax:
                self.cumulativeError = self.Imax
            if self.cumulativeError < self.Imin:
                self.cumulativeError = self.Imin

        def process(self, error):
            if -self.Ep < error < self.Ep:
                return 0
            P = self.Kp * error

            self.updateCumulativeError(error)
            I = self.Ki * self.cumulativeError

            D = self.Kd * (error - self.lastError)
            self.lastError = error

            vel = P + I + D
            return vel

    def startThreads(self):
        self.t1 = threading.Thread(target=self.subautomata1)
        self.t1.start()
        self.t2 = threading.Thread(target=self.subautomata2)
        self.t2.start()
        self.t3 = threading.Thread(target=self.subautomata3)
        self.t3.start()

    def createAutomata(self):
        guiSubautomataList = []

        # Creating subAutomata1
        guiSubautomata1 = GuiSubautomata(1, 0, self.automataGui)

        guiSubautomata1.newGuiNode(1, 0, 111, 195, 1, 'TakeOff')
        guiSubautomata1.newGuiNode(2, 2, 323, 206, 0, 'FollowTurtle')
        guiSubautomata1.newGuiNode(3, 3, 531, 229, 0, 'Land')

        guiSubautomata1.newGuiTransition((111, 195), (323, 206), (191, 244), 1,
                                         1, 2)
        guiSubautomata1.newGuiTransition((323, 206), (531, 229), (419, 267), 2,
                                         2, 3)
        guiSubautomataList.append(guiSubautomata1)

        # Creating subAutomata2
        guiSubautomata2 = GuiSubautomata(2, 2, self.automataGui)

        guiSubautomata2.newGuiNode(4, 0, 118, 156, 1, 'FindTurtle')
        guiSubautomata2.newGuiNode(5, 0, 410, 171, 0, 'FollowTurtle')

        guiSubautomata2.newGuiTransition((118, 156), (410, 171), (276, 101), 3,
                                         4, 5)
        guiSubautomata2.newGuiTransition((410, 171), (118, 156), (258, 221), 4,
                                         5, 4)
        guiSubautomataList.append(guiSubautomata2)

        # Creating subAutomata3
        guiSubautomata3 = GuiSubautomata(3, 3, self.automataGui)

        guiSubautomata3.newGuiNode(7, 0, 117, 188, 1, 'LoseTurtle')
        guiSubautomata3.newGuiNode(8, 0, 378, 213, 0, 'Landing')

        guiSubautomata3.newGuiTransition((378, 213), (117, 188), (271, 109), 6,
                                         8, 7)
        guiSubautomata3.newGuiTransition((117, 188), (378, 213), (250, 258), 7,
                                         7, 8)
        guiSubautomataList.append(guiSubautomata3)

        return guiSubautomataList

    def shutDown(self):
        self.run1 = False
        self.run2 = False
        self.run3 = False

    def runGui(self):
        app = QtGui.QApplication(sys.argv)
        self.automataGui = AutomataGui()
        self.automataGui.setAutomata(self.createAutomata())
        self.automataGui.loadAutomata()
        self.startThreads()
        self.automataGui.show()
        app.exec_()

    def subautomata1(self):
        self.run1 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        self.heighIters = 0
        self.maxIters = 50
        self.contours = []

        #Filter Values
        self.hmin = 50
        self.hmax = 70
        self.vmin = 20
        self.vmax = 235
        self.smin = 10
        self.smax = 245

        while (self.run1):
            totala = time.time() * 1000000

            # Evaluation if
            if (self.sub1 == "TakeOff"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 2.5):
                        self.sub1 = "FollowTurtle"
                        t_activated = False
                        print "Iters:", self.heighIters
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowTurtle')

            elif (self.sub1 == "FollowTurtle"):
                if (not t_activated):
                    t_ini = time.time()
                    t_activated = True
                else:
                    t_fin = time.time()
                    secs = t_fin - t_ini
                    if (secs > 90):
                        self.sub1 = "Land"
                        t_activated = False
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Land')

            # Actuation if
            if (self.sub1 == "TakeOff"):
                self.ExtraPrx.takeoff()

                #Get some heigh
                self.setVelocity(0, 0, 1, 0)

                self.heighIters += 1
            elif (self.sub1 == "FollowTurtle"):
                self.contours, hierarchy = self.getContours()
            elif (self.sub1 == "Land"):
                self.contours, hierarchy = self.getContours()

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata2(self):
        self.run2 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        targetX = 125
        targetY = 125

        minError = 8

        xPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)
        yPid = self.PID(minError, 0.01, 0.02, 0.03, 5, -5)

        while (self.run2):
            totala = time.time() * 1000000

            if (self.sub1 == "FollowTurtle"):
                if ((self.sub2 == "FindTurtle_ghost")
                        or (self.sub2 == "FollowTurtle_ghost")):
                    ghostStateIndex = self.StatesSub2.index(self.sub2)
                    self.sub2 = self.StatesSub2[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub2 == "FindTurtle"):
                    if (self.contours):
                        self.sub2 = "FollowTurtle"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FollowTurtle')

                elif (self.sub2 == "FollowTurtle"):
                    if (not self.contours):
                        self.sub2 = "FindTurtle"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'FindTurtle')

                # Actuation if
                if (self.sub2 == "FindTurtle"):
                    if self.heighIters <= self.maxIters:
                        self.setVelocity(0, 0, 1, 0)
                        self.heighIters += 1
                        print "iters:", self.heighIters
                    else:
                        print "max hight reached"
                elif (self.sub2 == "FollowTurtle"):
                    #We assume there is only one green target on the screen
                    cnt = self.contours[0]

                    #Locate the minimal circle enclosing the contour
                    (x, y), radius = cv2.minEnclosingCircle(cnt)
                    center = (int(x), int(y))
                    radius = int(radius)

                    xError = targetX - center[0]
                    yError = targetY - center[1]

                    xVel = xPid.process(xError)
                    yVel = yPidprocess(yError)

                    #Control Heigh#

                    self.setVelocity(xVel, yVel, 0, 0)
                    print "xError:", xError, "yError:", yError
                    print "xVel:", xVel, "yVel:", yVel
            else:
                if (self.sub2 == "FindTurtle"):
                    ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
                    self.sub2 = self.StatesSub2[ghostStateIndex]
                elif (self.sub2 == "FollowTurtle"):
                    ghostStateIndex = self.StatesSub2.index(self.sub2) + 1
                    self.sub2 = self.StatesSub2[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def subautomata3(self):
        self.run3 = True
        cycle = 100
        t_activated = False
        t_fin = 0

        while (self.run3):
            totala = time.time() * 1000000

            if (self.sub1 == "Land"):
                if ((self.sub3 == "LoseTurtle_ghost")
                        or (self.sub3 == "Landing_ghost")):
                    ghostStateIndex = self.StatesSub3.index(self.sub3)
                    self.sub3 = self.StatesSub3[ghostStateIndex - 1]
                    t_ini = time.time()

                # Evaluation if
                if (self.sub3 == "LoseTurtle"):
                    if (not self.contours):
                        self.sub3 = "Landing"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive('Landing')

                elif (self.sub3 == "Landing"):
                    if (self.contours):
                        self.sub3 = "LoseTurtle"
                        if self.displayGui:
                            self.automataGui.notifySetNodeAsActive(
                                'LoseTurtle')

                # Actuation if
                if (self.sub3 == "LoseTurtle"):
                    self.setVelocity(1, 0, 0, 0)
                elif (self.sub3 == "Landing"):
                    self.ExtraPrx.land()
            else:
                if (self.sub3 == "LoseTurtle"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]
                elif (self.sub3 == "Landing"):
                    ghostStateIndex = self.StatesSub3.index(self.sub3) + 1
                    self.sub3 = self.StatesSub3[ghostStateIndex]

            totalb = time.time() * 1000000
            msecs = (totalb - totala) / 1000
            if (msecs < 0 or msecs > cycle):
                msecs = cycle
            else:
                msecs = cycle - msecs

            time.sleep(msecs / 1000)
            if (msecs < 33):
                time.sleep(33 / 1000)

    def connectToProxys(self):
        self.ic = Ice.initialize(sys.argv)

        # Contact to Extra
        Extra = self.ic.propertyToProxy('automata.ArDroneExtra.Proxy')
        if (not Extra):
            raise Exception('could not create proxy with Extra')
        self.ExtraPrx = ArDroneExtraPrx.checkedCast(Extra)
        if (not self.ExtraPrx):
            raise Exception('invalid proxy automata.ArDroneExtra.Proxy')
        print 'Extra connected'

        # Contact to CMDVel
        CMDVel = self.ic.propertyToProxy('automata.CMDVel.Proxy')
        if (not CMDVel):
            raise Exception('could not create proxy with CMDVel')
        self.CMDVelPrx = CMDVelPrx.checkedCast(CMDVel)
        if (not self.CMDVelPrx):
            raise Exception('invalid proxy automata.CMDVel.Proxy')
        print 'CMDVel connected'

        # Contact to Camera
        Camera = self.ic.propertyToProxy('automata.Camera.Proxy')
        if (not Camera):
            raise Exception('could not create proxy with Camera')
        self.CameraPrx = CameraPrx.checkedCast(Camera)
        if (not self.CameraPrx):
            raise Exception('invalid proxy automata.Camera.Proxy')
        print 'Camera connected'

    def destroyIc(self):
        if (self.ic):
            self.ic.destroy()

    def start(self):
        if self.displayGui:
            self.guiThread = threading.Thread(target=self.runGui)
            self.guiThread.start()
        else:
            self.startThreads()

    def join(self):
        if self.displayGui:
            self.guiThread.join()
        self.t1.join()
        self.t2.join()
        self.t3.join()

    def readArgs(self):
        for arg in sys.argv:
            splitedArg = arg.split('=')
            if splitedArg[0] == '--displaygui':
                if splitedArg[1] == 'True' or splitedArg[1] == 'true':
                    self.displayGui = True
                    print 'runtime gui enabled'
                else:
                    self.displayGui = False
                    print 'runtime gui disabled'