From 6a9d7fab0eaac0459092f3deac6930af8812bbb4 Mon Sep 17 00:00:00 2001 From: Raspberry Pi Date: Thu, 14 Nov 2019 14:42:10 -0600 Subject: Update initialize to not use wait_for_event for detecting limit switches. Update linear encoder to reduce noisy behavior and properly keep track of position. NOTE: Linear encoder direction seems to be backwards relative to the system (the motor currently also moves in this direction). Not a big deal as long as the motor and linear encoder work together properly, but ideally positive would be to the right. --- System_Python/system.py | 43 ++++++++++++++++++++++++++----------------- 1 file changed, 26 insertions(+), 17 deletions(-) (limited to 'System_Python/system.py') diff --git a/System_Python/system.py b/System_Python/system.py index 469f475..078c1f9 100644 --- a/System_Python/system.py +++ b/System_Python/system.py @@ -18,7 +18,7 @@ encoder_data_pin = 3 ### Angular encoder pins encoder_angular_cs_pin = 4 ### Linear encoder pins -encoder_linear_cs_pin = 5 +encoder_linear_cs_pin = 14 ### Limit switch pins (configured to PULLUP) limit_negative_pin = 19 limit_positive_pin = 26 @@ -71,21 +71,33 @@ class System: # Begin moving slowly in the negative direction until the negative limit switch is triggered if not GPIO.input(limit_negative_pin) == False: self.motor.move(-1) - GPIO.wait_for_edge(limit_negative_pin, GPIO.FALLING) + pressed = True + while pressed != False: + pressed = GPIO.input(limit_negative_pin) + sleep(0.01) self.motor.brake() # Set zero at the negative end of the track for easy reference in determining the extent self.encoder_linear.set_zero() + sleep(1) # Begin moving slowly in the positive direction until the positive limit switch is triggered self.motor.move(1) - GPIO.wait_for_edge(limit_positive_pin, GPIO.FALLING) + pressed = True + while pressed != False: + # We must continue reading linear encoder motion to keep track of rotations + self.encoder_linear.read_position() + pressed = GPIO.input(limit_positive_pin) + sleep(0.01) + #GPIO.wait_for_edge(limit_positive_pin, GPIO.FALLING) self.motor.brake() # Get the current position (the extent of the track) extent = self.encoder_linear.read_position() # Move back towards the center until we reach position extent/2 position = extent + sleep(1) self.motor.move(-1) - while not position == extent / 2: + while position >= (extent / 2.): position = self.encoder_linear.read_position() + sleep(0.01) self.motor.brake() # Set zero again: this is the real zero self.encoder_linear.set_zero() @@ -205,23 +217,20 @@ class Linear_Encoder: # Set the zero position for the encoder self.encoder.set_zero() # Reset the internal position counter - self.rotations = 0 - self.last_position = 0 + self.rotations = 0. + self.last_position = 0. def read_position(self): - # Read the position of the encoder - position = self.encoder.read_position('Raw') + # Read the position of the encoder (apply a noise filter, we don't need that much precision here) + position = float(self.encoder.read_position('Raw') & 0b1111111100) # Compare to last known position # NOTE: For now, assume that we are moving the smallest possible distance (i.e. 5 -> 1 is -4, not 1020) - if position - self.last_position > 0: - if position < 512 and self.last_position > 512: - # We are moving to the right (positive) and have completed a new rotation - self.rotations = self.rotations + 1 - else: - if position > 512 and self.last_position < 512: - # We are moving to the left (negative) and have completed a new rotation - self.rotations = self.rotations - 1 + if (position - self.last_position) > 768.: + self.rotations = self.rotations - 1. + elif (position - self.last_position) < -768.: + self.rotations = self.rotations + 1. + # Save the last position for the next calculation self.last_position = position # compute the position based on the system parameters # linear position = (2pi*r)(n) + (2pi*r)(position/1024) = (2pi*r)(n + position/1024) = (pi*d)(n + position/1024) - return (self.PROPORTION)*(self.rotations + position/1024) + return (self.PROPORTION)*(self.rotations + position/1024.) -- cgit v1.2.3