From 066662d02c3185331af137a14cb1caab3b3af356 Mon Sep 17 00:00:00 2001 From: Raspberry Pi Date: Tue, 15 Oct 2019 17:10:04 -0500 Subject: Updated encoder for proper zero setting and offset behavior. Tested system library and successfully got the encoder position controlling the motor speed. --- System_Python/encoder.py | 6 +- System_Python/system.py | 137 ++++++++++++++++++++++--------------------- System_Python/test_Motor.py | 2 +- System_Python/test_System.py | 16 +++-- 4 files changed, 83 insertions(+), 78 deletions(-) diff --git a/System_Python/encoder.py b/System_Python/encoder.py index 4069a86..8b7053c 100644 --- a/System_Python/encoder.py +++ b/System_Python/encoder.py @@ -58,15 +58,15 @@ class Encoder: # Pull CS high after finish reading GPIO.output(self.cs_pin,1) # Format with offset, Max is 1024 - data=(data+offset)%1024 + data=(data-self.offset)%1024 # Data is linearly mapped if format=="Raw": return data elif format=="Degrees": - degrees=data/(1024/360) + degrees=(data/1024.0)*360.0 return degrees elif format=="Radian": - radians=data/(1024/(2*math.pi)) + radians=(data/1024.0)*(2.0*math.pi) return radians else: print("ERROR. Invalid format (Raw, Degrees, Radians)") diff --git a/System_Python/system.py b/System_Python/system.py index d3a7ba4..1e532fa 100644 --- a/System_Python/system.py +++ b/System_Python/system.py @@ -1,6 +1,7 @@ #!/usr/bin/env python from motor import Motor from encoder import Encoder +import math # IO pin definitions ### Motor pins @@ -20,82 +21,82 @@ encoder_linear_cs_pin = 5 # This is the primary interface a student will use to control the pendulum. class System: def __init__(self): - # Initialize the motor. - self.motor = Motor(motor_speed_pin, motor_forward_pin, motor_reverse_pin) - # Initialize the angular encoder. - self.encoder_angular = Encoder(encoder_clock_pin, encoder_angular_cs_pin, encoder_data_pin) - self.encoder_angular.set_zero() - # Initialize the linear encoder. - self.encoder_linear = Linear_Encoder(encoder_clock_pin, encoder_linear_cs_pin, encoder_data_pin) - self.encoder_linear.set_zero() + # Initialize the motor. + self.motor = Motor(motor_speed_pin, motor_forward_pin, motor_reverse_pin) + # Initialize the angular encoder. + self.encoder_angular = Encoder(encoder_clock_pin, encoder_angular_cs_pin, encoder_data_pin) + self.encoder_angular.set_zero() + # Initialize the linear encoder. + self.encoder_linear = Linear_Encoder(encoder_clock_pin, encoder_linear_cs_pin, encoder_data_pin) + self.encoder_linear.set_zero() # END __init__() - - # Get the values of the encoders to determine the angular and linear position of the pendulum. - # Values are returned as a tuple: (angle, linear). - ### angle: 0 indicates the pendulum is exactly straight up. - ##### 180 or -180 indicate the pendulum is exactly straight down. - ##### Positive values indicate the pendulum is leaning to the right. - ##### Negative values indicate the pendulum is leaning to the left. - ### linear: 0 indicates the pendulum is exactly in the middle of the track. - ##### Positive values indicate the pendulum is right-of-center. - ##### Negative values indicate the pendulum is left-of-center. - def measure(self): - angular_position = self.encoder_angular.read_position('Degrees') - if angular_position > 180: - angular_position = angular_position - 360 - #linear_position = self.encoder_linear.read_position() - linear_position = 0 - return (angular_position, linear_position) - # END measure() - - # Adjust the pendulum's linear position using the motor. - ### speed: Acceptable values range from -100 to 100 (as a percentage), with 100/-100 being the maximum adjustment speed. - ##### Negative values will move the pendulum to the left. - ##### Positive values will move the pendulum to the right. - def adjust(self, speed): - # cap the speed inputs - if speed > 100.0: - speed = 100.0 - if speed < -100.0: - speed = -100.0 - # change the motor speed - # TODO: Make sure the motor is oriented so that positive speed the correct direction (same for negative). Change the values otherwise. - self.motor.move(speed) - # END adjust() + + # Get the values of the encoders to determine the angular and linear position of the pendulum. + # Values are returned as a tuple: (angle, linear). + ### angle: 0 indicates the pendulum is exactly straight up. + ##### 180 or -180 indicate the pendulum is exactly straight down. + ##### Positive values indicate the pendulum is leaning to the right. + ##### Negative values indicate the pendulum is leaning to the left. + ### linear: 0 indicates the pendulum is exactly in the middle of the track. + ##### Positive values indicate the pendulum is right-of-center. + ##### Negative values indicate the pendulum is left-of-center. + def measure(self): + angular_position = self.encoder_angular.read_position('Degrees') + if angular_position > 180: + angular_position = angular_position - 360 + #linear_position = self.encoder_linear.read_position() + linear_position = 0 + return (angular_position, linear_position) + # END measure() + + # Adjust the pendulum's linear position using the motor. + ### speed: Acceptable values range from -100 to 100 (as a percentage), with 100/-100 being the maximum adjustment speed. + ##### Negative values will move the pendulum to the left. + ##### Positive values will move the pendulum to the right. + def adjust(self, speed): + # cap the speed inputs + if speed > 100.0: + speed = 100.0 + if speed < -100.0: + speed = -100.0 + # change the motor speed + # TODO: Make sure the motor is oriented so that positive speed the correct direction (same for negative). Change the values otherwise. + self.motor.coast() + self.motor.move(speed) + # END adjust() # END System # Linear Encoder class # This class is to help with using an absolute encoder for linear position sensing as assembled in the physical system. # The function definitions here are the same as with the regular encoder (pseudo-interface). class Linear_Encoder: - DIAMETER = 4 # MEASURE THIS - - def __init__(self, clk_pin, cs_pin, data_pin): - self.encoder = Encoder(clk_pin, cs_pin, data_pin) - set_zero() - def set_zero(self): + DIAMETER = 4.0 # MEASURE THIS + + def __init__(self, clk_pin, cs_pin, data_pin): + self.encoder = Encoder(clk_pin, cs_pin, data_pin) + self.set_zero() + def set_zero(self): # Set the zero position for the encoder self.encoder.set_zero() - # Reset the internal position counter - self.rotations = 0 - self.last_position = 0 + # Reset the internal position counter + self.rotations = 0 + self.last_position = 0 def read_position(self): # Read the position of the encoder - position = self.encoder.read_position('Raw') - # 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++ - else: - if position > 512 and self.last_position < 512: - # We are moving to the left (negative) and have completed a new rotation - self.rotations-- - # 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 (pi*DIAMETER)*(self.rotations + position/1024) - \ No newline at end of file + position = self.encoder.read_position('Raw') + # 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 + # 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 (math.pi*DIAMETER)*(self.rotations + position/1024) diff --git a/System_Python/test_Motor.py b/System_Python/test_Motor.py index 600f209..99db520 100644 --- a/System_Python/test_Motor.py +++ b/System_Python/test_Motor.py @@ -11,7 +11,7 @@ m = Motor(speed_pin, forward_pin, reverse_pin) dir = 'ascending' speed = 0.0 while 1: - m.Move(speed) + m.move(speed) if speed >= 100.0: dir = 'descending' elif speed <= -100.0: diff --git a/System_Python/test_System.py b/System_Python/test_System.py index af218e0..32ff56d 100644 --- a/System_Python/test_System.py +++ b/System_Python/test_System.py @@ -1,17 +1,21 @@ # This test file implements a super simple control-type function for testing the System library. # DO NOT TEST ON ASSEMBLED PHYSICAL SYSTEM! It will probably break it. +import time from system import System # Return a speed based on current encoder angle. # Convert an angle to speed (180 degrees = max speed) def control_function(angle): - return (angle / 180.0) * 100.0 - + return (abs(angle) / 180.0) * 100.0 + # Main program sys = System() while 1: - angle, linear = sys.measure() - speed = control_function(angle) - sys.adjust(speed) - \ No newline at end of file + angle, linear = sys.measure() + #print(angle) + speed = control_function(angle) + print(speed) + sys.adjust(speed) + time.sleep(0.05) + \ No newline at end of file -- cgit v1.2.3