pushing changes from the wet test

Author: pedrolei

main_module/gyro/ros_middleman.py

@@ -9,21 +9,25 @@ def __init__(self):
         del sys
 
         self._roll, self._pitch, self._yaw = [0.0, 0.0, 0.0]
+	self.got_first_angle = False
 
     @property
     def roll(self):
-        return self._roll
+        return self._roll - self.angle.tare_angles[0]
 
     @property
     def pitch(self):
-        return self._pitch
+        return self._pitch - self.angle.tare_angles[1]
 
     @property
     def yaw(self):
-        return self._yaw
+        return self._yaw - self.angle.tare_angles[2]
 
     def callback(self, new_message):
-        """value should be in degrees"""
-        self._roll = new_message.vector.x# degrees
-        self._pitch = new_message.vector.y# degrees
-        self._yaw = new_message.vector.z# degrees
+	if not self.got_first_angle:
+		self.got_first_angle = True
+		self.set_tare([new_message.vector.x, new_message.vector.y, new_message.vector.z], False)
+	else:
+        	self._roll = new_message.vector.y# degrees
+        	self._pitch = new_message.vector.x# degrees
+        	self._yaw = new_message.vector.z# degrees

main_module/planning/s2018/test.py

@@ -24,9 +24,10 @@ def __init__(self, gyro, odometer):
 
     def _get_leash(self):
         p0 = [0,0,0]
+	p3 = [0,0.5,0]
         p1 = [0.75,0.75,-0.5]
         p2 = [-0.75,0.75,-0.5]
-        path0 = PointList([p0, p1, p2, p0], 0.0)
+        path0 = PointList([p0, p3, p1, p2, p0], 0.0)
         return Leash([path0], 0.05, 0.005)
 
     def _get_events(self):

main_module/propulsion/ros_middleman.py

@@ -2,7 +2,7 @@
 
 from ._abstract import Propulsion
 class Middleman(Propulsion):
-    TRANSLATIONAL_P = 0.05
+    TRANSLATIONAL_P = 1.0
     TRANSLATIONAL_I = 0.0
     TRANSLATIONAL_D = 0.0