derped/robolab
1
0
Fork 0
Code Pull requests Releases Activity

reapplied commit: c323a6e

Browse source
This commit is contained in:
jureao 2018-03-23 11:36:44 +01:00 committed by d3rped
parent 800cf6302d
commit b49919a7ed
1 changed files with 9 additions and 8 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

17
src/odometry.py
View file

@ -39,19 +39,20 @@ class Odometry:
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 * pi) <pi*0.25 or self.alpa % (2 * pi ) >= 7/4*pi and self.alpha %(2 * pi) <2*pi:
return(Direction.NORTH)
if self.alpha % (2 * self._pi) >=0 and self.alpha % (2 * self._pi) <self._pi*0.25 or self.alpa % (2 * self._pi ) >= 7/4*self._pi and self.alpha %(2 * pi) <2*pi:
return(Direction.NORTH,"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.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.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)
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)
return(Direction.WEST,"Distance = ",self.distance)
def coordinates(self,delta_rota_l,delta_rota_r,Y_koord,X_koord): # worked as function outside the class
self.y= self.Y_koord