API tools faq
paste
Advertisement
Guest User

maze.py

a guest
Oct 14th, 2019
82
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
Python 16.93 KB | None | 0 0
raw download clone embed print report
  1. #!/usr/bin/env python3
  2. from eye import *
  3. from math import atan2, cos
  4. from numpy import abs
  5. from draw_map import *
  6. #include "eyebot.h"
  7.  
  8. #include <math.h>
  9. #include <stdio.h>
  10. #include <unistd.h>
  11. #include <stdlib.h>
  12.  
  13. #define DIST     360
  14. #define SPEED    180
  15. #define ASPEED    45
  16. #define THRES    175
  17. #define MAZESIZE  16
  18.  
  19. DIST = 360
  20. SPEED = 180
  21. ASPEED = 30
  22. THRES = 175
  23. MAZESIZE = 16
  24.  
  25. ''' pos.0,0 is bottom left, dir. 0 is facing up (north) '''
  26. mark = [([0]*MAZESIZE) for i in range(MAZESIZE)]   # 1 if visited
  27. #int wall[MAZESIZE+1][MAZESIZE+1][2]  ''' 1 if wall, 0 if free, -1 if unknown '''
  28. wall = [0]*(MAZESIZE+1)
  29. for i in range(len(wall)):
  30.     wall[i] = [0]*(MAZESIZE+1)
  31. for i in range(MAZESIZE+1):
  32.     for j in range(MAZESIZE+1):
  33.         wall[i][j] = [0]*2
  34. '''  BOTTOM: wall[x][y][0]  LEFT: wall[x][y][1] '''
  35. map = [([0]*MAZESIZE) for i in range(MAZESIZE)]     #distance to goal
  36. nmap = [([0]*MAZESIZE) for i in range(MAZESIZE)]    #copy
  37. path = [0]*MAZESIZE*MAZESIZE    #shortest path
  38.  
  39. GRAPH=1
  40. DEBUG=0
  41. DEBUG2=0
  42.  
  43. '''* print mark full to X window (sim only).
  44.    print positions that robot has already visited.  '''
  45. def print_mark_W():
  46.     ''' print to window (sim. only) '''
  47.     print("MARK\n")
  48.     for i in range(MAZESIZE-1,-1,-1):
  49.         for j in range(0,MAZESIZE):
  50.           if (mark[i][j]): print("x ")
  51.           else: print(". ")
  52.         print("\n")
  53.     print("\n")
  54.  
  55. '''* print mark on LCD (6 lines).
  56.    print positions that robot has already visited.  '''
  57. def print_mark():
  58.     LCDSetPos(1,0)
  59.     for i in range(5,-1,-1):
  60.         for j in range(0,14):
  61.             if (mark[i][j]): LCDPrintf("x")
  62.             else:LCDPrintf(".")
  63.         if (i>0):
  64.             LCDPrintf("\n")
  65.  
  66. '''* print ful maze in X window (sim only).
  67.    print maze as explored by robot.  '''
  68. def print_maze_W():
  69.     print("MAZE\n")
  70.     for i in range(MAZESIZE,-1,-1):
  71.         for j in range(0,MAZESIZE+1):
  72.             if (wall[i][j][1]==1): print("|")  #left
  73.             elif (wall[i][j][1]==0): print(" ")
  74.             else: print(".")
  75.             if (wall[i][j][0]==1): print("_")  # bottom
  76.             elif (wall[i][j][0]==0): print(" ")
  77.             else: print(".")
  78.         print("\n")
  79.     print("\n")
  80.  
  81. '''* print ful maze on LCD (6 lines).
  82.    print maze as explored by robot.  '''
  83. def print_maze():
  84.     LCDSetPos(1,0)
  85.     for i in range(5,-1,-1):
  86.         for j in range(0,7):
  87.             if (wall[i][j][1]==1): LCDPrintf("|")  #left
  88.             elif (wall[i][j][1]==0): LCDPrintf(" ")
  89.             else: LCDPrintf(".")
  90.             if (wall[i][j][0]==1): LCDPrintf("_")  #bottom
  91.             elif (wall[i][j][0]==0): LCDPrintf(" ")
  92.             else: LCDPrintf(".")
  93.         if i>0: LCDPrintf("\n")
  94.  
  95. def wall_set(wall,x,y,z, v):
  96.     if wall[x][y][z] == -1:
  97.         wall[x][y][z] = v  # not seen before, set value
  98.     elif wall[x][y][z] != v:     #seen before and CONTRADITION
  99.         wall[x][y][z] = 1        #assume wall to be safe
  100.         LCDSetPrintf(0,0,"CONTRADICTION\n") #AUBeep()
  101.  
  102. '''* maze_entry.
  103.    enter recognized walls or doors.  '''
  104. def maze_entry(x, y, dir, open):
  105.     if dir == 0:
  106.         wall_set(wall, y+1, x, 0, int(not open)) # top = bottom of next
  107.     elif dir == 2:
  108.         wall_set(wall, y, x, 0, int(not open))
  109.     elif dir == 1:
  110.         wall_set(wall, y, x, 1, int(not open))
  111.     elif dir == 3:
  112.         wall_set(wall, y, x+1, 1, int(not open)) #right = left of next
  113.  
  114. '''* robo position and orientation.
  115.   dir = 0, 1, 2, 3 equals: north, west, south, east.  '''
  116. rob_x = 0
  117. rob_y = 0
  118. rob_dir = 0
  119.  
  120. '''* init_maze.
  121.    inits internal map of maze.
  122.    set marks to 0 and walls to -1 (unknown).  '''
  123. def init_maze():
  124.     for i in range(MAZESIZE):
  125.         for j in range(MAZESIZE):
  126.             mark[i][j] = 0
  127.     for i in range(MAZESIZE+1):
  128.         for j in range(MAZESIZE+1):
  129.             wall[i][j][0] = -1
  130.             wall[i][j][1] = -1
  131.  
  132. def xneighbor(x, dir):
  133.     if dir == 0: return x #north
  134.     elif dir == 1: return x-1 #west
  135.     elif dir == 2: return x #south
  136.     elif dir == 3: return x+1 # east
  137.  
  138. def yneighbor(y, dir):
  139.     if dir == 0: return y+1 # north
  140.     elif dir == 1: return y # west
  141.     elif dir == 2: return y-1 # south
  142.     elif dir == 3: return y # east
  143.  
  144.  
  145. def unmarked(y, x, dir):
  146.     dir = int((dir+4) % 4)
  147.     return not mark [yneighbor(y,dir)] [xneighbor(x,dir)]
  148.  
  149.  
  150. def correct_angle_walk():
  151.  
  152.     l = PSDGet(PSD_LEFT)
  153.     r = PSDGet(PSD_RIGHT)
  154.     angle = 0
  155.     if abs(l-r) < 175:
  156.         LCDSetPrintf(13,0, "l,r = %d %d   ", l, r)
  157.         angle = 180/3.1415 * atan2(l-r,int(DIST))
  158.         if angle > 0:
  159.             angle = min(20, angle)
  160.         else:
  161.             angle = max(-20, angle)
  162.         if abs(angle) > 7:
  163.             LCDSetPrintf(14,0, "angle = %f   ",angle)
  164.             VWTurn(int(2*angle), ASPEED)  # turn
  165.             VWWait()
  166.         else:
  167.             angle = 0
  168.  
  169.     else:
  170.         if abs( l - DIST/2 ) > 70 and l < 300:
  171.             LCDSetPrintf(13,0, "l,r = %d %d   ", l, r)
  172.             angle = 180/3.1415 * atan2(l - DIST/2 ,int(DIST))
  173.             if angle > 0:
  174.                 angle = min(14, angle)
  175.             else:
  176.                 angle = max(-14, angle)
  177.             if abs(angle) > 7:
  178.                 LCDSetPrintf(14,0, "angle LEFT = %f   ",angle)
  179.                 VWTurn(int(2*angle), ASPEED)  # turn
  180.                 VWWait()
  181.             else:
  182.                 angle = 0
  183.  
  184.         else:
  185.             if abs( r - DIST/2 ) > 70 and r < 300:
  186.                 LCDSetPrintf(13,0, "l,r = %d %d   ", l, r)
  187.                 angle = - 180/3.1415 * atan2(r - DIST/2 ,int(DIST))
  188.                 if angle > 0:
  189.                     angle = min(14, angle)
  190.                 else:
  191.                     angle = max(-14, angle)
  192.                 if abs(angle) > 7:
  193.                     LCDSetPrintf(14,0, "angle RIGHT = %f   ",angle)
  194.                     VWTurn(int(2*angle), ASPEED)  # turn
  195.                     VWWait()
  196.                 else:
  197.                     angle = 0
  198.  
  199.     corr_dist = int((DIST/3) * (2 - cos(angle * 3.1415 / 180)))
  200.     LCDSetPrintf(15,0, "dist = %d   ",corr_dist)
  201.     VWStraight(corr_dist, SPEED)    # go one step
  202.     VWWait()
  203.     VWTurn(int(-angle), ASPEED)  # turn
  204.     VWWait()
  205.  
  206. '''* go_to.
  207.  walk one square in current direction '''
  208. def go_to(dir):
  209.     global rob_x, rob_y, rob_dir
  210.     dir = (dir+4) % 4  # keep goal dir in 0..3
  211.     turn = dir - rob_dir
  212.     if turn == 3:
  213.         turn = -1  # turn shorter angle
  214.     elif turn == -3:
  215.         turn =  1
  216.  
  217.     if turn:
  218.         if DEBUG:
  219.             LCDSetPrintf(13,0, "Turn %d %d   ", turn*90, ASPEED)
  220.         if turn != -1:
  221.             VWTurn(turn*90, ASPEED)  # turn
  222.             VWWait()
  223.         else:
  224.             VWTurn(turn*85, ASPEED)
  225.             VWWait()
  226.  
  227.  
  228.     #                       #
  229.     #                       #
  230.     #                       #
  231.     #                       #
  232.     #########################
  233.     ### CORRECTED VERSION ###
  234.     #########################
  235.     #                       #
  236.     #                       #
  237.     #                       #
  238.     #                       #
  239.  
  240.     # Forward 1/3, no way to know if the angle is correct
  241.     # l = PSDGet(PSD_LEFT)
  242.     # r = PSDGet(PSD_RIGHT)
  243.     # LCDSetPrintf(13,0, "l,r = %d %d   ", l, r)
  244.     # VWStraight(int(DIST/3), SPEED)    # go one step
  245.     # VWWait()
  246.  
  247.     # Correct on 2/3 and 3/3
  248.     correct_angle_walk()
  249.     correct_angle_walk()
  250.     correct_angle_walk()
  251.     global i
  252.     i += 1
  253.     LCDSetPrintf(17,0, "i = %d   ", i)
  254.     if i < 5:
  255.         robmap.draw()
  256.     if i  == 5:
  257.         robmap.map.end()
  258.  
  259.  
  260.  
  261.     #                       #
  262.     #                       #
  263.     #                       #
  264.     #                       #
  265.     #########################
  266.     ### CORRECTED VERSION ###
  267.     #########################
  268.     #                       #
  269.     #                       #
  270.     #                       #
  271.     #                       #
  272.  
  273.  
  274.     cur_x, cur_y, cur_p = VWGetPosition()
  275.     if DEBUG:
  276.         LCDSetPrintf(14,0, "X %d Y %d Phi %d   ", cur_x, cur_y, cur_p)
  277.  
  278.     rob_dir = dir
  279.     rob_x   = xneighbor(rob_x,rob_dir)
  280.     rob_y   = yneighbor(rob_y,rob_dir)
  281.  
  282.  
  283.  
  284. '''* check_mark.
  285.    if ALL walls of a square are known, mark square as visited.
  286.    this avoids unnecessary exploration.  '''
  287. def check_mark():
  288.     for i in range(1,MAZESIZE):
  289.         for j in range(0,MAZESIZE):
  290.             ''' careful: watch boundaries!! i from 1 / j until size-1 '''
  291.             if wall[i  ][j][0] != -1 and wall[i][j  ][1] != -1 \
  292.             and wall[i+1][j][0] != -1 and wall[i][j+1][1] != -1: #bottom / left, top / right
  293.                 mark[i][j] = 1
  294.  
  295.  
  296.  
  297. '''*  explore.
  298.    search maze goal from given start position and orientation.
  299.    if more than one possible way: search all recursively.
  300.    mark all visited maze squares.  '''
  301. def explore():
  302.     global rob_x, rob_y, rob_dir
  303.     mark[rob_y][rob_x] = 1   # mark current square
  304.     left_open  = int(PSDGet(PSD_LEFT) > THRES)
  305.     front_open = int(PSDGet(PSD_FRONT) > THRES)
  306.     right_open = int(PSDGet(PSD_RIGHT) > THRES)
  307.     maze_entry(rob_x,rob_y,rob_dir,       front_open)
  308.     maze_entry(rob_x,rob_y,(rob_dir+1)%4, left_open)
  309.     maze_entry(rob_x,rob_y,(rob_dir+3)%4, right_open)
  310.     check_mark()
  311.     old_dir = rob_dir
  312.  
  313.     if GRAPH:
  314.         LCDSetPos(0,0)
  315.         LCDPrintf("Pos[%2d,%2d,%1d]", rob_x,rob_y,rob_dir)
  316.         if left_open: LCDSetPrintf(0,13,"<")
  317.         else: LCDSetPrintf(0,13,"|")
  318.         if front_open: LCDSetPrintf(0,14,"^")
  319.         else: LCDSetPrintf(0,14,"-")
  320.         if right_open: LCDSetPrintf(0,15,">")
  321.         else: LCDSetPrintf(0,15,"|")
  322.         print_maze()
  323.  
  324.  
  325.     if DEBUG:
  326.         print_mark_W()
  327.         print_maze_W()
  328.         LCDMenu("Next"," "," "," ")
  329.         KEYWait(KEY1)
  330.         LCDMenu(" "," "," "," ")
  331.  
  332.     if front_open and unmarked(rob_y,rob_x,old_dir):   # then go straight
  333.         go_to(old_dir)   # go 1 forward, 0 if first choice
  334.         explore()        # recursive call
  335.         go_to(old_dir+2) # go 1 back
  336.  
  337.     if left_open and unmarked(rob_y,rob_x,old_dir+1):  # then turn left
  338.         go_to(old_dir+1) # go 1 left
  339.         explore()        # recursive call
  340.         go_to(old_dir-1) # go 1 right, -1 = +3
  341.  
  342.     if right_open and unmarked(rob_y,rob_x,old_dir-1): # then turn right
  343.         go_to(old_dir-1) # go 1 right, -1 = +3
  344.         explore()        # recursive call
  345.         go_to(old_dir+1) # go 1 left
  346.  
  347.  
  348. '''* print shortest distances from start in X window (sim only).  '''
  349. def print_map_W():
  350.     print("MAP\n")
  351.     for i in range(MAZESIZE-1,-1,-1):
  352.         for j in range(MAZESIZE):
  353.             print("%3d",map[i][j])
  354.         print("\n")
  355.     print("\n")
  356.  
  357.  
  358. '''* print shortest distances from start on LCD (6 lines).  '''
  359. def print_map():
  360.     LCDClear()
  361.     LCDPrintf("Map distances\n")
  362.     for i in range(5,-1,-1):
  363.         for j in range(4):
  364.           LCDPrintf("%3d",map[i][j])
  365.         if i>0: LCDPrintf("\n")
  366.  
  367.  
  368. '''* shortest_path.
  369.    analyze shortest path after maze has been searched.
  370.    returns path length to goal.
  371.    or -1 if no path could be found.  '''
  372. def shortest_path(goal_y, goal_x):
  373.     LCDSetPrintf(0,0, "                ") #clear top line
  374.     LCDSetPrintf(0,0, "Map..")
  375.     for i in range(MAZESIZE):
  376.         for j in range(MAZESIZE):
  377.             map [i][j] = -1  #init
  378.             nmap[i][j] = -1
  379.  
  380.     map [0][0] = 0
  381.     nmap[0][0] = 0
  382.     iter=0
  383.  
  384.     while True:
  385.         iter += 1
  386.         for i in range(MAZESIZE):
  387.             for j in range(MAZESIZE):
  388.                 if map[i][j] == -1:
  389.                     if i>0:
  390.                         if not wall[i][j][0] and map[i-1][j] != -1:
  391.                             nmap[i][j] = map[i-1][j] + 1
  392.                     if i<MAZESIZE-1:
  393.                         if not wall[i+1][j][0] and map[i+1][j] != -1:
  394.                             nmap[i][j] = map[i+1][j] + 1
  395.                     if j>0:
  396.                         if not wall[i][j][1] and map[i][j-1] != -1:
  397.                             nmap[i][j] = map[i][j-1] + 1
  398.                     if j<MAZESIZE-1:
  399.                         if not wall[i][j+1][1] and map[i][j+1] != -1:
  400.                             nmap[i][j] = map[i][j+1] + 1
  401.  
  402.         for i in range(MAZESIZE):
  403.             for j in range(MAZESIZE):
  404.                 map[i][j] = nmap[i][j]  # copy back
  405.  
  406.         if DEBUG2:
  407.             print_map()
  408.             print_map_W()
  409.             LCDMenu("Next"," "," "," ")
  410.             KEYWait(KEY1)
  411.             LCDMenu(" "," "," "," ")
  412.         if map[goal_y][goal_x] != -1 or iter >= (MAZESIZE*MAZESIZE): break
  413.  
  414.     LCDPrintf("done\n")
  415.     return map[goal_y][goal_x]
  416.  
  417.  
  418.  
  419. '''* build path.
  420.  build shortest path after finding it.
  421.  uses map and wall.
  422.  sets path.  '''
  423. def build_path(i, j, len):
  424.     LCDSetPrintf(0,0, "                ") # clear top line
  425.     LCDSetPrintf(0,0, "Path..")
  426.  
  427.     if i<=5 and j<=6:
  428.         LCDSetPrintf(6-i,2*j+1, "G") # mark goal
  429.     for k in range(len-1,-1,-1):
  430.         if i>0 and not wall[i][j][0] and map[i-1][j] == k:
  431.             i -= 1
  432.             path[k] = 0 # north
  433.         elif i<MAZESIZE-1 and not wall[i+1][j][0] and map[i+1][j] == k:
  434.             i += 1
  435.             path[k] = 2 # south
  436.  
  437.         elif j>0 and not wall[i][j][1] and map[i][j-1] == k:
  438.             j -= 1
  439.             path[k] = 3 # east
  440.  
  441.         elif j<MAZESIZE-1 and not wall[i][j+1][1] and map[i][j+1] == k:
  442.             j += 1
  443.             path[k] = 1 # west
  444.         else:
  445.             LCDPrintf("ERROR") #AUBeep()
  446.             KEYWait(ANYKEY)
  447.  
  448.         ''' mark path in maze on LCD '''
  449.         if i<=5 and j<=6:
  450.             if k>0:
  451.                 LCDSetPrintf(6-i,2*j+1, "*") # path
  452.             else:
  453.                 LCDSetPrintf(6-i,2*j+1, "S") # start
  454.         if DEBUG2:
  455.             print("path %3d:%1d\n", k, path[k])
  456.  
  457.     LCDSetPrintf(0,6, "done")
  458.  
  459.  
  460.  
  461. '''* drive path.
  462.  drive path after building it.
  463.  parametere specifies start to finish (0).
  464.  or finish to start (1).  '''
  465. def drive_path(len, reverse):
  466.     global rob_x, rob_y, rob_dir
  467.     if reverse:
  468.         for i in range(len-1,-1,-1):
  469.             go_to(path[i]+2)
  470.  
  471.         if rob_dir != 0: # back in start field
  472.             VWTurn(-rob_dir*90, ASPEED)  # turn
  473.             LCDSetPrintf(19,0, "Turn %d    ", rob_dir, ASPEED)
  474.             VWWait()
  475.             rob_dir = 0
  476.     else:
  477.         for i in range(len):
  478.             go_to(path[i])
  479.  
  480.  
  481.  
  482. '''* main program.
  483.    search maze from start in (0,0).
  484.    search whole maze and generate map.
  485.    @AUTHOR    Thomas Braunl, UWA, 1998.-- Updated 2017  '''
  486. def main():
  487.     global map
  488.     global robmap, i
  489.     global rob_x, rob_y, rob_dir
  490.     map = Map("Maze")
  491.     i = 0
  492.     robmap = Robo(map)
  493.     LCDPrintf("MAZE\nfull search\n\n")
  494.     init_maze()
  495.  
  496.     LCDMenu("GO","ROT","DEBUG","END")
  497.     key = KEYGet()
  498.     DEBUG = int(key == KEY3)
  499.     if key == KEY2:
  500.         VWTurn(90, 90)
  501.         VWWait()
  502.     if key == KEY4:
  503.         return 0
  504.  
  505.     rob_x = 0
  506.     rob_y = 0
  507.     rob_dir = 0  # start in pos. 0,0, facing north
  508.     explore()
  509.  
  510.     ''' back in [0,0] turn robot to original direction '''
  511.     if rob_dir != 0:
  512.         VWTurn( -rob_dir*90, ASPEED)  # turn
  513.         VWWait()
  514.         rob_dir = 0
  515.  
  516.     while True:
  517.         print_maze()
  518.         LCDMenu("Y+-","X+-","+/-","GO")
  519.         incr = 1
  520.         goalX=0
  521.         goalY=0
  522.         while True:
  523.             LCDSetPos(0,0)
  524.             LCDPrintf("GOAL y,x: %2d %2d", goalY, goalX) #AUBeep()
  525.             if goalY<=5 and goalX <=6:
  526.                 LCDSetPos(6-goalY,2*goalX+1)
  527.             key = KEYGet()
  528.             if key == KEY1:
  529.                 goalY = (goalY+incr+MAZESIZE) % MAZESIZE
  530.                 break
  531.             elif key == KEY2:
  532.                 goalX = (goalX+incr+MAZESIZE) % MAZESIZE
  533.                 break
  534.             elif key == KEY3:
  535.                 incr = -incr
  536.                 break
  537.             elif key == KEY4:
  538.                 break
  539.  
  540.         LCDMenu(" "," "," "," ")
  541.         path_len = shortest_path(goalY,goalX)  # from start 0,0 to goal
  542.         if path_len != -1: # if path exists
  543.             print_map_W()
  544.             LCDSetPos(0,0)
  545.             LCDPrintf("Path length: %3d", path_len)
  546.             build_path(goalY,goalX,path_len)
  547.  
  548.             while True:  # optional data display
  549.                 LCDMenu("Map","Mrk","Maz","DRV")
  550.                 key = KEYGet()
  551.                 if key == KEY1:
  552.                     print_map()
  553.                     break
  554.                 elif key == KEY2:
  555.                     print_mark()
  556.                     break
  557.                 elif key == KEY3:
  558.                     print_maze()
  559.                     break
  560.                 elif key == KEY4:
  561.                     break
  562.             drive_path(path_len,0) # drive start to finish
  563.             drive_path(path_len,1) # drive finish to start
  564.         else:
  565.             LCDSetPos(0,0)
  566.             LCDPrintf("No path exists!") #AUBeep()
  567.  
  568.         LCDMenu("REP"," "," ","END")
  569.         if KEYGet() == KEY4: break
  570.     return 0
  571.  
  572.  
  573. if __name__ == "__main__":
  574.     main()
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!