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