diff options
| -rw-r--r-- | System_Python/system.py | 66 |
1 files changed, 42 insertions, 24 deletions
diff --git a/System_Python/system.py b/System_Python/system.py index 469f475..8f67872 100644 --- a/System_Python/system.py +++ b/System_Python/system.py @@ -18,15 +18,23 @@ encoder_data_pin = 3 ### Angular encoder pins
encoder_angular_cs_pin = 4
### Linear encoder pins
-encoder_linear_cs_pin = 5
+encoder_linear_cs_pin = 14
### Limit switch pins (configured to PULLUP)
limit_negative_pin = 19
limit_positive_pin = 26
+# System parameters
+system_max_x = 16.5
+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')):
+ GPIO.setwarnings(False)
+ # Initialize public variables
+ self.max_x = system_max_x
+ self.min_x = system_min_x
# Initialize the motor.
self.motor = Motor(motor_speed_pin, motor_forward_pin, motor_reverse_pin)
# Initialize the angular encoder.
@@ -70,22 +78,34 @@ class System: GPIO.remove_event_detect(limit_positive_pin)
# Begin moving slowly in the negative direction until the negative limit switch is triggered
if not GPIO.input(limit_negative_pin) == False:
- self.motor.move(-1)
- GPIO.wait_for_edge(limit_negative_pin, GPIO.FALLING)
+ self.motor.move(-4)
+ pressed = True
+ while pressed != False:
+ pressed = GPIO.input(limit_negative_pin)
+ sleep(0.01)
self.motor.brake()
# Set zero at the negative end of the track for easy reference in determining the extent
self.encoder_linear.set_zero()
+ sleep(1)
# Begin moving slowly in the positive direction until the positive limit switch is triggered
- self.motor.move(1)
- GPIO.wait_for_edge(limit_positive_pin, GPIO.FALLING)
+ self.motor.move(4)
+ pressed = True
+ while pressed != False:
+ # We must continue reading linear encoder motion to keep track of rotations
+ self.encoder_linear.read_position()
+ pressed = GPIO.input(limit_positive_pin)
+ sleep(0.01)
+ #GPIO.wait_for_edge(limit_positive_pin, GPIO.FALLING)
self.motor.brake()
# Get the current position (the extent of the track)
extent = self.encoder_linear.read_position()
# Move back towards the center until we reach position extent/2
position = extent
- self.motor.move(-1)
- while not position == extent / 2:
+ sleep(1)
+ self.motor.move(-4)
+ while position >= (extent / 2.):
position = self.encoder_linear.read_position()
+ sleep(0.01)
self.motor.brake()
# Set zero again: this is the real zero
self.encoder_linear.set_zero()
@@ -159,9 +179,9 @@ class System: position = self.encoder_linear.read_position()
# slowly move towards 0 until we get there
if position > 0:
- self.motor.move(-1)
+ self.motor.move(-4)
elif position < 0:
- self.motor.move(1)
+ self.motor.move(4)
while not position == 0:
position = self.encoder_linear.read_position()
self.motor.brake()
@@ -169,6 +189,7 @@ class System: # Callback for when negative limit switch is triggered.
def negative_limit_callback(self, channel):
+ self.motor.brake()
# Print negative limit trigger to the results file.
result_file = open(self.result_filename, "a")
result_file.write("Negative hardware limit has been reached!")
@@ -178,6 +199,7 @@ class System: # END negative_limit_callback
# Callback for when positive limit switch is triggered.
def positive_limit_callback(self, channel):
+ self.motor.brake()
# Print positive limit trigger to the results file.
result_file = open(self.result_filename, "a")
result_file.write("Positive hardware limit has been reached!")
@@ -186,8 +208,7 @@ class System: self.limit_triggered()
# END positive_limit_callback
def limit_triggered(self):
- self.motor.brake()
- sleep(2)
+ sleep(1)
self.return_home()
sys.exit(1)
# END System
@@ -205,23 +226,20 @@ class Linear_Encoder: # Set the zero position for the encoder
self.encoder.set_zero()
# Reset the internal position counter
- self.rotations = 0
- self.last_position = 0
+ self.rotations = 0.
+ self.last_position = 0.
def read_position(self):
- # Read the position of the encoder
- position = self.encoder.read_position('Raw')
+ # Read the position of the encoder (apply a noise filter, we don't need that much precision here)
+ position = float(self.encoder.read_position('Raw') & 0b1111111100)
# Compare to last known position
# NOTE: For now, assume that we are moving the smallest possible distance (i.e. 5 -> 1 is -4, not 1020)
- if position - self.last_position > 0:
- if position < 512 and self.last_position > 512:
- # We are moving to the right (positive) and have completed a new rotation
- self.rotations = self.rotations + 1
- else:
- if position > 512 and self.last_position < 512:
- # We are moving to the left (negative) and have completed a new rotation
- self.rotations = self.rotations - 1
+ if (position - self.last_position) > 768.:
+ self.rotations = self.rotations - 1.
+ elif (position - self.last_position) < -768.:
+ self.rotations = self.rotations + 1.
+
# Save the last position for the next calculation
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)
- return (self.PROPORTION)*(self.rotations + position/1024)
+ return (self.PROPORTION)*(self.rotations + position/1024.)
|