Add encoder measurements via thread. This should help with linear encoder accuracy and SW interrupt accuracy. Coupled with changes from the interrupt enhancement branch, the limit behavior should be much improved.

Added a parameter to allow user to set their own SW limit-reached routine (default behavior is still the same). This should also help prevent over-excursion in the swingup test.
Slightly modified swingup test to use new SW limit-reached routine. This should help prevent the system from over-excursing when the soft limits of the program are reached. Theoretically the program shouldn't be able to hit the HW limits anymore.

2 files changed, 49 insertions, 12 deletions

diff --git a/System/system.py b/System/system.py
index 75573c5..c06063a 100644
--- a/System/system.py
+++ b/System/system.py
@@ -7,6 +7,7 @@ from time import sleep
 import RPi.GPIO as GPIO

 import sys

 import os

+from threading import Thread

 

 # IO pin definitions

 ### Motor pins

@@ -31,7 +32,7 @@ system_min_x = -16.5
 # System Class

 # This is the primary interface a student will use to control the pendulum.

 class System:

-    def __init__(self, negative_limit=float('nan'), positive_limit=float('nan'), angular_units='Degrees'):

+    def __init__(self, negative_limit=float('nan'), positive_limit=float('nan'), angular_units='Degrees', sw_limit_routine=self.limit_triggered):

         GPIO.setwarnings(False)

         # Initialize public variables

         self.max_x = system_max_x

@@ -46,6 +47,12 @@ class System:
         # We assume that the system has been initialized on startup to a 0 position, or that the previous run ended by returning the system to 0

         self.encoder_linear.set_zero()

         

+        # Setup a thread to constantly be measuring encoder positions

+        self.encoder_thread = Thread(target = self.encoder_thread_routine)

+        self.angular_position = 0.

+        self.linear_position = 0.

+        self.encoder_thread.start()

+        

         # Enable hardware interrupts for hardware limit switches

         GPIO.setup(limit_negative_pin, GPIO.IN, pull_up_down=GPIO.PUD_UP)

         GPIO.add_event_detect(limit_negative_pin, GPIO.FALLING, callback=self.negative_limit_callback)

@@ -64,6 +71,7 @@ class System:
             self.positive_soft_limit = float('nan')

         # NOTE: If only one limit has been defined, this should always work (hardware limits will be the absolute edge on the undefined side, although this would be difficult for users to utilize unless we provide the position of the hardware limits on each side

         # NOTE: If neither limit is defined, the hardware limits will be the only limits in effect.

+        self.sw_limit_routine = sw_limit_routine

         

         self.angular_units = angular_units

         

@@ -122,6 +130,7 @@ class System:
     def deinitialize(self):

         self.return_home()

         self.motor.brake()

+        self.encoder_thread.terminate()

         GPIO.cleanup()

     

     # Get the values of the encoders to determine the angular and linear position of the pendulum.

@@ -134,28 +143,43 @@ class System:
     #####       Positive values indicate the pendulum is right-of-center.

     #####       Negative values indicate the pendulum is left-of-center.

     def measure(self):

+        return (self.angular_position, self.linear_position)

+    # END measure()

+    

+    # Thread routine (0.1s interval). Get the values of the encoders to determine the angular and linear position of the pendulum.

+    # Values are saved in the class (self.angular_position and self.linear_position), which are then simply returned by measure()

+    def encoder_thread_routine(self):

         angular_position = self.encoder_angular.read_position(self.angular_units)

         if self.angular_units == 'Degrees':

             if angular_position > 180.:

                 angular_position = angular_position - 360.

-        linear_position = self.encoder_linear.read_position()

+        self.angular_position = angular_position

+        self.linear_position = self.encoder_linear.read_position()

         # Check soft limits

-        if (not math.isnan(self.negative_soft_limit) and linear_position < self.negative_soft_limit) or linear_position < self.min_x:

+        if (not math.isnan(self.negative_soft_limit) and self.linear_position < self.negative_soft_limit) or self.linear_position < self.min_x:

+            # SW limit reached: stop the motor, set the interrupted flag so that the motor cannot continue to move until the interrupt has been completely serviced

+            #self.interrupted = True

+            self.motor.brake()

             # Print negative soft limit violation to the results file.

             result_file = open(self.result_filename, "a")

             result_file.write("Negative software limit %f has been reached!" % self.negative_soft_limit)

             result_file.close()

             # Fire the limit trigger method (stops motor, kills program immediately).

-            self.limit_triggered()

-        if (not math.isnan(self.positive_soft_limit) and linear_position > self.positive_soft_limit) or linear_position > self.max_x:

+            self.sw_limit_routine()

+        if (not math.isnan(self.positive_soft_limit) and self.linear_position > self.positive_soft_limit) or self.linear_position > self.max_x:

+            # SW limit reached: stop the motor, set the interrupted flag so that the motor cannot continue to move until the interrupt has been completely serviced

+            #self.interrupted = True

+            self.motor.brake()

             # Print positive soft limit violation to the results file.

             result_file = open(self.result_filename, "a")

             result_file.write("Positive software limit %f has been reached!" % self.positive_soft_limit)

             result_file.close()

             # Fire the limit trigger method (stops motor, kills program immediately).

-            self.limit_triggered()

-        return (angular_position, linear_position)

-    # END measure()

+            self.sw_limit_routine()

+            # If this point is reached, the SW limit routine was user-defined and did not kill the program - un-set the 'interrupted' flag

+            #self.interrupted = False

+        # This thread should run on ~0.1s intervals

+        sleep(0.1)

     

     # 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.

diff --git a/System/system_swingup_test_2.py b/System/system_swingup_test_2.py
index 81d5419..e240b01 100644
--- a/System/system_swingup_test_2.py
+++ b/System/system_swingup_test_2.py
@@ -53,7 +53,7 @@ class SwingUpEnv():
     }

 

     def __init__(self):

-        self.sys = System(angular_units='Radians')

+        self.sys = System(angular_units='Radians', positive_limit=10., negative_limit=-10., sw_limit_routine=self.x_threshold_routine)

 		

         self.force_mag = 10.

         self.last_time = time.time()  # time for seconds between updates

@@ -73,6 +73,7 @@ class SwingUpEnv():
         self.state = None

 

         self.steps_beyond_done = None

+        self.done = False

 

     def seed(self, seed=None):

         self.np_random, seed = seeding.np_random(seed)

@@ -83,7 +84,9 @@ class SwingUpEnv():
         state = self.state

         x, x_dot, theta, theta_dot = state

         force = self.force_mag * action[0]

-        self.sys.adjust(force)

+        # Do not adjust the motor further if the x_threshold has been triggered by the SW limit

+        if self.done == False:

+            self.sys.adjust(force)

 

         costheta = math.cos(theta)

         sintheta = math.sin(theta)

@@ -105,10 +108,15 @@ class SwingUpEnv():
         self.state = (new_x, x_dot, new_theta, theta_dot)

         self.sys.add_results(new_theta, new_x, force)

 

-        done =  x < -self.x_threshold \

+        done =  theta_dot < -self.theta_dot_threshold \

+                or theta_dot > self.theta_dot_threshold \

+                or self.done == True

+        

+        '''done =  x < -self.x_threshold \

                 or x > self.x_threshold \

                 or theta_dot < -self.theta_dot_threshold \

-                or theta_dot > self.theta_dot_threshold

+                or theta_dot > self.theta_dot_threshold \

+                or self.done == True'''

         done = bool(done)

 

         if not done:

@@ -125,6 +133,10 @@ class SwingUpEnv():
 

         return np.array(self.state), reward, done, {'max_up_time' : self.max_up_time}

 

+    def x_threshold_routine(self):

+        self.done = True

+        self.sys.adjust(0)

+

     def reset(self, home = True):

         if home == True:

             self.sys.return_home()

@@ -138,6 +150,7 @@ class SwingUpEnv():
         self.max_up_time = 0

         self.up = False

         self.steps_beyond_done = None

+        self.done = False

         return np.array(self.state)

         

     def end(self):