From 6e8c893503cac801d930c1785826463204f115e0 Mon Sep 17 00:00:00 2001 From: Matt Strapp Date: Thu, 14 Apr 2022 13:39:18 -0500 Subject: Add new changes from local testing Signed-off-by: Matt Strapp --- System/Tests/test_angular.py | 19 +++++++++++ System/pendulum/system.py | 67 ++++++++++++++++++++++++------------- System/pendulum/test_Return_Home.py | 19 +++++++++++ System/test_Return_Home.py | 17 ++++++++++ System/test_angular.py | 19 +++++++++++ System/test_back_forth_10_times.py | 33 ++++++++++++++++++ Web | 2 +- 7 files changed, 151 insertions(+), 25 deletions(-) create mode 100644 System/Tests/test_angular.py create mode 100644 System/pendulum/test_Return_Home.py create mode 100644 System/test_Return_Home.py create mode 100644 System/test_angular.py create mode 100644 System/test_back_forth_10_times.py diff --git a/System/Tests/test_angular.py b/System/Tests/test_angular.py new file mode 100644 index 0000000..c46d957 --- /dev/null +++ b/System/Tests/test_angular.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(0) +ang,lin = sys.measure() +sleep(1.0) +while lin < 10: + ang,lin = sys.measure() + sleep(0.01) +sys.return_home() 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() diff --git a/System/test_Return_Home.py b/System/test_Return_Home.py new file mode 100644 index 0000000..98b291b --- /dev/null +++ b/System/test_Return_Home.py @@ -0,0 +1,17 @@ +from pendulum import System +from time import sleep + +sys = System() +sys.initialize() + +ang,lin = sys.measure() + +sys.torque(-100) + +ang,lin = sys.measure() +sleep(0.01) +while lin > -20: + ang,lin = sys.measure() + sleep(0.01) + +sys.return_home() diff --git a/System/test_angular.py b/System/test_angular.py new file mode 100644 index 0000000..c46d957 --- /dev/null +++ b/System/test_angular.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(0) +ang,lin = sys.measure() +sleep(1.0) +while lin < 10: + ang,lin = sys.measure() + sleep(0.01) +sys.return_home() diff --git a/System/test_back_forth_10_times.py b/System/test_back_forth_10_times.py new file mode 100644 index 0000000..b3d7289 --- /dev/null +++ b/System/test_back_forth_10_times.py @@ -0,0 +1,33 @@ +from pendulum import System # REQUIRED import to use the inverted pendulum +from time import sleep # OPTIONAL import, needed to use the sleep function + +sys = System() # Create System object. This is REQUIRED to use the inverted pendulum. +sys.initialize() # Initialize the inverted pendulum. This is REQUIRED. The pendulum will move all the way to the left, then all the way to the right, then go to the center and pause for 1 second. + +ang,lin = sys.measure() # Measure the angular position of the pendulum, and the linear position of the sled. + + # This example moves the inverted pendulum back and forth 10 times. +counter = 0 +while (counter < 10): # Loop 10 times + + sleep(0.2) # Sleep for 0.2 seconds + sys.torque(50) # Apply a torque of 5. The max is 80. The inverted pendulum will move the right. + + while lin < 6: # Loop while the linear position of the sled is less than 10 inches right of center. + ang,lin = sys.measure() # Measure again + sleep(0.01) # Sleep for 0.01 seconds + + sys.torque(0) # Apply a torque of 0. This will let the pendulum coast in whichever direction it was headed. It will stop shortly after. + sleep(0.6) # Sleep for 0.6 seconds. This gives the pendulum time to stop and stay idle. This isn't required. + sys.torque(-30) # Apply a torque of -5. The max is -80. This is still a torque of 5, but in the opposite direction. The pendulum moves left. + + while lin > -6: # Loop until a linear position of -10 (10 inches left of center) is measured. + ang,lin = sys.measure() + sleep(0.01) + + sys.torque(0) # Have the pendulum coast + sleep(0.1) # Sleep for 0.1 seconds + + counter += 1 # increment the counter variable + +sys.return_home() # REQUIRED function call, always return home at the end of every run. diff --git a/Web b/Web index 7389209..747addc 160000 --- a/Web +++ b/Web @@ -1 +1 @@ -Subproject commit 73892092f9af26658d16f5cb9c5b79b5aed2b348 +Subproject commit 747addc70e0f0ec34f31a50f3faa45687d065afc -- cgit v1.2.3