-
Notifications
You must be signed in to change notification settings - Fork 6
/
SunfounderLidar_rover.py
182 lines (161 loc) · 8.18 KB
/
SunfounderLidar_rover.py
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
#!/usr/bin/env python3
import sys
import time
from rplidar import RPLidar
import math
import Adafruit_PCA9685
method = 2 # which analytical technique should we use? 1 = average, 2 = longest free range, 3 = histogram
# Initialise the PCA9685 servo driver board using the default address (0x40).
pwm = Adafruit_PCA9685.PCA9685()
angle_offset = 0 # this compensates for the Lidar being placed in a rotated position
gain = 1.5 # this is the steering gain. The PWM output to the steering servo must be between 0 (left) and 500 (right)
speed = 2000 # crusing speed, must be between 0 and 3600
steering_correction = 60 # this compensates for any steering bias the car has. Positive numbers steer to the right
start = time.time()
stop = False
left_motor = 4 # which PWM output this is attached to
right_motor = 5
steer_servo = 0
speed_adj = 2 # gain for slowing down in turns
PORT_NAME = '/dev/ttyUSB0' # this is for the Lidar
def constrain(val, min_val, max_val):
if val < min_val: return min_val
if val > max_val: return max_val
return val
def drive(speed): # speed must be between 0 and 3600
servo (left_motor,speed)
servo (right_motor,speed)
def servo (channel, PWM):
pulse = PWM + 300 # make sure pulse width at least 500
pwm.set_pwm(channel,0, pulse) # channel, start of wave, end of wave
def steer(angle):
angle = 250 + steering_correction + gain*angle
angle = int(constrain(angle,0,600))
print ("PWM output: ", angle)
servo (steer_servo,angle)
new_speed = speed - (speed_adj*abs(angle-100))
new_speed = constrain(new_speed, 100, 5000)
# drive (new_speed) # slow down in turns
drive (speed) # don't slow down in turns
def range_average():
global range_sum
if (measurment[3] < 1000 and measurment[3] > 100): # in distance range
if (measurment[2] < 45):
temp = measurment[2]
else:
temp = -1* (360-measurment[2]) # convert to negative angle to the left of center
data = data + temp # sum of the detected angles, so we can average later
# range_sum = range_sum + measurment[3] # sum all the distances so we can normalize later
counter = counter + 1 # increment counter
if time.time() > (lasttime + 0.2): # do this five times a second
if counter > 0: # this means we see something
average_angle = (data/counter) - angle_offset # average of detected angles
print ("Average angle: ", average_angle)
obstacle_direction = int(100*math.atan(math.radians(average_angle))) # convert to a vector component
drive_direction = -1 * obstacle_direction # steer in the opposite direction as obstacle (I'll replace this with a PID)
# print ("Drive direction: ", drive_direction)
counter = 0 # reset counter
data = 0 # reset data
range_sum = 0
def scan(lidar):
global stop
global range_sum
# longest[0] = (0,0) # initialize a variable to keep the furthest-away 100 readings (angle, distance)
drive_direction = 0
furthest = 1000
time1 = time.time()
while True:
counter = 0
print('Recording measurements... Press Crl+C to stop.')
data = 0
range_sum = 0
lasttime = time.time()
for measurment in lidar.iter_measurments():
if stop == True:
lidar.stop()
lidar.stop_motor()
drive(0)
lidar.disconnect()
break
if (measurment[2] < 120 or measurment[2] > 240): # in angular range; 0 degrees is straight ahead
if method == 1: # this is the averaging method, best for large areas
if (measurment[3] < 1000 and measurment[3] > 100): # in distance range
# print (measurment[2])
if (measurment[2] < 45):
temp = measurment[2]
else:
temp = -1* (360-measurment[2]) # convert to negative angle to the left of center
data = data + temp # sum of the detected angles, so we can average later
# range_sum = range_sum + measurment[3] # sum all the distances so we can normalize later
counter = counter + 1 # increment counter
if time.time() > (lasttime + 0.2): # do this five times a second
if counter > 0: # this means we see something
average_angle = (data/counter) - angle_offset # average of detected angles
print ("Average angle: ", average_angle)
obstacle_direction = int(100*math.atan(math.radians(average_angle))) # convert to a vector component
drive_direction = -1 * obstacle_direction # steer in the opposite direction as obstacle (I'll replace this with a PID)
# print ("Drive direction: ", drive_direction)
counter = 0 # reset counter
data = 0 # reset data
range_sum = 0
else:
drive_direction = 0
steer(drive_direction) # Send data to motors
lasttime = time.time() # reset 10Hz timer
if method == 2: # this is the "steer towards furthest empty area" approach, best for tight tracks
if measurment[3] > 1000:
if measurment[3] > furthest:
furthest = measurment[3]
angle = measurment[2]
if angle > 240:
angle = -1 * (360 - angle)
drive_direction = int(100*math.atan(math.radians(angle))) # convert to a vector component, and drive in the direction of free space
print ('New furthest: ', furthest, "Angle:", angle, "Drive direction: ", drive_direction )
if time.time() > (lasttime + 0.2): # do this five times a second
print ("Selected furthest angle: ", drive_direction)
furthest = 1000 # reset this
steer(drive_direction) # Send data to motors
lasttime = time.time() # reset 10Hz timer
if method == 3: # this is the histogram approach, best for tracks with two walls
i = 0
while i <= 120:
if measurment[2] >= i and measurment[2] < i+20:
range_sum = 0
range_average()
i = i+20
i = 220
while i <= 360:
if measurment[2] >= i and measurment[2] < i+20:
i = i+20
if measurment[3] > 1000:
if measurment[3] > furthest:
furthest = measurment[3]
angle = measurment[2]
if angle > 240:
angle = -1 * (360 - angle)
drive_direction = int(100*math.atan(math.radians(angle))) # convert to a vector component, and drive in the direction of free space
print ('New furthest: ', furthest, "Angle:", angle, "Drive direction: ", drive_direction )
if time.time() > (lasttime + 0.2): # do this five times a second
print ("Selected furthest angle: ", drive_direction)
furthest = 1000 # reset this
steer(drive_direction) # Send data to motors
lasttime = time.time() # reset 10Hz timer
def run():
'''Main function'''
lidar = RPLidar(PORT_NAME)
lidar.start_motor()
servo (steer_servo,150) # center servo
time.sleep(1)
info = lidar.get_info()
print(info)
try:
scan(lidar)
except KeyboardInterrupt:
stop = True
print('Stopping.')
lidar.stop()
lidar.stop_motor()
drive (0)
lidar.disconnect()
if __name__ == '__main__':
run()