update blue

coordinator/nodes/match.py

@@ -298,6 +298,8 @@ class CoordinatorNode():
 
         start = rospy.get_rostime().secs
 
+        hit_ok = True
+
         if self.team:  # yellow
             self.move(0.6, forward=True)
             self.turn(radians(-90))
@@ -356,34 +358,38 @@ class CoordinatorNode():
             rospy.loginfo("Premier mur touché")
             self.move(0.05, forward=False)
             self.turn(radians(90))
-            self.hit_wall()
-            rospy.loginfo("Second mur touché")
+            hit_ok = self.hit_wall()
+            if hit_ok:
+                rospy.loginfo("Second mur touché")
 
-            self.move(0.06, forward=False)
-            self.turn(radians(-135))
+                self.move(0.06, forward=False)
+                self.turn(radians(-135))
 
-            rospy.loginfo("Activation du phare")
-            self.set_speed(0, 0)
-            time.sleep(send_interval)
-            self.lower_arm()
-            self.brake_for_n_seconds(2)
-            self.raise_arm()
-            self.brake_for_n_seconds(1)
-            rospy.loginfo("Phare activé")
+                rospy.loginfo("Activation du phare")
+                self.set_speed(0, 0)
+                time.sleep(send_interval)
+                self.lower_arm()
+                self.brake_for_n_seconds(2)
+                self.raise_arm()
+                self.brake_for_n_seconds(1)
+                rospy.loginfo("Phare activé")
 
-            self.turn(radians(135))
+                self.turn(radians(135))
 
-            self.disable_lidar = True
+                self.disable_lidar = True
 
-            self.move(0.5, forward=False)
-            self.turn(radians(90))
+                self.move(0.5, forward=False)
+                self.turn(radians(90))
 
-            self.disable_lidar = False
+                self.disable_lidar = False
 
-            self.move(0.82, forward=True)
-            self.turn(radians(90))
-            self.move(0.3, forward=False)
-            self.move(0.4, forward=True)
+                self.move(0.82, forward=True)
+                self.turn(radians(90))
+                self.move(0.3, forward=False)
+            else:
+                self.set_speed(0, 0)
+                self.reset_error()
+                self.change_pwm(False)
 
             # ??
 
@@ -411,7 +417,7 @@ class CoordinatorNode():
             time.sleep(send_interval)
 
         score_msg = Int32()
-        score_msg.data = 51
+        score_msg.data = 51 if hit_ok else 24
         self.estimated_score_pub.publish(score_msg)
 
         self.change_pwm(False)