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()