def scan(self):
pointCloud = []
lidar = RPLidar(self.portname)
for i, measurement in enumerate(lidar.iter_measures()):
for j, v in enumerate(measurement):
if j == 0:
newscan = v
if j == 1:
quality = v
if j == 2:
angle = v
if j == 3:
length = v
# change angle to match orientation of vehicle
angle = -1 * (angle - 90)
pointCloud.append([angle, length])
if i > 360:
break
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
return pointCloud
def run(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement= 18000
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
stop = False
scale = False
lidarToBack = 33
TopTri = 340 - 20
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 149 or degrees >= 211):
loop = True
if (degrees >= 330 or degrees <= 30) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if distance >= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = (ultimateMeasurement/25.4) - lidarToBack
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if degrees == 0:
Median = distance
if degrees == 340:
FirstSide = distance
if degrees == 20:
SecondSide = distance
DistanceMeasurements = [FirstSide, Median, SecondSide]
def detect(lidar_ranges, port):
lidar = RPLidar(port)
lidar.start_motor()
sleep(1)
COUNT_LIMIT = 10
dispatcher = FilterDispatcher(lidar_ranges, COUNT_LIMIT)
try:
for _, _, angle, distance in lidar.iter_measures(scan_type='normal',
max_buf_meas=4500):
process_data(dispatcher, distance, angle)
except RPLidarException as e:
print(e)
finally:
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
def measure():
lidar = RPLidar(PORT_NAME)
path = sys.argv[2] if len(
sys.argv) >= 3 else 'lidar_measurements_' + time_stamp + '.log'
outfile = open(path, 'w')
try:
print('Recording measurments... Press Ctrl+C to stop.')
for measurment in lidar.iter_measures():
line = '\t'.join(str(v) for v in measurment)
outfile.write(line + '\n')
print(line)
except KeyboardInterrupt:
print('Stopping.')
lidar.stop()
lidar.stop_motor()
lidar.disconnect()
outfile.close()
return
def LidarMoveAway360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
stop = False
turnLeft = False
turnRight = False
lidarToBack = 23
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
ultimateMeasurement = 10909090909
baseMeasurement = 1837.5
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 50 and degrees >= 1):
loop = True
if (degrees >= 150 and degrees <= 210) and distance != 0:
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
Inches = ultimateMeasurement / 25.4 - lidarToBack
if loop == True:
loop = False
print "[inches %f]", (Inches)
print "Degrees", (lidarDegrees)
ultimateMeasurement = baseMeasurement
if Inches <= 48:
if lidarDegrees > 180 and lidarDegrees <= 210:
turnRight = True
turnLeft = False
print "Turning Right"
sd.putBoolean("turnRight", turnRight)
sd.putBoolean("turnLeft", turnLeft)
elif lidarDegrees < 180 and lidarDegrees >= 150:
turnLeft = True
turnRight = False
print "Turning Left"
sd.putBoolean("turnLeft", turnLeft)
sd.putBoolean("turnRight", turnRight)
else:
print "Driving Sraight"
turnLeft = False
turnRight = False
sd.putBoolean("turnLeft", turnLeft)
sd.putBoolean("turnRight", turnRight)
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
def LidarMoveAway360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
stop = False
turnLeft = False
turnRight = False
lidarToBack = 33
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
ultimateMeasurement = 10909090909
baseMeasurement = 1837.5
position = 0
p1dist = 0
p1deg = 0
p2dist = 0
p2deg = 0
deltadist = 0
deltadeg = 0
moving = "center"
FirstDeg = 0
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 50 and degrees >= 1):
loop = True
if (degrees >= 90 and degrees <= 270) and distance != 0:
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
p1deg = degrees
print("HERE", p1deg)
if loop == True:
loop = False
Inches = ultimateMeasurement / 25.4 - lidarToBack
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
FirstDeg = p1deg
p1deg = 0
print("NOW", FirstDeg)
if Inches <= 48:
if deltadeg > 2:
moving = "left"
if deltadeg < -1:
moving = "right"
print("deltadeg", deltadeg)
if moving == "right":
#position = 2
#turnRight = True
#print "Turning Right"
#sd.putBoolean("turnRight", turnRight)
#sd.putNumber("position", position)
turnLeft = True
print "Turning Left"
sd.putBoolean("turnLeft", turnLeft)
if moving == "left":
# position = 1
turnRight = True
print "Turning Right"
sd.putBoolean("turnRight", turnRight)
sd.putNumber("position", position)
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
def run(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement = 18000
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
stop = False
scale = False
lidarToBack = 33
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 149 or degrees >= 211):
loop = True
if (degrees >= 150 and degrees <= 210) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = (ultimateMeasurement / 25.4) - lidarToBack
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if Inches is None:
continue
#note: in all situations below, left should be listed above right
##if Inches <= 25:
##stop = True
##33sd.putBoolean("Stop", stop)
##sd.putBoolean
#print("Boolean Stop is True")
if Inches <= 75:
scale = True
sd.putBoolean("Scale", scale)
print("Scale Is Good! :) ")
else:
stop = False
scale = False
#sd.putBoolean("Stop", stop)
#print("Boolean Stop is False")
sd.putNumber("Scale", scale)
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
import pygame
from sys import *
from math import sin,cos,pi
from rplidar import RPLidar
lidar=RPLidar("/dev/ttyUSB0")
pygame.init()
screen=pygame.display.set_mode((500,500))
def degrees(a):
return a*pi/180
a=0
rots=0
lastrot=0
try:
for measurement in lidar.iter_measures(max_buf_meas=1000):
scale=0.1
# print(lidar.iter_measures()[-1][2])
# last
measurment=list(measurement)
a=max(a,degrees(measurment[2]))
if measurment[2]-lastrot<0 and measurment[3]!=0:
if int(rots%1)==0:
pygame.display.flip()
screen.fill((0,0,0))
# lidar.clean_input()
pygame.draw.circle(screen,(255,0,0),(250,250),3)
#continue
# print(rots)
rots+=1
# print(rots)
# raise KeyboardInterrupt
# exit(0)
def LidarAlign360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 27463478276547
baseMeasurement = 2540
ultimateMeasurement = 6400
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 185 and degrees >= 175):
loop = True
if (degrees >= 0 or degrees <= 360) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if distance <= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = ultimateMeasurement / 25.4
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if Inches is None:
continue
#note: in all situations below, left should be listed above right
if Inches <= 100:
robotSpeed = (0.00005018 * (Inches * Inches)) + 0.1
elif Inches > 100:
robotSpeed = 0.5
print("robotSpeed", robotSpeed)
if lidarDegrees > 270 and lidarDegrees < 355 and Inches > 12:
left = robotSpeed
right = robotSpeed + 0.1
elif lidarDegrees > 5 and lidarDegrees < 90 and Inches > 12:
left = robotSpeed + 0.1
right = robotSpeed
elif lidarDegrees >= 90 and lidarDegrees < 175 and Inches > 12:
left = -1 * (robotSpeed + 0.1)
right = robotSpeed
elif lidarDegrees > 185 and lidarDegrees < 270 and Inches > 12:
left = robotSpeed
right = -1 * (robotSpeed + .1)
elif lidarDegrees >= 175 and lidarDegrees <= 185 and Inches >= 36:
left = -1 * robotSpeed
right = -1 * robotSpeed
print("back", robotSpeed)
elif lidarDegrees >= 355 and lidarDegrees <= 5 and Inches >= 12:
left = robotSpeed
right = robotSpeed
elif Inches <= 6 and lidarDegrees >= 270 and lidarDegrees <= 90:
print 'stopped'
right = 0
left = 0
elif Inches <= 36 and lidarDegrees >= 90 and lidarDegrees <= 270:
print 'stopped'
right = 0
left = 0
else:
left = 0.5
right = 0.5
sd.putNumber("left", left)
sd.putNumber("right", right)
print("lidarDegrees", lidarDegrees)
print("right", right)
print("left", left)
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
def LidarAlign360(path):
'''Main function'''
loop = False
Inches = None
lidarDegrees = 0
reset = 10
baseMeasurement= 3000
ultimateMeasurement = 10
PORT_NAME = '/dev/ttyUSB0'
lidar = RPLidar(PORT_NAME)
outfile = open(path, 'w')
ZeroBlocked = False
NinetyBlocked = False
OneEightyBlocked = False
TwoSeventyBlocked = False
try:
print('Recording measures... Press Crl+C to stop.')
for measurment in lidar.iter_measures():
distance = measurment[3]
degrees = measurment[2]
if (degrees <= 185 and degrees >= 175):
loop = True
if (degrees >= 0 or degrees <= 360) and distance != 0:
#print '[ultimateMeasurement %f]' % (ultimateMeasurement)
#print '[degrees %f]' % (degrees)
#print '[distance %f]' % (distance)
#if distance < baseMeasurement:
#ultimateMeasurement = distance
if(degrees >= 358 and degrees <= 2):
if distance >= ultimateMeasurement:
ultimateMeasurement = distance
lidarDegrees = degrees
if loop == True:
loop = False
Inches = ultimateMeasurement/25.4
print "[inches %f]", (Inches)
ultimateMeasurement = baseMeasurement
if (degrees >= 358 or degrees <= 2) and Inches <= 10:
ZeroBlocked = True;
elif (degrees >= 88 and degrees <= 92) and Inches <= 10:
NinetyBlocked = True;
elif (degrees >= 178 and degrees <= 182) and Inches <= 10:
OneEightyBlocked = True
elif (degrees >= 268 and degrees <= 272) and Inches < = 10:
TwoSeventyBlocked = True
else:
ZeroBlocked = False
NinetyBlocked = False
OneEightyBlocked = False
TwoSeventyBlocked = False
if ZeroBlocked == False and NinetyBlocked == True and TwoSeventyBlocked == True:
sd.putDouble("MazeSpeed", 0.5)
if OneEightyBlocked = False and ZeroBlocked == True and NinetyBlocked = True and TwoSeventyBlocked = True:
sd.putDouble("MazeSpeed", -0.5)
if
sd.putNumber("left", left)
sd.putNumber("right", right)
print ("lidarDegrees", lidarDegrees)
print ("right", right)
print ("left", left)
#print'[robotSpeed = %f]' % (robotSpeed)
#sd.putNumber('ultimateMeasurement', ultimateMeasurement)
#if loop == True:
#ultimateMeasurement = baseMeasurement
except KeyboardInterrupt:
print('Stopping.')
except:
traceback.print_exc()
lidar.stop_motor()
lidar.stop()
lidar.disconnect()
outfile.close()
