diff options
Diffstat (limited to '')
| -rw-r--r-- | System_Python/encoder.py | 6 | ||||
| -rw-r--r-- | System_Python/system.py | 137 | ||||
| -rw-r--r-- | System_Python/test_Motor.py | 2 | ||||
| -rw-r--r-- | 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 |