From 151723bdf8f8f241b281ff3a0512fdc1192d9a89 Mon Sep 17 00:00:00 2001
From: LeProblemeEDMN <72100759+LeProblemeEDMN@users.noreply.github.com>
Date: Wed, 5 Jun 2024 19:09:58 +0000
Subject: [PATCH] remove spin urgency

---
 raspberrypi/daughter_cards/wheeledbase.py | 109 +++++++++++-----------
 1 file changed, 54 insertions(+), 55 deletions(-)

diff --git a/raspberrypi/daughter_cards/wheeledbase.py b/raspberrypi/daughter_cards/wheeledbase.py
index feae062..dd50e51 100644
--- a/raspberrypi/daughter_cards/wheeledbase.py
+++ b/raspberrypi/daughter_cards/wheeledbase.py
@@ -261,7 +261,7 @@ def goto(self, x, y, theta=None, direction=None, finalangle=None, lookahead=None
         # Go to the setpoint position
         self.purepursuit([(x0,y0), (x, y)], direction, finalangle, lookahead, lookaheadbis, linvelmax, angvelmax)
         #self.wait(**kwargs)
-        while not self.isarrived(raiseSpinUrgency=True):
+        while not self.isarrived(raiseSpinUrgency=False):
             pass
         # Get the setpoint orientation
         if theta is not None:
@@ -326,8 +326,59 @@ def goto_stop(self, x, y,sensors, theta=None, direction=None, finalangle=None, l
                             direction = 'backward'
                     print([(x0,y0), (x, y)],finalangle)
                     self.purepursuit([(x0,y0), (x, y)], direction, finalangle, lookahead, lookaheadbis, linvelmax, angvelmax)
-                #else:
-                    #print("avance:",sen)
+
+            
+            self.log.sendLog("ARRIVE")
+            
+            # Get the setpoint orientation
+            if theta is not None:
+                if(theta<0):
+                    theta+=2*math.pi
+                rPos=self.get_position()
+                radiusRobot=370
+                ang=rPos[2]%(2*math.pi)
+                
+                #fait en sorte que |ang-theta|<pi pr trouver le sens du robot
+                if(theta-ang>math.pi):
+                    theta-=2*math.pi
+                if(ang-theta>math.pi):
+                    ang-=2*math.pi
+                print(ang,theta)
+                trigo=theta>ang#va ds le sens trigo
+                print(trigo)
+                #le rbot va bouger ds l'intervalle [a;b] inclus ds [-pi;2pi]
+                a=min(ang,theta)
+                b=max(ang,theta)
+
+                #optimise le x et y (condition optimilité ss contrainte donne borne du problème)
+                xmin=math.cos(theta)*radiusRobot+rPos[0]
+                xmax=xmin
+                ymin=math.sin(theta)*radiusRobot+rPos[1]
+                ymax=ymin
+                #test x-
+                if(a<=-math.pi or (a<math.pi and b >math.pi)):
+                    xmin=-radiusRobot+rPos[0]
+                if(b>=math.pi*2 or (a<0 and b>0)):
+                    xmax=radiusRobot+rPos[0]
+                if((a<math.pi/2 and b >math.pi/2)):
+                    ymax=radiusRobot+rPos[1]
+                if((a<3*math.pi/2 and b >3*math.pi/2) or (a<-math.pi/2 and b >-math.pi/2)):
+                    ymin=radiusRobot+rPos[1]
+                
+                #print(trigo,ang,theta)
+                #print("     ",xmin,xmax,ymin,ymax)    
+                if(xmin<0 or xmax>2000 or ymin<0 or ymax>3000):
+                    trigo =not trigo
+                
+                #print(trigo,['clock','trig'][trigo])
+                while(np.min(sensors.get_all_sensors())<600):
+                    print("sensors detection")
+                    sleep(0.1)
+                print("fin trigo:",trigo,rPos)
+                self.turnonthespot(theta,direction=['clock','trig'][trigo])
+                print("fin commande turn on the spot")
+                self.wait(**kwargs)
+
         except RuntimeError:
             self.goto_stop(x, y,sensors, theta, direction, finalangle, lookahead, lookaheadbis, linvelmax, angvelmax)
             print("FUUUUCK runtime")
@@ -337,58 +388,6 @@ def goto_stop(self, x, y,sensors, theta=None, direction=None, finalangle=None, l
             time.sleep(0.5)
             self.wheeledbase.connect()
 
-            
-        self.log.sendLog("ARRIVE")
-        
-        # Get the setpoint orientation
-        if theta is not None:
-            if(theta<0):
-                theta+=2*math.pi
-            rPos=self.get_position()
-            radiusRobot=370
-            ang=rPos[2]%(2*math.pi)
-            
-            #fait en sorte que |ang-theta|<pi pr trouver le sens du robot
-            if(theta-ang>math.pi):
-                theta-=2*math.pi
-            if(ang-theta>math.pi):
-                ang-=2*math.pi
-            print(ang,theta)
-            trigo=theta>ang#va ds le sens trigo
-            print(trigo)
-            #le rbot va bouger ds l'intervalle [a;b] inclus ds [-pi;2pi]
-            a=min(ang,theta)
-            b=max(ang,theta)
-
-            #optimise le x et y (condition optimilité ss contrainte donne borne du problème)
-            xmin=math.cos(theta)*radiusRobot+rPos[0]
-            xmax=xmin
-            ymin=math.sin(theta)*radiusRobot+rPos[1]
-            ymax=ymin
-            #test x-
-            if(a<=-math.pi or (a<math.pi and b >math.pi)):
-                xmin=-radiusRobot+rPos[0]
-            if(b>=math.pi*2 or (a<0 and b>0)):
-                xmax=radiusRobot+rPos[0]
-            if((a<math.pi/2 and b >math.pi/2)):
-                ymax=radiusRobot+rPos[1]
-            if((a<3*math.pi/2 and b >3*math.pi/2) or (a<-math.pi/2 and b >-math.pi/2)):
-                ymin=radiusRobot+rPos[1]
-            
-            #print(trigo,ang,theta)
-            #print("     ",xmin,xmax,ymin,ymax)    
-            if(xmin<0 or xmax>2000 or ymin<0 or ymax>3000):
-                trigo =not trigo
-            
-            #print(trigo,['clock','trig'][trigo])
-            while(np.min(sensors.get_all_sensors())<600):
-                print("snesors detection")
-                sleep(0.1)
-            print("fin trigo:",trigo,rPos)
-            self.turnonthespot(theta,direction=['clock','trig'][trigo])
-            print("fin commande turn on the spot")
-            self.wait(**kwargs)
-            
     def stop(self):
         self.set_openloop_velocities(0, 0)