Gal Yaroslavsky / MicroMouse-v1

asdf

Dependencies:   L3GD20 LSM303DLHC mbed

Headers/Flooding.h@2:997f57aee3b7, 2014-04-03 (annotated)

Committer:
goy5022
Date:
Thu Apr 03 13:15:40 2014 +0000
Revision:
2:997f57aee3b7
Child:
7:95ebadc83fc7
Gal last touched this inappropriately

Who changed what in which revision?

UserRevisionLine numberNew contents of line
goy5022 2:997f57aee3b7 1 #ifndef FLOODING_H
goy5022 2:997f57aee3b7 2 #define FLOODING_H
goy5022 2:997f57aee3b7 3
goy5022 2:997f57aee3b7 4
goy5022 2:997f57aee3b7 5 #include "Mapping.h"
goy5022 2:997f57aee3b7 6
goy5022 2:997f57aee3b7 7 enum MOVE
goy5022 2:997f57aee3b7 8 {
goy5022 2:997f57aee3b7 9 M_NORTH,
goy5022 2:997f57aee3b7 10 M_SOUTH,
goy5022 2:997f57aee3b7 11 M_EAST,
goy5022 2:997f57aee3b7 12 M_WEST,
goy5022 2:997f57aee3b7 13 M_INVALID
goy5022 2:997f57aee3b7 14 };
goy5022 2:997f57aee3b7 15
goy5022 2:997f57aee3b7 16 unsigned char destX = 8;
goy5022 2:997f57aee3b7 17 unsigned char destY = 8;
goy5022 2:997f57aee3b7 18 unsigned char pathX = 8;
goy5022 2:997f57aee3b7 19 unsigned char pathY = 8;
goy5022 2:997f57aee3b7 20
goy5022 2:997f57aee3b7 21 unsigned char flood[16][16] =
goy5022 2:997f57aee3b7 22 {
goy5022 2:997f57aee3b7 23 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 24 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 25 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 26 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 27 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 28 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 29 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 30 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 31 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 32 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 33 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 34 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 35 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 36 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 37 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00},
goy5022 2:997f57aee3b7 38 {0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}
goy5022 2:997f57aee3b7 39 };
goy5022 2:997f57aee3b7 40
goy5022 2:997f57aee3b7 41 void clearFlood()
goy5022 2:997f57aee3b7 42 {
goy5022 2:997f57aee3b7 43 for(unsigned char i = 0; i < (unsigned char)16; i++)
goy5022 2:997f57aee3b7 44 for(unsigned char j = 0; j < (unsigned char)16; j++)
goy5022 2:997f57aee3b7 45 flood[i][j] = 0x00;
goy5022 2:997f57aee3b7 46 }
goy5022 2:997f57aee3b7 47
goy5022 2:997f57aee3b7 48 bool flood_getTop(int i, int j)
goy5022 2:997f57aee3b7 49 {
goy5022 2:997f57aee3b7 50 if(validDimen(i, j) && flood[i][j] == 0)
goy5022 2:997f57aee3b7 51 return !((MAP[i + 1][j] & TOP) == TOP || ((MAP[i][j] & BOTTOM) == BOTTOM));
goy5022 2:997f57aee3b7 52
goy5022 2:997f57aee3b7 53 return false;
goy5022 2:997f57aee3b7 54 }
goy5022 2:997f57aee3b7 55
goy5022 2:997f57aee3b7 56 bool flood_getRight(int i, int j)
goy5022 2:997f57aee3b7 57 {
goy5022 2:997f57aee3b7 58 if(validDimen(i, j) && flood[i][j] == 0)
goy5022 2:997f57aee3b7 59 return !((MAP[i][j - 1] & RIGHT) == RIGHT || ((MAP[i][j] & LEFT) == LEFT));
goy5022 2:997f57aee3b7 60
goy5022 2:997f57aee3b7 61 return false;
goy5022 2:997f57aee3b7 62 }
goy5022 2:997f57aee3b7 63
goy5022 2:997f57aee3b7 64 bool flood_getLeft(int i, int j)
goy5022 2:997f57aee3b7 65 {
goy5022 2:997f57aee3b7 66 if(validDimen(i, j) && flood[i][j] == 0)
goy5022 2:997f57aee3b7 67 return !((MAP[i][j + 1] & LEFT) == LEFT || ((MAP[i][j] & RIGHT) == RIGHT));
goy5022 2:997f57aee3b7 68
goy5022 2:997f57aee3b7 69 return false;
goy5022 2:997f57aee3b7 70 }
goy5022 2:997f57aee3b7 71
goy5022 2:997f57aee3b7 72 bool flood_getBottom(int i, int j)
goy5022 2:997f57aee3b7 73 {
goy5022 2:997f57aee3b7 74 if(validDimen(i, j) && flood[i][j] == 0)
goy5022 2:997f57aee3b7 75 return !((MAP[i - 1][j] & BOTTOM) == BOTTOM || ((MAP[i][j] & TOP) == TOP));
goy5022 2:997f57aee3b7 76
goy5022 2:997f57aee3b7 77 return false;
goy5022 2:997f57aee3b7 78 }
goy5022 2:997f57aee3b7 79
goy5022 2:997f57aee3b7 80 int flood_getCell(int i, int j)
goy5022 2:997f57aee3b7 81 {
goy5022 2:997f57aee3b7 82 if(validDimen(i, j))
goy5022 2:997f57aee3b7 83 return flood[i][j];
goy5022 2:997f57aee3b7 84
goy5022 2:997f57aee3b7 85 return 0;
goy5022 2:997f57aee3b7 86 }
goy5022 2:997f57aee3b7 87
goy5022 2:997f57aee3b7 88 void flood_floodStep(int step)
goy5022 2:997f57aee3b7 89 {
goy5022 2:997f57aee3b7 90 for(int i = 4; i >= 0 ; i--)
goy5022 2:997f57aee3b7 91 {
goy5022 2:997f57aee3b7 92 for(int j = 0; j < 5; j++)
goy5022 2:997f57aee3b7 93 {
goy5022 2:997f57aee3b7 94 if(flood[i][j] == step)
goy5022 2:997f57aee3b7 95 {
goy5022 2:997f57aee3b7 96 if(flood_getTop(i - 1, j))
goy5022 2:997f57aee3b7 97 {
goy5022 2:997f57aee3b7 98 flood[i - 1][j] = (char) (step + 1);
goy5022 2:997f57aee3b7 99 }
goy5022 2:997f57aee3b7 100
goy5022 2:997f57aee3b7 101 if(flood_getBottom(i + 1, j))
goy5022 2:997f57aee3b7 102 {
goy5022 2:997f57aee3b7 103 flood[i + 1][j] = (char) (step + 1);
goy5022 2:997f57aee3b7 104 }
goy5022 2:997f57aee3b7 105
goy5022 2:997f57aee3b7 106 if(flood_getLeft(i, j - 1))
goy5022 2:997f57aee3b7 107 {
goy5022 2:997f57aee3b7 108 flood[i][j - 1] = (char) (step + 1);
goy5022 2:997f57aee3b7 109 }
goy5022 2:997f57aee3b7 110 if(flood_getRight(i, j + 1))
goy5022 2:997f57aee3b7 111 {
goy5022 2:997f57aee3b7 112 flood[i][j + 1] = (char) (step + 1);
goy5022 2:997f57aee3b7 113 }
goy5022 2:997f57aee3b7 114 }
goy5022 2:997f57aee3b7 115 }
goy5022 2:997f57aee3b7 116 }
goy5022 2:997f57aee3b7 117 }
goy5022 2:997f57aee3b7 118
goy5022 2:997f57aee3b7 119 MOVE flood_bestMove(int x, int y)
goy5022 2:997f57aee3b7 120 {
goy5022 2:997f57aee3b7 121 int target = flood_getCell(x, y) - 1;
goy5022 2:997f57aee3b7 122
goy5022 2:997f57aee3b7 123 if(flood_getCell(x + 1, y) == target && ((MAP[x + 1][y] & TOP) != TOP))
goy5022 2:997f57aee3b7 124 {
goy5022 2:997f57aee3b7 125 pathX++;
goy5022 2:997f57aee3b7 126 return M_NORTH;
goy5022 2:997f57aee3b7 127 }
goy5022 2:997f57aee3b7 128 if(flood_getCell(x, y - 1) == target && ((MAP[x][y - 1] & RIGHT) != RIGHT))
goy5022 2:997f57aee3b7 129 {
goy5022 2:997f57aee3b7 130 pathY--;
goy5022 2:997f57aee3b7 131 return M_EAST;
goy5022 2:997f57aee3b7 132 }
goy5022 2:997f57aee3b7 133
goy5022 2:997f57aee3b7 134 if(flood_getCell(x, y + 1) == target && ((MAP[x][y + 1] & LEFT) != LEFT))
goy5022 2:997f57aee3b7 135 {
goy5022 2:997f57aee3b7 136 pathY++;
goy5022 2:997f57aee3b7 137 return M_WEST;
goy5022 2:997f57aee3b7 138 }
goy5022 2:997f57aee3b7 139 if(flood_getCell(x - 1, y) == target && ((MAP[x - 1][y] & BOTTOM) != BOTTOM))
goy5022 2:997f57aee3b7 140 {
goy5022 2:997f57aee3b7 141 pathX--;
goy5022 2:997f57aee3b7 142 return M_SOUTH;
goy5022 2:997f57aee3b7 143 }
goy5022 2:997f57aee3b7 144
goy5022 2:997f57aee3b7 145 return M_INVALID;
goy5022 2:997f57aee3b7 146 }
goy5022 2:997f57aee3b7 147
goy5022 2:997f57aee3b7 148 MOVE flood_findPath(int xt, int yt)
goy5022 2:997f57aee3b7 149 {
goy5022 2:997f57aee3b7 150 flood[xt][yt] = 1;
goy5022 2:997f57aee3b7 151
goy5022 2:997f57aee3b7 152 int step = 1;
goy5022 2:997f57aee3b7 153 while(flood[destX][destY] == 0)
goy5022 2:997f57aee3b7 154 {
goy5022 2:997f57aee3b7 155 flood_floodStep(step++);
goy5022 2:997f57aee3b7 156 }
goy5022 2:997f57aee3b7 157
goy5022 2:997f57aee3b7 158 MOVE last = M_INVALID;
goy5022 2:997f57aee3b7 159
goy5022 2:997f57aee3b7 160 while (flood_getCell(pathX, pathY) != 1)
goy5022 2:997f57aee3b7 161 {
goy5022 2:997f57aee3b7 162 last = flood_bestMove(pathX, pathY);
goy5022 2:997f57aee3b7 163 }
goy5022 2:997f57aee3b7 164
goy5022 2:997f57aee3b7 165 return last;
goy5022 2:997f57aee3b7 166 }
goy5022 2:997f57aee3b7 167
goy5022 2:997f57aee3b7 168 #endif