From 2935893f0241288756485785b0aed50e534683f2 Mon Sep 17 00:00:00 2001
From: leo <leo.lemaire007@orange.fr>
Date: Thu, 10 Feb 2022 18:36:19 +0100
Subject: [PATCH] =?UTF-8?q?Ajout=C3=A9=20l'algo=20de=20conduite=20selon=20?=
 =?UTF-8?q?les=20instructions=20du=20chef?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

---
 Car.py           | 38 +++++++++++++++++++++++++++++---------
 CarController.py | 10 +++++++++-
 2 files changed, 38 insertions(+), 10 deletions(-)

diff --git a/Car.py b/Car.py
index e894047..190b5b1 100644
--- a/Car.py
+++ b/Car.py
@@ -10,15 +10,18 @@ class Car:
         inverted=not (startEdge.getToNode()==nextEdge.getFromNode() or startEdge.getToNode()==nextEdge.getToNode())
         
         laneId=0 if not inverted else 1
-        self.laneShape=startEdge.getLane(laneId).getShape()
+        lane=startEdge.getLane(laneId)
+        self.vmax=lane.getSpeed()
+        self.laneShape=lane.getShape()
         if(inverted):
             self.laneShape.reverse()
 
         print(f"{self.id} : {startEdge.getID()} -> {nextEdge.getID()} via {laneId}")
     
-    def __init__(self,carID,route,parentMap,surface):
+    def __init__(self,carID,route,parentMap,parentController,surface):
         self.id=carID
         self.map=parentMap
+        self.controller=parentController
         self.index=0
         self.laneInd=0
         self.route=[]
@@ -49,20 +52,37 @@ class Car:
     def draw(self,col):
         pg.draw.circle(self.surf,col,self.map.convertPos(self.pos),5)
    
-    def conduite(self):
-        vmax=100 # vitesse max sur la route actuelle
-        vleader=vmax # vitesse de la voiture leader
-        bleader=15
+    def conduite(self,vmax,leader,dt):
+        if(leader==None):
+            self.v=vmax
+            return
+        else:
+            vleader=leader.v # vitesse de la voiture leader
+            bleader=leader.b
+
         vbar=(self.v+vleader)/2
-        bbar=(bleader+self.b)/2 # decelleration de la vleader et de self
+        bbar=(bleader+self.b)/2
         S=5
         T=0.3
         vsec=vleader+(S-vmax*T)/(vbar/bbar+T)
-        vd=min(vmax,vsec)
+        vd=min(self.v*self.a*dt,vmax,vsec)
         self.v=max(0,vd)
 
     def update(self,dt):
-        self.conduite()
+        leaders=self.controller.getCarsOnEdge(self.route[self.index].getID())
+        leader=None
+        minDist=self.route[self.index].getLength()
+        for l in leaders:
+            db=math.dist(self.pos,l.pos)
+            de=math.dist(l.pos,self.laneShape[-1])
+            df=math.dist(self.pos,self.laneShape[-1])
+
+            if(de<df and db<minDist):
+                minDist=db
+                leader=l
+
+        self.conduite(self.vmax,leader,dt)
+        
         lgt=self.v*dt
 
         while(lgt>0):
diff --git a/CarController.py b/CarController.py
index 9363270..4846cf3 100644
--- a/CarController.py
+++ b/CarController.py
@@ -11,7 +11,15 @@ class CarController:
     def fromPath(self,path):
         for vehicle in sumolib.xml.parse(path,"vehicle"):
             route=vehicle.route[0].edges.split()
-            self.cars.append(Car(vehicle.id,route,self.map,self.surf))
+            self.cars.append(Car(vehicle.id,route,self.map,self,self.surf))
+    
+    def getCarsOnEdge(self,edgeID): # on devrai probablement utiliser une map ici (dict en python?) mais flemme
+        res=[]
+        for car in self.cars:
+            cid=car.route[car.index].getID()
+            if(cid==edgeID):
+                res.append(car)
+        return res
 
     def update(self):
         for car in self.cars: