def __init__(self, num1=params.odometry_num1, num2=params.odometry_num2, radius=params.odometry_radius): threading.Thread.__init__(self) self.m1=Mouse(num1) self.m2=Mouse(num2) self.radius=radius self.x=0. self.y=0. self.theta=0. self.interval=params.odometry_interval self.que=deque() self.running=False
class OdometryThread(threading.Thread): def __init__(self, num1=params.odometry_num1, num2=params.odometry_num2, radius=params.odometry_radius): threading.Thread.__init__(self) self.m1=Mouse(num1) self.m2=Mouse(num2) self.radius=radius self.x=0. self.y=0. self.theta=0. self.interval=params.odometry_interval self.que=deque() self.running=False def run(self): self.running=True self.m1.get_delta() self.m2.get_delta() while self.running: x1, y1=self.m1.get_delta() x2, y2=self.m2.get_delta() dx=(x1+x2)/2. dy=(y1+y2)/2. self.x+=dx*math.cos(self.theta)-dy*math.sin(self.theta) self.y+=dx*math.sin(self.theta)+dy*math.cos(self.theta) self.theta+=(y2-y1)/(2.*self.radius) self.que.append((time.time(), self.x, self.y, self.theta)) self.m1.stop() self.m2.stop() def get_pos(self): return (self.x, self.y, self.theta) def get_speed(self): t=time.time() while len(self.que)>0 and t-self.que[0][0]>self.interval: self.que.popleft() if len(self.que)>1: one=self.que[0] two=self.que[-1] dt=two[0]-one[0] return ((two[1]-one[1])/dt, (two[2]-one[2])/dt, (two[3]-one[3])/dt) return (0., 0., 0.) def stop(self): self.running=False