Add code that was not committed before

1 files changed, 58 insertions, 10 deletions

diff --git a/System/system.py b/System/system.py
index 7b2ffcf..ff7c778 100755..100644
--- a/System/system.py
+++ b/System/system.py
@@ -7,6 +7,14 @@ from time import sleep
 import RPi.GPIO as GPIO

 import sys

 import os

+count2 = 0

+##

+import cmath

+##

+

+

+

+

 from threading import Thread, Lock

 

 # IO pin definitions

@@ -15,19 +23,26 @@ motor_speed_pin = 17
 motor_forward_pin = 27

 motor_reverse_pin = 22

 ### Encoder pins (shared by both encoders)

-encoder_clock_pin = 2

-encoder_data_pin = 3

+encoder_clock_pin = 3

+encoder_data_pin = 2

 ### Angular encoder pins

 encoder_angular_cs_pin = 4

 ### Linear encoder pins

-encoder_linear_cs_pin = 14

+encoder_linear_cs_pin = 23

 ### Limit switch pins (configured to PULLUP)

-limit_negative_pin = 19

-limit_positive_pin = 26

+

+#FLIPPING THESE BELOW

+#limit_negative_pin = 19

+#limit_positive_pin = 26

+limit_negative_pin = 26

+limit_positive_pin = 19

 

 # System parameters

 system_max_x = 16.5

 system_min_x = -16.5

+downloads_reference_dest = "."

+default_results_fileName = "results.csv"

+

 

 # System Class

 # This is the primary interface a student will use to control the pendulum.

@@ -74,9 +89,11 @@ class System:
         

         # Create and setup results file (to be sent back to the server and displayed/downloaded to the user)

         # Results file is a CSV with the following entries: angle, position, speed

-        self.result_filename = "Results/" + os.path.basename(sys.argv[0]).split('.')[0] + "_results.csv"

-        

-        result_file = open(self.result_filename, "w+")

+        self.result_filename = downloads_reference_dest + "/Downloads/" + default_results_fileName

+        print("self.result_filename")

+        print(self.result_filename)

+        # Open the file for write mode.  The file contents will get cleared and overwritten

+        result_file = open(self.result_filename, "w")

         result_file.write("timestamp,angle(" + angular_units + "),position(inches),speed(percentage)\n")

         result_file.close()

         

@@ -97,6 +114,7 @@ class System:
     # END __del__()

     

     def initialize(self):

+        print("begin initialize")

         # Temporarily disable the limit switch interrupts: we do not want the program to exit if the switch is triggered

         GPIO.remove_event_detect(limit_negative_pin)

         GPIO.remove_event_detect(limit_positive_pin)

@@ -106,8 +124,10 @@ 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")

         # Set zero at the negative end of the track for easy reference in determining the extent

         self.encoder_linear.set_zero()

         sleep(1)

@@ -119,6 +139,7 @@ class System:
             pressed = GPIO.input(limit_positive_pin)

             sleep(0.01)

         self.motor.brake()

+        print("hit positive endstop")

         # Get the current position (the extent of the track)

         extent = self.linear_position

         # Move back towards the center until we reach position extent/2

@@ -127,6 +148,7 @@ class System:
         self.motor.move(-4)

         while position >= (extent / 2.):

             position = self.linear_position

+            #print(position)

             sleep(0.015) 

         self.motor.brake()

         # Set zero again: this is the real zero

@@ -134,6 +156,7 @@ class System:
         # 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)

+        print("Finsihed the initaialize func")

     # END initialize

     

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

@@ -308,9 +331,32 @@ class Linear_Encoder:
         # Reset the internal position counter

         self.rotations = 0.

         self.last_position = 0.

+        ###sam debug

+        self.last_position_filter = 0.

+        ###sam debug

     def read_position(self):

+        ###sam debug

+        global count2

+        test = 0

+        count = 0

+        while test == 0:

+            count += 1

+            position = float(self.encoder.read_position('Raw') & 0b1111111100)

+            complex_current = cmath.exp(((position*2*math.pi)/1023)*1j)

+            complex_last = cmath.exp(((self.last_position*2*math.pi)/1023)*1j)

+            distance = math.sqrt((complex_current.real-complex_last.real)**2 + (complex_current.imag-complex_last.imag)**2)

+            if distance < 0.5 or count > 5: #this corresponds to a difference of 50 in the raw encoder position

+                test = 1

+            #print(count)

+            count2 = count2 + count - 1

+            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)

-        position = float(self.encoder.read_position('Raw') & 0b1111111100)

+        #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) > 768.:

@@ -322,4 +368,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)

-        return (self.PROPORTION)*(self.rotations + position/1024.)

+        print("sled positin in inches")

+        print(self.PROPORTION*(self.rotations + position/1024.))

+        return((self.PROPORTION)*(self.rotations + position/1024.))