Add new changes from local testing

Signed-off-by: Matt Strapp <matt@mattstrapp.net>

2 files changed, 62 insertions, 24 deletions

diff --git a/System/pendulum/system.py b/System/pendulum/system.py
index 07c04bf..4ea237e 100644
--- a/System/pendulum/system.py
+++ b/System/pendulum/system.py
@@ -29,13 +29,10 @@ encoder_data_pin = 2
 encoder_angular_cs_pin = 4
 ### Linear encoder pins
 encoder_linear_cs_pin = 23
-### Limit switch pins (configured to PULLUP)
 
-#FLIPPING THESE BELOW
-#limit_negative_pin = 19
-#limit_positive_pin = 26
+### Limit switch pins (configured to PULLUP)
 limit_negative_pin = 26
-limit_positive_pin = 19
+limit_positive_pin = 14
 
 # System parameters
 system_max_x = 16.5
@@ -93,7 +90,7 @@ class System:
         print("self.result_filename")
         print(self.result_filename)
         # Open the file for write mode.  The file will get created, assuming it does not already exist.
-        result_file = open(self.result_filename, "x")
+        result_file = open(self.result_filename, "w")
         result_file.write(f"timestamp,angle({angular_units}),position(inches),torque(percentage)\n")
         result_file.close()
         
@@ -103,6 +100,8 @@ class System:
         self.encoder_thread.setDaemon(True)
         self.angular_position = 0.
         self.linear_position = 0.
+        self.torqueVal = 0.
+        self.record_data = False
         self.encoder_thread.start()
     # END __init__()
     
@@ -124,7 +123,6 @@ class System:
             pressed = True
             while pressed != False:
                 pressed = GPIO.input(limit_negative_pin)
-                #print(pressed)
                 sleep(0.01)
         self.motor.brake()
         print("hit negative end stop")
@@ -154,8 +152,16 @@ class System:
         # Set zero again: this is the real zero
         self.encoder_linear.set_zero()
         # Re-enable the limit switch interrupts
-        GPIO.add_event_detect(limit_negative_pin, GPIO.FALLING, callback=self.negative_limit_callback, bouncetime=300)
-        GPIO.add_event_detect(limit_positive_pin, GPIO.FALLING, callback=self.positive_limit_callback, bouncetime=300)
+        GPIO.add_event_detect(limit_negative_pin, GPIO.FALLING, callback=self.negative_limit_callback, bouncetime=1)
+        GPIO.add_event_detect(limit_positive_pin, GPIO.FALLING, callback=self.positive_limit_callback, bouncetime=1)
+
+        self.record_data = True
+        sleep(1)
+        if (self.linear_position < -2 or self.linear_position > 2):
+            print("Initialization failed")
+            self.deinitialize()
+            sys.exit(10)
+
         print("Finished the initialize func")
     # END initialize
     
@@ -164,8 +170,6 @@ class System:
         self.return_home()
         self.motor.brake()
         self.deinit = True
-        if self.encoder_thread.is_alive():
-            self.encoder_thread.join()
         sleep(1)
         GPIO.cleanup()
     
@@ -194,6 +198,8 @@ class System:
                     angular_position = angular_position - 360.
             self.angular_position = angular_position
             self.linear_position = self.encoder_linear.read_position()
+            if self.record_data:
+                self.add_results(self.angular_position, self.linear_position, self.torqueVal)
             # Check soft limits
             if (not math.isnan(self.negative_soft_limit)) and self.linear_position < self.negative_soft_limit: #or self.linear_position < self.min_x:
                 if limit_serviced == False:
@@ -206,7 +212,7 @@ class System:
                     result_file.write("Negative software limit %f has been reached!\n" % self.negative_soft_limit)
                     result_file.close()
                     # Fire the limit trigger method
-                    self.sw_limit_routine()
+                    self.sw_limit_routine(5)
             elif (not math.isnan(self.positive_soft_limit)) and self.linear_position > self.positive_soft_limit: #or self.linear_position > self.max_x:
                 if limit_serviced == False:
                     limit_serviced = True
@@ -218,7 +224,7 @@ class System:
                     result_file.write("Positive software limit %f has been reached!\n" % self.positive_soft_limit)
                     result_file.close()
                     # Fire the limit trigger method
-                    self.sw_limit_routine()
+                    self.sw_limit_routine(6)
             elif limit_serviced == True and self.linear_position > (self.negative_soft_limit+0.5) and self.linear_position < (self.positive_soft_limit-0.5):
                 # Clear the limit service flag once we return to a reasonable range that the limit will not trigger again
                 limit_serviced = False
@@ -229,21 +235,24 @@ class System:
     ### torque: Acceptable values range from -100 to 100 (as a percentage), with 100/-100 being the maximum adjustment torque.
     #####      Negative values will move the pendulum to the left.
     #####      Positive values will move the pendulum to the right.
-    def adjust(self, torque):
+    ##### Note: Torque greater than 80 doesn't seem to work, so it's capped at 80 for now
+    def torque(self, torque):
         if self.interrupted == False:
             if torque != 0:
                 # cap the torque inputs
-                if torque > 100.:
-                    torque = 100.
-                if torque < -100.:
-                    torque = -100.
+                if torque > 80.:
+                    torque = 80.
+                if torque < -80.:
+                    torque = -80.
                 # change the motor torque
                 # TODO: Make sure the motor is oriented so that positive torque the correct direction (same for negative). Change the values otherwise.
                 self.motor.coast()
                 self.motor.move(torque)
+                self.torqueVal = torque
             else:
+                self.torqueVal = 0
                 self.motor.coast()
-    # END adjust()
+    # END torque()
     
     # Append data to the results file
     def add_results(self, angle, position, torque):
@@ -257,6 +266,14 @@ class System:
         # Close the results file
         result_file.close()
     # END add_results
+
+    def add_note_to_results(self, note):
+        # open the results file
+        result_file = open(self.result_filename, "a")
+        # Write note
+        result_file.write("%s\n" % note)
+        # Close the results file
+        result_file.close()
     
     def add_log(self, message):
         # open the results file
@@ -270,6 +287,7 @@ class System:
     
     # Go back to the zero position (linear) so that the next execution starts in the correct place.
     def return_home(self):
+        self.record_data = False
         position = self.linear_position
         # slowly move towards 0 until we get there
         if position > 0:
@@ -311,9 +329,10 @@ class System:
         self.limit_triggered(4)
     # END positive_limit_callback
     def limit_triggered(self, code):
-        sleep(1)
+        sleep(0.01)
         self.deinitialize()
         sys.exit(code)
+
 # END System
 
 # Linear Encoder class
@@ -349,8 +368,8 @@ class Linear_Encoder:
                 test = 1
             #print(count)
             count2 = count2 + count - 1
-            print("global count")
-            print(count2)
+            #print("global count")
+            #print(count2)
         ###sam debug
 
         # Read the position of the encoder (apply a noise filter, we don't need that much precision here)
@@ -368,6 +387,6 @@ class Linear_Encoder:
         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)
-        print("sled position in inches")
-        print(self.PROPORTION*(self.rotations + position/1024.))
+        #print("sled position in inches")
+        #print(self.PROPORTION*(self.rotations + position/1024.))
         return((self.PROPORTION)*(self.rotations + position/1024.))
diff --git a/System/pendulum/test_Return_Home.py b/System/pendulum/test_Return_Home.py
new file mode 100644
index 0000000..9f45925
--- /dev/null
+++ b/System/pendulum/test_Return_Home.py
@@ -0,0 +1,19 @@
+from pendulum import System

+from time import sleep

+        

+# Main program

+#print("before system()call")

+sys = System()

+#print("after system() call")

+sys.initialize()

+#print("after sys.inintalize called")

+

+ang,lin = sys.measure()

+print("Starting position before moving: " + str(lin))

+sys.adjust(4)

+ang,lin = sys.measure()

+sleep(0.01)

+while lin < 10:

+    ang,lin = sys.measure()

+    sleep(0.01)

+sys.return_home()