daily pastebin goal
60%
SHARE
TWEET

Untitled

a guest Dec 7th, 2017 57 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. from gopigo import *
  2. import time
  3. import pigpio
  4.  
  5. #This program scans a 180 degree map of its surroundings then turns towards its most distant measurement
  6.  
  7. #Methods to control the robot better
  8. #!/usr/bin/python
  9. '''
  10. This module contains convenience functions to simplify
  11. the coding of simple tasks.
  12. This really needs to be moved to a GoPiGo package
  13. e.g. from gopigo.control import *
  14. '''
  15.  
  16. en_debug=1
  17.  
  18. ## 360 rotation is ~64 encoder pulses or 5 deg/pulse
  19. ## DPR is the Deg:Pulse Ratio or the # of degrees per
  20. ##  encoder pulse.
  21. DPR = 360.0/64
  22. WHEEL_RAD = 3.25 # Wheels are ~6.5 cm diameter.
  23. CHASS_WID = 11 # Chassis is ~13.5 cm wide.
  24.  
  25. def left_deg(deg=None):
  26.     '''
  27.     Turn chassis left by a specified number of degrees.
  28.     DPR is the #deg/pulse (Deg:Pulse ratio)
  29.     This function sets the encoder to the correct number
  30.      of pulses and then invokes left().
  31.     '''
  32.     if deg is not None:
  33.         pulse= int(deg/DPR)
  34.         enc_tgt(0,1,pulse)
  35.     left()
  36.  
  37. def right_deg(deg=None):
  38.     '''
  39.     Turn chassis right by a specified number of degrees.
  40.     DPR is the #deg/pulse (Deg:Pulse ratio)
  41.     This function sets the encoder to the correct number
  42.      of pulses and then invokes right().
  43.     '''
  44.     if deg is not None:
  45.         pulse= int(deg/DPR)
  46.         enc_tgt(0,1,pulse)
  47.     right()
  48.  
  49. def fwd_cm(dist=None):
  50.     '''
  51.     Move chassis fwd by a specified number of cm.
  52.     This function sets the encoder to the correct number
  53.      of pulses and then invokes fwd().
  54.     '''
  55.     if dist is not None:
  56.         pulse = int(cm2pulse(dist))
  57.         enc_tgt(1,1,pulse)
  58.     fwd()
  59.  
  60. def bwd_cm(dist=None):
  61.     '''
  62.     Move chassis bwd by a specified number of cm.
  63.     This function sets the encoder to the correct number
  64.      of pulses and then invokes bwd().
  65.     '''
  66.     if dist is not None:
  67.         pulse = int(cm2pulse(dist))
  68.         enc_tgt(1,1,pulse)
  69.     bwd()
  70.  
  71. def cm2pulse(dist):
  72.     '''
  73.     Calculate the number of pulses to move the chassis dist cm.
  74.     pulses = dist * [pulses/revolution]/[dist/revolution]
  75.     '''
  76.     wheel_circ = 2*math.pi*WHEEL_RAD # [cm/rev] cm traveled per revolution of wheel
  77.     revs = dist/wheel_circ
  78.     PPR = 18 # [p/rev] encoder Pulses Per wheel Revolution
  79.     pulses = PPR*revs # [p] encoder pulses required to move dist cm.
  80.     if en_debug:
  81.         print 'WHEEL_RAD',WHEEL_RAD
  82.         print 'revs',revs
  83.         print 'pulses',pulses
  84.     return pulses
  85. def int sleep = .5
  86. def int servo_angle = 36
  87. def center_angle(servo_angle):
  88.     a = int(180/servo_angle)
  89.     return a
  90.  
  91.    
  92. def scanArea():
  93.     distance = [0] * int(180/servo_angle)
  94.     for i in range(0, 5):
  95.         servo(i*servo_angle) #Moves the servo at the angles 0, 10 up to 180
  96.         time.sleep(sleep) #Gives time for the sensor to be in position before start measuring
  97.         distance[i] = us_dist(15)
  98.         time.sleep(sleep) #So that the servo doesn't spas out
  99.     return distance
  100.  
  101. def determineAngle(distance):
  102.     m = max(distance) #finds max measured distance in the array
  103.     angles = [i for i, j in enumerate(distance) if j == m]
  104.     #might return two angles if the values are the same
  105.     #returns indexes of largest values
  106.     #indexes are used to determine the angles
  107.     #enumerate creates a list [(index, value) etc]
  108.     #the for loop loops through the list
  109.     #the first i is the index of what it puts in the new list
  110.     #the second i and the j get increased until the if statement is met
  111.     #once it is met the index second i gets put in the first i
  112.     #I don't fully understand it but this is a really smart way to do it
  113.     return max(angles) #always picks the lowest value because it corresponds to the rightmost value
  114.  
  115.  
  116. while True:
  117.  
  118.     distance = scanArea()
  119.     print(distance)
  120.     angle = determineAngle(distance) #Angles: 0 to 8 is right, 9 is front, 10 to 18 is left
  121.     print(angle)
  122.     center_angle = center_angle(servo_angle)
  123.     #Example of angles:
  124.    
  125.     #Right 0 1 2 3 4 5 6 7 8 9* 10 11 12 13 14 15 16 17 18  
  126.     #angle value times servo_angle represents the angle to go to
  127.  
  128.     rotation = 90-(angle*servo_angle) #Right will be a positive value, left will be negative
  129.  
  130.     if center_angle % 2 == 0: #even amount of mesurements will not have a forward mesurement
  131.  
  132.         if angle < center_angle:
  133.             right_deg(rotation) #Rotate right by that angle
  134.             time.sleep(1)
  135.             fwd_cm(distance[angle]-1) #Go forward one less
  136.         elif angle > center_angle:
  137.             left_deg(-rotation) #Rotate left by that
  138.             time.sleep(1)
  139.             fwd_cm(distance[angle]-1)
  140.        
  141.     else: #odd amount of measurements will have a forward measurement
  142.    
  143.         if angle < center_angle:
  144.             right_deg(rotation) #Rotate right by that angle
  145.             time.sleep(1)
  146.             fwd_cm(distance[angle]-1) #Go forward one less
  147.         elif angle > center_angle:
  148.             left_deg(-rotation) #Rotate left by that
  149.             time.sleep(1)
  150.             fwd_cm(distance[angle]-1)
  151.         elif angle == center_angle:
  152.             fwd_cm(distance[angle]-1)
  153.             time.sleep(.2)
  154.    
  155.     time.sleep(3) #Waits for the robot to finish going forward before taking the next measurement
RAW Paste Data
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
 
Top