Updated encoder for proper zero setting and offset behavior. Tested system library and successfully got the encoder position controlling the motor speed.

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