Fixed encoder zeroing. Other minor changes, still trying to get the swingup controller working. Not sure if the code is good enough to make it happen...

1 files changed, 14 insertions, 3 deletions

diff --git a/System_Python/system.py b/System_Python/system.py
index b5a68cc..c6145ce 100644
--- a/System_Python/system.py
+++ b/System_Python/system.py
@@ -94,7 +94,7 @@ class System:
         pressed = True

         while pressed != False:

             # We must continue reading linear encoder motion to keep track of rotations

-            self.encoder_linear.read_position()

+            position = self.encoder_linear.read_position()

             pressed = GPIO.input(limit_positive_pin)

             sleep(0.01)

         #GPIO.wait_for_edge(limit_positive_pin, GPIO.FALLING)

@@ -116,6 +116,12 @@ class System:
         GPIO.add_event_detect(limit_positive_pin, GPIO.FALLING, callback=self.positive_limit_callback, bouncetime=300)

     # END initialize

     

+    # Return home, cleanup IO. This should be called when exiting the program

+    def deinitialize(self):

+        self.return_home()

+        self.motor.brake()

+        GPIO.cleanup()

+    

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

@@ -185,11 +191,16 @@ class System:
             self.motor.move(-4)

             while position > 0:

                 position = self.encoder_linear.read_position()

+                sleep(0.01)

+            self.motor.brake()

+            return

         elif position < 0:

             self.motor.move(4)

             while position < 0:

                 position = self.encoder_linear.read_position()

-        self.motor.brake()

+                sleep(0.01)

+            self.motor.brake()

+            return

     # END return_home

     

     # Callback for when negative limit switch is triggered.

@@ -214,7 +225,7 @@ class System:
     # END positive_limit_callback

     def limit_triggered(self):

         sleep(1)

-        self.return_home()

+        self.deinitialize()

         sys.exit(1)

 # END System