Advertisement
Guest User

Untitled

a guest
Dec 7th, 2017
85
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 5.17 KB | None | 0 0
  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
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement