Update wheeledbase.py

clubrobot · Jun 1, 2024 · 1af9ed3 · 1af9ed3
1 parent 95e61c4
commit 1af9ed3
Showing 1 changed file with 3 additions and 1 deletion.
diff --git a/raspberrypi/daughter_cards/wheeledbase.py b/raspberrypi/daughter_cards/wheeledbase.py
@@ -347,13 +347,15 @@ def goto_stop(self, x, y,sensors, theta=None, direction=None, finalangle=None, l
             rPos=self.get_position()
             radiusRobot=370
             ang=rPos[2]%(2*math.pi)
-            print(ang,theta)
+
             #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)