API tools faq
paste
Advertisement
Guest User

Untitled

a guest
Nov 22nd, 2017
70
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 15.89 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/python
  2. # -*- coding: utf-8 -*-
  3. import RPi.GPIO as GPIO  # Import GPIO library
  4. import time  # Import time library
  5. import threading
  6. import signal
  7. import math
  8. import numpy as np
  9.  
  10. # IGNORE:
  11. class PiSoftPwm(threading.Thread):
  12.  
  13.     def __init__(
  14.         self,
  15.         baseTime,
  16.         nbSlices,
  17.         gpioPin,
  18.         gpioScheme,
  19.         ):
  20.         """
  21.     Init the PiSoftPwm instance. Expected parameters are :
  22.     - baseTime : the base time in seconds for the PWM pattern. You may choose a small value (i.e 0.01 s)
  23.     - nbSlices : the number of divisions of the PWM pattern. A single pulse will have a min duration of baseTime * (1 / nbSlices)
  24.     - gpioPin : the pin number which will act as PWM ouput
  25.     - gpioScheme : the GPIO naming scheme (see RPi.GPIO documentation)
  26.     """
  27.  
  28.         self.sliceTime = baseTime / nbSlices
  29.         self.baseTime = baseTime
  30.         self.nbSlices = nbSlices
  31.         self.gpioPin = gpioPin
  32.         self.terminated = False
  33.         self.toTerminate = False
  34.         GPIO.setmode(gpioScheme)
  35.  
  36.     def start(self, nbSlicesOn):
  37.         """
  38.    Start PWM output. Expected parameter is :
  39.    - nbSlicesOn : number of divisions (on a total of nbSlices - see init() doc) to set HIGH output on the GPIO pin
  40.  
  41.    Exemple : with a total of 100 slices, a baseTime of 1 second, and an nbSlicesOn set to 25, the PWM pattern will
  42.    have a duty cycle of 25%. With a duration of 1 second, will stay HIGH for 1*(25/100) seconds on HIGH output, and
  43.    1*(75/100) seconds on LOW output.
  44.    """
  45.  
  46.         self.nbSlicesOn = nbSlicesOn
  47.         GPIO.setup(self.gpioPin, GPIO.OUT)
  48.         self.thread = threading.Thread(None, self.run, None, (), {})
  49.         self.thread.start()
  50.  
  51.     def run(self):
  52.  
  53.         """
  54.    Run the PWM pattern into a background thread. This function should not be called outside of this class.
  55.    """
  56.  
  57.         while self.toTerminate == False:
  58.             GPIO.setmode(GPIO.BCM)
  59.             if self.nbSlicesOn > 0:
  60.                 GPIO.output(self.gpioPin, GPIO.HIGH)
  61.                 time.sleep(self.nbSlicesOn * self.sliceTime)
  62.             if self.nbSlicesOn < self.nbSlices:
  63.                 GPIO.output(self.gpioPin, GPIO.LOW)
  64.                 time.sleep((self.nbSlices - self.nbSlicesOn)
  65.                            * self.sliceTime)
  66.         self.terminated = True
  67.  
  68.     def changeNbSlicesOn(self, nbSlicesOn):
  69.         """
  70.    Change the duration of HIGH output of the pattern. Expected parameter is :
  71.    - nbSlicesOn : number of divisions (on a total of nbSlices - see init() doc) to set HIGH output on the GPIO pin
  72.  
  73.    Exemple : with a total of 100 slices, a baseTime of 1 second, and an nbSlicesOn set to 25, the PWM pattern will
  74.    have a duty cycle of 25%. With a duration of 1 second, will stay HIGH for 1*(25/100) seconds on HIGH output, and
  75.    1*(75/100) seconds on LOW output.
  76.    """
  77.  
  78.         self.nbSlicesOn = nbSlicesOn
  79.  
  80.     def changeNbSlices(self, nbSlices):
  81.         """
  82.    Change the number of slices of the PWM pattern. Expected parameter is :
  83.    - nbSlices : number of divisions of the PWM pattern.
  84.  
  85.    Exemple : with a total of 100 slices, a baseTime of 1 second, and an nbSlicesOn set to 25, the PWM pattern will
  86.    have a duty cycle of 25%. With a duration of 1 second, will stay HIGH for 1*(25/100) seconds on HIGH output, and
  87.    1*(75/100) seconds on LOW output.
  88.    """
  89.  
  90.         if self.nbSlicesOn > nbSlices:
  91.             self.nbSlicesOn = nbSlices
  92.  
  93.         self.nbSlices = nbSlices
  94.         self.sliceTime = self.baseTime / self.nbSlices
  95.  
  96.     def changeBaseTime(self, baseTime):
  97.         """
  98.    Change the base time of the PWM pattern. Expected parameter is :
  99.    - baseTime : the base time in seconds for the PWM pattern.
  100.  
  101.    Exemple : with a total of 100 slices, a baseTime of 1 second, and an nbSlicesOn set to 25, the PWM pattern will
  102.    have a duty cycle of 25%. With a duration of 1 second, will stay HIGH for 1*(25/100) seconds on HIGH output, and
  103.    1*(75/100) seconds on LOW output.
  104.    """
  105.  
  106.         self.baseTime = baseTime
  107.         self.sliceTime = self.baseTime / self.nbSlices
  108.  
  109.     def stop(self):
  110.         """
  111.    Stops PWM output.
  112.    """
  113.  
  114.         self.toTerminate = True
  115.         while self.terminated == False:
  116.       # Just wait
  117.             time.sleep(0.01)
  118.  
  119.         GPIO.output(self.gpioPin, GPIO.LOW)
  120.         GPIO.setup(self.gpioPin, GPIO.IN)
  121.  
  122. # IGNORE
  123. class Motor():
  124.     def __init__(self):
  125.         # MC33932 pins
  126.  
  127.         self.PWMA = 25
  128.         self.PWMB = 22
  129.         self._IN1 = 24
  130.         self._IN2 = 23
  131.         self._IN3 = 17
  132.         self._IN4 = 27
  133.  
  134.         # Initialize PWMA PWMB
  135.  
  136.         GPIO.setmode(GPIO.BCM)
  137.         GPIO.setup(self.PWMA, GPIO.OUT)
  138.         GPIO.setup(self.PWMB, GPIO.OUT)
  139.         GPIO.setup(self._IN1, GPIO.OUT)
  140.         GPIO.setup(self._IN2, GPIO.OUT)
  141.         GPIO.setup(self._IN3, GPIO.OUT)
  142.         GPIO.setup(self._IN4, GPIO.OUT)
  143.         GPIO.output(self.PWMA, True)
  144.         GPIO.output(self.PWMB, True)
  145.  
  146.         # Initialize PWM outputs
  147.  
  148.         self.OUT_1 = PiSoftPwm(0.1, 100, self._IN1, GPIO.BCM)
  149.         self.OUT_2 = PiSoftPwm(0.1, 100, self._IN2, GPIO.BCM)
  150.         self.OUT_3 = PiSoftPwm(0.1, 100, self._IN3, GPIO.BCM)
  151.         self.OUT_4 = PiSoftPwm(0.1, 100, self._IN4, GPIO.BCM)
  152.  
  153.         # Close pwm output
  154.  
  155.         self.OUT_1.start(0)
  156.         self.OUT_2.start(0)
  157.         self.OUT_3.start(0)
  158.         self.OUT_4.start(0)
  159.  
  160.         self.frequency = 0.01
  161.         self.duty = 20
  162.  
  163.     def Setting(self, frequency, duty):
  164.         self.frequency = frequency
  165.         self.duty = duty
  166.  
  167.     def Go_1(self, duty=100, freq=0.01):
  168.         self.OUT_1.changeBaseTime(freq)
  169.         self.OUT_2.changeBaseTime(freq)
  170.         self.OUT_1.changeNbSlicesOn(duty)
  171.         self.OUT_2.changeNbSlicesOn(0)
  172.  
  173.     def Back_1(self,duty=100, freq=0.01):
  174.         self.OUT_1.changeBaseTime(freq)
  175.         self.OUT_2.changeBaseTime(freq)
  176.         self.OUT_1.changeNbSlicesOn(0)
  177.         self.OUT_2.changeNbSlicesOn(duty)
  178.  
  179.     def Go_2(self,duty=100,freq=0.01):
  180.         self.OUT_3.changeBaseTime(freq)
  181.         self.OUT_4.changeBaseTime(freq)
  182.         self.OUT_3.changeNbSlicesOn(duty)
  183.         self.OUT_4.changeNbSlicesOn(0)
  184.  
  185.     def Back_2(self,duty=100, freq=0.01):
  186.         self.OUT_3.changeBaseTime(freq)
  187.         self.OUT_4.changeBaseTime(freq)
  188.         self.OUT_3.changeNbSlicesOn(0)
  189.         self.OUT_4.changeNbSlicesOn(duty)
  190.  
  191.     def Stop(self):
  192.         self.OUT_1.changeNbSlicesOn(0)
  193.         self.OUT_2.changeNbSlicesOn(0)
  194.         self.OUT_3.changeNbSlicesOn(0)
  195.         self.OUT_4.changeNbSlicesOn(0)
  196.  
  197.  
  198. # change under advisement
  199. TIMEOUT_BETWEEN_ULTRASOUND = 0.1
  200.  
  201. # IGNORE
  202. class UltrasoundSensor(threading.Thread):
  203.  
  204.     def __init__(self, trig_pin, echo_pin):
  205.         self.trig_pin = trig_pin
  206.         self.echo_pin = echo_pin
  207.         self.terminated = False
  208.         self.toTerminate = False
  209.         self.val = -1
  210.  
  211.     def start(self):
  212.         self.thread = threading.Thread(None, self.run, None, (), {})
  213.         self.thread.start()
  214.         return self
  215.  
  216.     def stop(self):
  217.         self.toTerminate = True
  218.         while self.terminated == False:
  219.             time.sleep(0.01)
  220.        
  221.  
  222.     def setup(self):
  223.        
  224.         GPIO.setmode(GPIO.BCM)
  225.         GPIO.setup(self.trig_pin, GPIO.OUT)  # Set pin as GPIO out
  226.         GPIO.setup(self.echo_pin, GPIO.IN)  # Set pin as GPIO in
  227.    
  228.     def sense(self):
  229.         return self.val
  230.     def run(self):
  231.         self.setup()
  232.        
  233.         while not self.toTerminate:
  234.            
  235.             self.setup()
  236.             # Return distance in cm
  237.             GPIO.output(self.trig_pin, False)
  238.             time.sleep(0.05)
  239.             self.setup()
  240.             GPIO.output(self.trig_pin, True)  # Set self.trig_pin as HIGH
  241.             time.sleep(0.0001)  # Delay of 0.00001 seconds
  242.  
  243.             self.setup()
  244.             GPIO.output(self.trig_pin, False)
  245.  
  246.             pulse_start = pulse_end = None
  247.             start_time = time.time()
  248.             while GPIO.input(self.echo_pin) == 0 and time.time() \
  249.                 - start_time <= TIMEOUT_BETWEEN_ULTRASOUND:  # Check whether the self.echo_pin is LOW
  250.                 pulse_start = time.time()  # Saves the last known time of LOW pulse
  251.  
  252.             start_time = time.time()
  253.             while GPIO.input(self.echo_pin) == 1 and time.time() \
  254.                 - start_time <= TIMEOUT_BETWEEN_ULTRASOUND:
  255.                 pulse_end = time.time()
  256.  
  257.  
  258.             if pulse_start is None or pulse_end is None:
  259.                 self.val = -1
  260.                 continue
  261.             pulse_duration = pulse_end - pulse_start
  262.  
  263.             distance = pulse_duration * 17150
  264.  
  265.             self.val = distance - .5
  266.             time.sleep(0.1)
  267.            
  268.         self.terminated = True
  269.  
  270. ## actual start
  271. #constants
  272. wait_for_press = True
  273. POWER = 83
  274. calibrate = True
  275. # always run
  276.  
  277. motor1=None
  278.  
  279. def resetMotors(signalnum=None, handler=None):
  280.      global motor1
  281.      motor1.OUT_1.stop()
  282.      motor1.OUT_2.stop()
  283.      motor1.OUT_3.stop()
  284.      motor1.OUT_4.stop()
  285.      GPIO.cleanup()
  286.      exit(0)
  287.  
  288. def find_mode(data):
  289.     try:
  290.         if data is None or len(data) == 0:
  291.             return [0]
  292.        
  293.        
  294.         from collections import Counter
  295.         mo = Counter(data).most_common()  
  296.         if len(mo) == 0:
  297.             return data
  298.         return [mo]
  299.     except Exception as e:
  300.         print(e)
  301.  
  302. def remove_outliers(data, m=2):
  303.  
  304.     if len(data) == 0:
  305.         return []
  306.     return data[abs(data - np.mean(data)) < m * np.std(data)]
  307.  
  308. def go():
  309.     global motor1
  310.     sensor1=sensor2=sensor3=None
  311.     try:
  312.         GPIO.setmode(GPIO.BCM)
  313.         GPIO.setup(3, GPIO.IN)
  314.     if wait_for_press:
  315.            time_cnt = 0
  316.            
  317.            while True:
  318.               if time_cnt % 20 == 0:
  319.                  print("Waiting for button press %i"% int(time_cnt/20))
  320.               if not GPIO.input(3):
  321.                  break
  322.               time_cnt += 1
  323.               time.sleep(0.05)
  324.            print("Waiting 5 second then start!")        
  325.        
  326.  
  327.         distances_left = []
  328.         distances_mid = []
  329.         distances_right =[]
  330.         diffs_left = []
  331.         diffs_mid =[]
  332.         diffs_right = []
  333.     left_duty = 100
  334.     right_duty = 100
  335.         sensor1 = UltrasoundSensor(12,16).start()
  336.         sensor2 = UltrasoundSensor(12,20).start()
  337.         sensor3 = UltrasoundSensor(12,21).start()
  338.         motor1 = Motor()
  339.    
  340.         time.sleep(5)
  341.         # Called on process interruption. Set all pins to "Input" default mode.
  342.  
  343.        
  344.        # motor1.Go_2()
  345.         min_distance = 20
  346.         start = time.time()
  347.         print("MEH")
  348.         while time.time() - start < 40 and GPIO.input(3):
  349.                                
  350.             left_sensor = sensor1.sense()
  351.             mid_sensor = sensor2.sense()
  352.             right_sensor = sensor3.sense()
  353.  
  354.             distances_left.append(int(left_sensor - 1))
  355.             diffs_left.append(int(left_sensor - 1))
  356.             distances_mid.append(int(mid_sensor))
  357.             diffs_mid.append(int(mid_sensor))
  358.             distances_right.append(int(right_sensor - 5))
  359.             diffs_right.append(int(right_sensor -5 ))            
  360.             print("t0")
  361.             #distances_left = remove_outliers(distances_left)
  362.             print("t1")
  363.             #diffs_left = remove_outliers(diffs_left)
  364.             print("t2")
  365.             #distances_mid = remove_outliers(distances_mid)
  366.             print("t3")
  367.             #diffs_mid = remove_outliers(diffs_mid)
  368.             #distances_right = remove_outliers(distances_right)
  369.             #diffs_right = remove_outliers(diffs_right)
  370.  
  371.             if len(distances_left) > 10:
  372.                 distances_left.pop(0)
  373.             if len(distances_mid) > 10:
  374.                 distances_mid.pop(0)
  375.             if len(distances_right) > 10:
  376.                 distances_right.pop(0)
  377.                        
  378.  
  379.             if len(diffs_left) > 20:
  380.                 diffs_left.pop(0)
  381.             if len(diffs_mid) > 20:
  382.                 diffs_mid.pop(0)
  383.             if len(diffs_right) > 20:
  384.                 diffs_right.pop(0)
  385.          
  386.             # Predict the future O_O
  387.             if len(diffs_left) < 5 or len(diffs_right) < 5 or len(diffs_mid) < 5:
  388.                 left_diff = 0
  389.                 mid_diff = 0
  390.                 right_diff = 0    
  391.             else:
  392.                 left_fit = np.polyfit(np.arange(len(diffs_left)),diffs_left,1)
  393.                 left_fit_fn = np.poly1d(left_fit)  
  394.                 left_diff = left_fit_fn.c[0]
  395.                 mid_fit = np.polyfit(np.arange(len(diffs_mid)), diffs_mid, 1)
  396.                 mid_fit_fn = np.poly1d(mid_fit)
  397.                 mid_diff = mid_fit_fn.c[0]
  398.                 right_fit = np.polyfit(np.arange(len(diffs_right)), diffs_right, 1)
  399.                 right_fit_fn = np.poly1d(right_fit)
  400.                 right_diff = right_fit_fn.c[0]
  401.             #            
  402.             print("justbefore")
  403.             mo_left = find_mode(distances_left)
  404.             print("CAKE")
  405.             mo_mid = find_mode(distances_mid)
  406.             mo_right = find_mode(distances_right)
  407.            
  408.             distance_left = sorted(mo_left)[len(mo_left)//2] + left_diff
  409.             distance_mid = sorted(mo_mid)[len(mo_mid)//2] + mid_diff
  410.             distance_right = sorted(mo_right)[len(mo_right)//2] + right_diff
  411.            
  412.             print("Sensor 1: %dcm, Sensor 2: %dcm, Sensor 3: %dcm, Dist left: %dcm, Dist mid: %dcm, Dist right: %dcm, Grad left: %dcm, Grad mid: %dcm, Grad right: %dcm" % (left_sensor, mid_sensor, right_sensor, distance_left, distance_mid, distance_right, left_diff, mid_diff, right_diff))
  413.             print(sorted(distances_left))
  414.             distance_right = min(abs(distance_right), 100)
  415.             distance_mid = min(abs(distance_mid), 100)
  416.             distance_left = min(abs(distance_left), 100)
  417.             distance_diff = distance_right-distance_left
  418.             t_power  =POWER* math.e**(-(abs(distance_diff)-0.01)/((20-0.01)/(-math.log(0.03, math.e))))
  419.            
  420.             if abs(distance_diff) > 3:
  421.                if distance_diff >= 0:
  422.                   power_right = max(t_power, POWER*.3)
  423.                   power_left = max(POWER-t_power, POWER*.3)
  424.                else:
  425.                   power_left = max(t_power, POWER*.3)
  426.                   power_right = max(POWER-t_power, POWER*.3)
  427.             else:
  428.                 power_left = power_right = t_power      
  429.                    
  430.             print(str(power_left) + ":"+ str(power_right) + "---" + str(t_power))
  431.             if not calibrate:
  432.                 motor1.Go_1(duty=power_left)
  433.                 motor1.Go_2(duty=power_right)
  434.            
  435.             #if distance_left < min_distance:
  436.                 #left_duty = (left_duty -1) % 100
  437.             #right_duty = (right_duty + 1) % 100
  438.             #    motor1.Go_2(duty=POWER*0.7)
  439.             #    motor1.Go_1(duty=POWER)
  440.             #elif distance_right < min_distance:
  441.                 #right_duty = (right_duty - 1 ) % 100
  442.                 #left_duty = (left_duty + 1) % 100
  443.             #    motor1.Go_2(duty=POWER)
  444.             #    motor1.Go_1(duty=POWER*0.7)
  445.             #else:
  446.             #    motor1.Go_1(duty=POWER)
  447.             #    motor1.Go_2(duty=POWER)
  448.             #motor1.Go_1(duty=left_duty)
  449.             #motor1.Go_2(duty=right_duty)
  450.            
  451.             time.sleep(0.05)
  452.     except:
  453.         print(sys.exc_info()[0])    
  454.     finally:
  455.         print("FINALLY...")
  456.         resetMotors()
  457.         if sensor1 is not None:
  458.             sensor1.stop()
  459.         if sensor2 is not None:
  460.             sensor2.stop()
  461.         if sensor3 is not None:
  462.             sensor3.stop()
  463.         GPIO.cleanup()
  464.    
  465.  
  466.  
  467.  
  468. if __name__=="__main__":
  469.     signal.signal(signal.SIGTERM, resetMotors)
  470.     signal.signal(signal.SIGINT, resetMotors)
  471.     signal.signal(signal.SIGHUP, resetMotors)
  472.     signal.signal(signal.SIGQUIT, resetMotors)
  473.     try:
  474.         go()
  475.     finally:
  476.         resetMotors()
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!