diff --git a/main_module/gyro/ros_middleman.py b/main_module/gyro/ros_middleman.py index 13f0741..ff67588 100644 --- a/main_module/gyro/ros_middleman.py +++ b/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 diff --git a/main_module/planning/s2018/test.py b/main_module/planning/s2018/test.py index 01edd83..08687b6 100644 --- a/main_module/planning/s2018/test.py +++ b/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): diff --git a/main_module/propulsion/ros_middleman.py b/main_module/propulsion/ros_middleman.py index 986ab06..8e18175 100644 --- a/main_module/propulsion/ros_middleman.py +++ b/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