class ThreadSimu(threading.Thread): """dérivation d'un objet thread pour gérer la connexion avec un client""" def __init__(self): global M global Lock global LockEnd # init the server threading.Thread.__init__(self) self.nbsRobots = 4 self.listRobots = [] # self.Command = command() self.listRobotsReels = [] # self.Command.ListRobots # Initialisation Simulation self.simu = SimulationControl(self.nbsRobots, 550, 20) self.alive = True # objet initialisant les threads des GPS self.MainThread = ThreadGPSAll(self.nbsRobots, Lock, LockEnd) def run(self): global M global Lock global LockEnd theta_i = 0 # initial angle of the ellipse l_i = 0.1 # initial major axis of the ellipse centre_i = np.array([[0], [0]]) # initial center of the ellipse k = 0 # start with position number 0 tk = 0 l_f, theta_f, centre_f = self.simu.param_ellipse(self.simu.objectifs, k) # ellipse parameter to reach x0 = np.array([[0], [0], [0], [1]]) X = np.dot(x0, np.ones((1, self.simu.N))) # position of the robots ListPWM = [] self.MainThread.run() # Init affichage vibes.beginDrawing() vibes.newFigure("Simulation Gascogne avec Terrain de Foot") vibes.setFigureProperties({"x": 200, "y": 100, "width": 800, "height": 800}) points = [ [0, 0], [40.863285863772035, 4.2179290521889925], [37.32119551504729, 39.42228888720274], [-3.5161916106007993, 36.53818473871797], ] objectifs = [ [0, 0], [40.863285863772035, 4.2179290521889925], [37.32119551504729, 39.42228888720274], [-3.5161916106007993, 36.53818473871797], ] vibes.drawLine(points) # drawing of the coast vibes.drawLine(objectifs, "green") # drawing of ellipse target bounds for i in range(0, self.nbsRobots): self.listRobots.append(Robot(-4.0, 45.0, 0.0, 0.0, i, "b", self.nbsRobots)) for t in np.arange(0, self.simu.Tf, self.simu.dt): ## Récupère la postion des robots réels with Lock: M = self.MainThread.M print(self.MainThread.M) Arr = M print(M) # time.sleep(self.simu.dt) ## Affichage des robot ref = [391039.7802301956, 5363846.097328593] vibes.clearFigure() vibes.drawLine(points, "black") vibes.drawLine(objectifs, "green") for i in range(self.nbsRobots): vibes.drawCircle(Arr[i, 3] - ref[0], Arr[i, 4] - ref[1], 0.5, "blue") ## mettre a jour coordonnées du robot for i in range(self.nbsRobots): # cas où x,y = lat,lon # self.listRobots[i].setNewState(np.array(Arr[i,0],Arr[i,1]),Arr[i,2]) # cas où x,y = east,north self.listRobots[i].setNewState(np.array([Arr[i, 3], Arr[i, 4]]), Arr[i, 2]) theta = self.listRobots[i].thetaMesure X[0, i] = Arr[i, 3] X[1, i] = Arr[i, 4] X[2, i] = Arr[i, 2] * np.cos(theta) + 1 X[3, i] = Arr[i, 2] * np.sin(theta) + 1 # Probleme probable ici car orientation de theta a revoir # calcule la consigne t, k, tk, theta_i, theta_f, l_i, l_f, centre_i, centre_f, positions, X, cons, dcons = self.simu.simuOneStep( t, k, tk, theta_i, theta_f, l_i, l_f, centre_i, centre_f, X ) # X correspond à la position théorique des robots, cons[-self.nbsRobots] correspond à la dernière consigne calculée pour les robots for i in range(self.nbsRobots): ## Reception consigne PWM PWMrobot = self.listRobots[i].regulate( np.array([cons[i][0, 0], cons[i][1, 0]]), np.array([dcons[i][0, 0], dcons[i][1, 0]]) ) ListPWM.append(PWMrobot) print(ListPWM) for i in range(self.nbsRobots): ## completer avec le code de commande robot # commande à utiliser pour normaliser la commande # appliquqe la commande trouvée # setPWM(PWM) :: append à la liste Pwnforw et Pwmturn les valeurs de PWM[0] et PWM[1] 1 + 1 ListPWM.clear() with LockEnd: cont = self.alive if not cont: break vibes.endDrawing() with Lock: M = [] def stopT(self): with LockEnd: self.alive = False
class ThreadSimu(threading.Thread): '''dérivation d'un objet thread pour gérer la connexion avec un client''' def __init__(self): global M # init the server threading.Thread.__init__(self) self.nbsRobots = 4 self.listRobots = [] self.Command,self.joystick,self.axes = initVarCommunication() self.listRobotsReels = [] #self.Command.ListRobots # Initialisation Simulation self.simu = SimulationControl(self.nbsRobots,550,10) self.alive = True # Init GPS self.ser=initGPS(); def run(self): global M theta_i = 0 # initial angle of the ellipse l_i = 0.1 # initial major axis of the ellipse centre_i = np.array([[0],[0]]) # initial center of the ellipse k = 0 # start with position number 0 tk = 0 l_f,theta_f,centre_f = self.simu.param_ellipse(self.simu.objectifs,k) # ellipse parameter to reach x0 = np.array([[0],[0],[0],[1]]) X = np.dot(x0,np.ones((1,self.simu.N))) # position of the robots ListPWM = [] # Init affichage vibes.beginDrawing() vibes.newFigure('Simulation Gascogne avec Terrain de Foot') vibes.setFigureProperties({'x': 200, 'y': 100, 'width': 800, 'height': 800}) points = [[0,0],[40.863285863772035, 4.2179290521889925],[37.32119551504729, 39.42228888720274],[-3.5161916106007993, 36.53818473871797]] objectifs = [[20.0,30.0],[20.01,10.0]] #objectifs = [[0,0],[40.863285863772035, 4.2179290521889925],[37.32119551504729, 39.42228888720274],[-3.5161916106007993, 36.53818473871797]] vibes.drawLine(points) # drawing of the coast vibes.drawLine(objectifs,'green') # drawing of ellipse target bounds for i in range(0,self.nbsRobots): self.listRobots.append(Robot(-4.0,45.0,0.0,0.0,i,'b',self.nbsRobots)) # init the robot control # self.Command.start() # beginning of the regulation for t in np.arange(0,self.simu.Tf,self.simu.dt): ## Récupère la postion des robots réels with Lock: M = returnAllLatLong(self.ser,self.nbsRobots) Arr = M.copy() print(Arr) # time.sleep(self.simu.dt) ## Affichage des robot ref = [391039.7802301956, 5363846.097328593] #vibes.clearFigure() vibes.drawLine(points, 'black') vibes.drawLine(objectifs,'green') for i in range(self.nbsRobots): vibes.drawCircle(Arr[i,3]-ref[0],Arr[i,4]-ref[1],0.5,'blue') ## mettre a jour coordonnées du robot for i in range(self.nbsRobots): # cas où x,y = lat,lon #self.listRobots[i].setNewState(np.array(Arr[i,0],Arr[i,1]),Arr[i,2]) # cas où x,y = east,north self.listRobots[i].setNewState(np.array([Arr[i,3],Arr[i,4]]),Arr[i,2]) theta = self.listRobots[i].thetaMesure X[0,i]=Arr[i,3] X[1,i]=Arr[i,4] X[2,i]=Arr[i,2]*np.cos(theta)+1 X[3,i]=Arr[i,2]*np.sin(theta)+1 # Probleme probable ici car orientation de theta a revoir # calcule la consigne t,k,tk,theta_i,theta_f,l_i,l_f,centre_i,centre_f, positions, X ,cons,dcons = self.simu.simuOneStep(t,k,tk,theta_i,theta_f,l_i,l_f,centre_i,centre_f,X) # X correspond à la position théorique des robots, cons[-self.nbsRobots] correspond à la dernière consigne calculée pour les robots # print(" cons : ",cons) for i in range(self.nbsRobots): ## Reception consigne PWM # PWMrobot = self.listRobots[i].regulate(np.array([cons[i][0,0],cons[i][1,0]]),np.array([dcons[i][0,0],dcons[i][1,0]])) # ListPWM.append([np.int16(PWMrobot[0]),np.int16(PWMrobot[1])]) vibes.drawCircle(cons[i][0,0],cons[i][1,0],0.5,'red') # print(ListPWM) ## completer avec le code de commande robot # commande à utiliser pour normaliser la commande # appliquqe la commande trouvée # self.Command.setConsigne(ListPWM[0][0],ListPWM[0][1],ListPWM[1][0],ListPWM[1][1],ListPWM[2][0],ListPWM[2][1],ListPWM[3][0],ListPWM[3][1]) ListPWM.clear() with LockEnd: cont = self.alive if not cont: break vibes.endDrawing() with Lock: M = [] def stopT(self): with LockEnd: self.alive = False