• API
• FAQ
• Tools
• Archive
daily pastebin goal
11%
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:
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.

Top