API tools faq
paste
Advertisement
Robomatics

Laser Scanner R-Pi

Robomatics
Jul 14th, 2014
314
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 9.74 KB | None | 0 0
raw download clone embed print report
  1. #import pygame
  2. import gc
  3. import sys
  4. import select
  5. import tty
  6. import termios
  7. import serial
  8. import time
  9. import os
  10. import datetime
  11. from pygame.locals import *
  12.  
  13. #pygame.init()
  14. s = serial.Serial("/dev/ttyAMA0", baudrate=9600, timeout=0.1)
  15. s.open
  16. #screen = pygame.display.set_mode((640,480))
  17. #font = pygame.font.Font(None,36)
  18. resultstring = 'Init'
  19. rcv = 'init'
  20. enc1 = 0
  21. enc2 = 0
  22. encH = False
  23. encV = False
  24. Vlowlimit = 0
  25. Vhighlimit = 0
  26. Hlowlimit = 0
  27. Hhighlimit = 0
  28. VlowSet = False
  29. VhighSet = False
  30. HlowSet = False
  31. HhighSet = False
  32. LimitsSet = False
  33. prevupdate = 0
  34. pointstaken = [[False for i in range(4096)] for i in range(4096)]
  35. datetimez = str(datetime.datetime.now().strftime('%Y%m%d%H%M'))
  36. fileopened = False
  37. Hmotorstage = 0
  38. Vmotorstage = 0
  39. Motorbyte = 0
  40. Started = False
  41. Finished = False
  42. VerticalDirection = 0
  43.  
  44. def keycheck():
  45.     return select.select([sys.stdin],[],[],0)==([sys.stdin],[],[])
  46.  
  47. def motor(motorz, dir):
  48.     global Hmotorstage
  49.     global Vmotorstage
  50.     global s
  51.     global Motorbyte
  52.  
  53.     if motorz == 1:
  54.         if dir == 1:
  55.             if Hmotorstage == 0:
  56.                 Motorbyte = (Motorbyte & 15) + 80
  57.                 Hmotorstage = 1
  58.             elif Hmotorstage == 1:
  59.                 Motorbyte = (Motorbyte & 15) + 144
  60.                 Hmotorstage = 2
  61.             elif Hmotorstage == 2:
  62.                 Motorbyte = (Motorbyte & 15) + 208
  63.                 Hmotorstage = 3
  64.             elif Hmotorstage == 3:
  65.                 Motorbyte = (Motorbyte & 15) + 224
  66.                 Hmotorstage = 4
  67.             elif Hmotorstage == 4:
  68.                 Motorbyte = (Motorbyte & 15) + 240
  69.                 Hmotorstage = 5
  70.             elif Hmotorstage == 5:
  71.                 Motorbyte = (Motorbyte & 15) + 176
  72.                 Hmotorstage = 6
  73.             elif Hmotorstage == 6:
  74.                 Motorbyte = (Motorbyte & 15) + 112
  75.                 Hmotorstage = 7
  76.             elif Hmotorstage == 7:
  77.                 Motorbyte = (Motorbyte & 15) + 96
  78.                 Hmotorstage = 0
  79.         elif dir == 2:
  80.             if Hmotorstage == 0:
  81.                 Motorbyte = (Motorbyte & 15) + 112
  82.                 Hmotorstage = 7
  83.             elif Hmotorstage == 1:
  84.                 Motorbyte = (Motorbyte & 15) + 96
  85.                 Hmotorstage = 0
  86.             elif Hmotorstage == 2:
  87.                 Motorbyte = (Motorbyte & 15) + 80
  88.                 Hmotorstage = 1
  89.             elif Hmotorstage == 3:
  90.                 Motorbyte = (Motorbyte & 15) + 144
  91.                 Hmotorstage = 2
  92.             elif Hmotorstage == 4:
  93.                 Motorbyte = (Motorbyte & 15) + 208
  94.                 Hmotorstage = 3
  95.             elif Hmotorstage == 5:
  96.                 Motorbyte = (Motorbyte & 15) + 224
  97.                 Hmotorstage = 4
  98.             elif Hmotorstage == 6:
  99.                 Motorbyte = (Motorbyte & 15) + 240
  100.                 Hmotorstage = 5
  101.             elif Hmotorstage == 7:
  102.                 Motorbyte = (Motorbyte & 15) + 176
  103.                 Hmotorstage = 6
  104.         elif dir == 3:
  105.             Motorbyte = 170
  106.         s.write(chr(Motorbyte))
  107.         time.sleep(0.01)
  108.     if motorz == 2:
  109.         if dir == 1:
  110.             if Vmotorstage == 0:
  111.                 Motorbyte = (Motorbyte & 240) + 5
  112.                 Vmotorstage = 1
  113.             elif Vmotorstage == 1:
  114.                 Motorbyte = (Motorbyte & 240) + 9
  115.                 Vmotorstage = 2
  116.             elif Vmotorstage == 2:
  117.                 Motorbyte = (Motorbyte & 240) + 13
  118.                 Vmotorstage = 3
  119.             elif Vmotorstage == 3:
  120.                 Motorbyte = (Motorbyte & 240) + 14
  121.                 Vmotorstage = 4
  122.             elif Vmotorstage == 4:
  123.                 Motorbyte = (Motorbyte & 240) + 15
  124.                 Vmotorstage = 5
  125.             elif Vmotorstage == 5:
  126.                 Motorbyte = (Motorbyte & 240) + 11
  127.                 Vmotorstage = 6
  128.             elif Vmotorstage == 6:
  129.                 Motorbyte = (Motorbyte & 240) + 7
  130.                 Vmotorstage = 7
  131.             elif Vmotorstage == 7:
  132.                 Motorbyte = (Motorbyte & 240) + 6
  133.                 Vmotorstage = 0
  134.         elif dir == 2:
  135.             if Vmotorstage == 0:
  136.                 Motorbyte = (Motorbyte & 240) + 7
  137.                 Vmotorstage = 7
  138.             elif Vmotorstage == 1:
  139.                 Motorbyte = (Motorbyte & 240) + 6
  140.                 Vmotorstage = 0
  141.             elif Vmotorstage == 2:
  142.                 Motorbyte = (Motorbyte & 240) + 5
  143.                 Vmotorstage = 1
  144.             elif Vmotorstage == 3:
  145.                 Motorbyte = (Motorbyte & 240) + 9
  146.                 Vmotorstage = 2
  147.             elif Vmotorstage == 4:
  148.                 Motorbyte = (Motorbyte & 240) + 13
  149.                 Vmotorstage = 3
  150.             elif Vmotorstage == 5:
  151.                 Motorbyte = (Motorbyte & 240) + 14
  152.                 Vmotorstage = 4
  153.             elif Vmotorstage == 6:
  154.                 Motorbyte = (Motorbyte & 240) + 15
  155.                 Vmotorstage = 5
  156.             elif Vmotorstage == 7:
  157.                 Motorbyte = (Motorbyte & 240) + 11
  158.                 Vmotorstage = 6
  159.         elif dir == 3:
  160.             Motorbyte = 170
  161.         s.write(chr(Motorbyte))
  162.         time.sleep(0.01)
  163.  
  164.     return
  165.  
  166. def convert(rcvstring):
  167.     resultstring = ""
  168.     ok = True
  169.     for i in range(len(rcvstring)):
  170.         if i > 6 and i < 12:
  171.             if i == 9:
  172.                 resultstring = resultstring + '.'
  173.             if ord(rcvstring[i]) == 207:
  174.                 resultstring = resultstring + '0'
  175.             elif ord(rcvstring[i]) == 223:
  176.                 resultstring = resultstring + '0'
  177.             elif ord(rcvstring[i]) == 6:
  178.                 resultstring = resultstring + '1'
  179.             elif ord(rcvstring[i]) == 22:
  180.                 resultstring = resultstring + '1'
  181.             elif ord(rcvstring[i]) == 107:
  182.                 resultstring = resultstring + '2'
  183.             elif ord(rcvstring[i]) == 123:
  184.                 resultstring = resultstring + '2'
  185.             elif ord(rcvstring[i]) == 47:
  186.                 resultstring = resultstring + '3'
  187.             elif ord(rcvstring[i]) == 63:
  188.                 resultstring = resultstring + '3'
  189.             elif ord(rcvstring[i]) == 166:
  190.                 resultstring = resultstring + '4'
  191.             elif ord(rcvstring[i]) == 182:
  192.                 resultstring = resultstring + '4'
  193.             elif ord(rcvstring[i]) == 173:
  194.                 resultstring = resultstring + '5'
  195.             elif ord(rcvstring[i]) == 189:
  196.                 resultstring = resultstring + '5'
  197.             elif ord(rcvstring[i]) == 237:
  198.                 resultstring = resultstring + '6'
  199.             elif ord(rcvstring[i]) == 253:
  200.                 resultstring = resultstring + '6'
  201.             elif ord(rcvstring[i]) == 14:
  202.                 resultstring = resultstring + '7'
  203.             elif ord(rcvstring[i]) == 30:
  204.                 resultstring = resultstring + '7'
  205.             elif ord(rcvstring[i]) == 239:
  206.                 resultstring = resultstring + '8'
  207.             elif ord(rcvstring[i]) == 255:
  208.                 resultstring = resultstring + '8'
  209.             elif ord(rcvstring[i]) == 175:
  210.                 resultstring = resultstring + '9'
  211.             elif ord(rcvstring[i]) == 191:
  212.                 resultstring = resultstring + '9'
  213.             else:  
  214.                 if i <> 7:
  215.                     ok = False
  216.     return resultstring;
  217.  
  218. print 'initz'
  219. s.flushOutput()
  220. motor(1,3)
  221. motor(2,3)
  222. while 1:
  223.     try:
  224.         s.flushInput()
  225.         while 1:
  226.             rcv = s.read(1)
  227.             if ord(rcv[0]) == 128:
  228.                 rcv = s.read(15)
  229.                 break
  230.     except Exception, e1:
  231.         print str(e1)
  232.         s.close()
  233.         s.open()
  234.  
  235.     if len(rcv) > 11:
  236.         if ord(rcv[6]) == ord(rcv[13]) and ord(rcv[14]) == 128:
  237.             enc1 = (ord(rcv[1]) + (ord(rcv[2]) * 256)) % 4096
  238.             enc2 = 4096 - ((ord(rcv[3]) + (ord(rcv[4]) * 256)) % 4096)
  239.             if ord(rcv[5]) == 1 or ord(rcv[5]) == 3:
  240.                 encH = True
  241.             if ord(rcv[5]) >= 2 or ord(rcv[5]) == 3:
  242.                 encV = True
  243.  
  244.         if ord(rcv[6]) <> prevupdate and ord(rcv[6]) == ord(rcv[13]):
  245.             prevupdate = ord(rcv[6])
  246.             resultstring = ""
  247.             resultstring = convert(rcv)
  248.  
  249.     if encH == False or encV == False or LimitsSet == False:
  250.         os.system('clear')
  251.         print 'Laser:',resultstring
  252.         print 'Vertical:',enc2
  253.         print 'Horizontal:',enc1
  254.     if encH == False:
  255.         print 'Horizontal Encoder Not Referenced'
  256.     if encV == False:
  257.         print 'Vertical Encoder Not Referenced'
  258.     if LimitsSet == False:
  259.         print 'Limit Set, type ? to refresh current values'
  260.         if VlowSet == False:
  261.             rinput = raw_input("Vertical Low Limit:")
  262.             if rinput <> '?':
  263.                 Vlowlimit = int(rinput)
  264.                 VlowSet = True
  265.             if rinput == '?':
  266.                 continue
  267.         else:
  268.             print 'Vertical Low Limit:', Vlowlimit
  269.         if VhighSet == False:
  270.             rinput = raw_input("Vertical High Limit:")
  271.             if rinput <> '?':
  272.                 Vhighlimit = int(rinput)
  273.                 VhighSet = True
  274.             if rinput == '?':
  275.                 continue
  276.         else:
  277.             print 'Vertical High Limit:', Vhighlimit
  278.         if HlowSet == False:
  279.             rinput = raw_input("Horizontal Low Limit:")
  280.             if rinput <> '?':
  281.                 Hlowlimit = int(rinput)
  282.                 HlowSet = True
  283.             if rinput == '?':
  284.                 continue
  285.         else:
  286.             print 'Horizontal Low Limit:', Hlowlimit
  287.         if HhighSet == False:
  288.             rinput = raw_input("Horizontal High Limit:")
  289.             if rinput <> '?':
  290.                 Hhighlimit = int(rinput)
  291.                 HhighSet = True
  292.             if rinput == '?':
  293.                 continue
  294.         if VhighSet == True and VlowSet == True and HhighSet == True and HlowSet == True:
  295.             LimitsSet = True
  296.  
  297.     if encH == True and encV == True and resultstring <> '.' and resultstring <> '0.000' and resultstring <> 'init' and pointstaken[enc1][enc2] == False and LimitsSet == True:
  298.         if Started == False:
  299.             os.system('clear')
  300.             print 'Returning to Start'
  301.             print 'Horizontal Start', Hlowlimit
  302.             print 'Vertical Start', Vlowlimit
  303.             print 'Vertical:',enc2
  304.             print 'Horizontal:',enc1
  305.             if enc1 > Hlowlimit:
  306.                 motor(1,2)
  307.             if enc2 > Vlowlimit:
  308.                 motor(2,2)
  309.             if enc1 <= Hlowlimit and enc2 <= Vlowlimit:
  310.                 Started = True
  311.  
  312.         if fileopened == False and Started == True:
  313.             f = open(datetimez + '.bls','w')
  314.             print 'file opened'
  315.             old_settings=termios.tcgetattr(sys.stdin)
  316.             tty.setcbreak(sys.stdin.fileno())
  317.             fileopened = True
  318.         if Started == True:
  319.             pointstaken[enc1][enc2] = True
  320.             print enc1, enc2, resultstring
  321.             f.write(str(enc1) + ' ' + str(enc2) + ' ' + resultstring + '\n')
  322.  
  323.     if Started == True:
  324.         if VerticalDirection == 0:
  325.             if enc2 < Vhighlimit:
  326.                 motor(2,1)
  327.             else:
  328.                 VerticalDirection = 1
  329.                 motor(1,1)
  330.         elif VerticalDirection == 1:
  331.             if enc2 > Vlowlimit:
  332.                 motor(2,2)
  333.             else:
  334.                 VerticalDirection = 0
  335.                 motor(1,1)
  336.         if enc1 > Hhighlimit:
  337.             print 'Finished'
  338.             break
  339.     if keycheck():
  340.         c = sys.stdin.read(1)
  341.         if c == '\x1b':
  342.             break
  343.  
  344.  
  345. #   pygame.draw.rect(screen, (0,0,255),(0,0,640,480),0)
  346. #   text = font.render(str(enc1), 1, (255,255,255))
  347. #   screen.blit(text,(0,0))
  348. #   text = font.render(str(enc2), 1, (255,255,255))
  349. #   screen.blit(text,(0,40))
  350. #   text = font.render(resultstring, 1, (255,255,255))
  351. #   screen.blit(text,(0,80))
  352. #   if encH == False:
  353. #       text = font.render('Horizontal No Ref', 1, (255,255,255))
  354. #       screen.blit(text,(0,120))
  355. #   if encV == False:
  356. #       text = font.render('Vert No Ref', 1, (255,255,255))
  357. #       screen.blit(text,(0,160))
  358. #   pygame.display.flip()
  359. #   pygame.event.pump()
  360. #   keyz = pygame.key.get_pressed()
  361. #   if keyz[pygame.K_ESCAPE]:
  362. #       break
  363.     pass
  364. motor(1,3)
  365. motor(2,3)
  366. f.close()
  367. termios.tcsetattr(sys.stdin, termios.TCSADRAIN, old_settings)
  368. pass
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!