daily pastebin goal
48%
SHARE
TWEET

Untitled

a guest Dec 7th, 2017 55 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
Top