예제 #1
0
	def getAngle(self, motor_port):
		"""
		Function to get encoder angle (in radian)
		"""
		BrickPiUpdateValues()
		degree = ( BrickPi.Encoder[motor_port] % 720 ) * 0.5
		radian = toPIPI( degree * math.pi / 180 )
		return radian
예제 #2
0
	def getMovingDistance(self, motor_port):
		"""
		Function to change encoder steps into distance(in cm)
		"""
		current_radian = self.getAngle(motor_port)
		current_time = time.time()
		if(motor_port == LEFT_MOTOR):
			dt = current_time - self.prev_timeL
			dth = toPIPI(current_radian - self.prev_radianL)*ENCODER_RATIO
			distance = dth * W_RADIUS
			self.prev_radianL = current_radian
			self.prev_timeL = current_time
		else:	
			dt = current_time - self.prev_timeR
			dth = toPIPI(current_radian - self.prev_radianR)*ENCODER_RATIO
			distance = dth * W_RADIUS
			self.prev_radianR = current_radian
			self.prev_timeR = current_time
		return (FWD_SIGN*distance, dt)