aboutsummaryrefslogtreecommitdiffstats
path: root/System/pendulum
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--System/pendulum/system.py67
-rw-r--r--System/pendulum/test_Return_Home.py19
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()