From 245cfa91fba7375244fad8d7924fb8084e658a43 Mon Sep 17 00:00:00 2001 From: jureao Date: Thu, 22 Mar 2018 22:05:43 +0100 Subject: [PATCH] reapplied commit: 3c4cb19 --- src/odometry.py | 26 ++++++++++++++++---------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/src/odometry.py b/src/odometry.py index 862f1e1..f61e504 100644 --- a/src/odometry.py +++ b/src/odometry.py @@ -35,24 +35,23 @@ class Odometry: self.distance = 0 - #worked fine outside the class as function in nano, maybe we must fix it a bit :) + #worked fine outside the class as function in nano, maybe we must fix it a bit :) def degree(self, delta_rotation_l, delta_rotation_r,star_Direc): #delta_rotation_l/r should work with getmovement method self.alpha = 0 #should be start_Direc self.alpha=star_Direc - + while True: self.alpha = ((self.delta_rotation_l * self._distance_per_tick - self.delta_rotation_r * self._distance_per_tick)/self._wheel_axis) + self.alpha self.distance=(self.delta_rotation_r * self._distance_per_tick + self.delta_rotation_l * self._distance_per_tick)/2 + self.distance - if self.alpha % (2 * self._pi) >=0 and self.alpha % (2 * self._pi) = 7/4*self._pi and self.alpha %(2 * pi) <2*pi: - return(Direction.NORTH,"Distance = ",self.distance) + if self.alpha % (2 * self._pi) >=0 and self.alpha % (2 * pi) = 7/4*pi and self.alpha %(2 * pi) <2*pi: + return(Direction.NORTH) - elif self.alpha % (2 * self._pi) >= self._pi*0.25 and self.alpha % ( 2 * self._pi ) < self._pi*0.5 or self.alpha % (2 * self._pi ) >= self._pi * 0.5 and self.alpha % (2 * self._pi ) < self._pi *0.75: - return(Direction.EAST,"Distance = ",self.distance) + elif self.alpha % (2 * self._pi) >= self._pi*0.25 and self.alpha % ( 2 * pi ) < self._pi*0.5 or self.alpha % (2 * self._pi ) >= self._pi * 0.5 and self.alpha % (2 * self._pi ) < self._pi *0.75: + return(Direction.EAST) - elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self._pi) >=self._pi and self.alpha < self.alpha * 5/4 pi : - return(Direction.SOUTH,"Distance = ",self.distance) + elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self_pi) >=self._pi and self.alpha < self.alpha * 5/4 pi : + return(Direction.SOUTH) else: - return(Direction.WEST,"Distance = ",self.distance) - + return(Direction.WEST) def coordinates(self,delta_rota_l,delta_rota_r,Y_koord,X_koord): # worked as function outside the class self.y= self.Y_koord @@ -62,3 +61,10 @@ class Odometry: self.y = self.y + wheel_center * math.cos(alpha) self.x = self.x + wheel_center * math.sin(alpha) return(self.x,self.y) + + + + elif self.alpha % (2 * self._pi)>=self._pi * 0.75 and self.alpha % (2* self._pi)< self._pi or self.alpha % (2 * self._pi) >=self._pi and self.alpha < self.alpha * 5/4 pi : + return(Direction.SOUTH,"Distance = ",self.distance) + else: + return(Direction.WEST,"Distance = ",self.distance)