Philippe ROCHETTE / LabyLib

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

LabyLib.h@0:810679c5659e, 2018-12-29 (annotated)

Committer:
garphil
Date:
Sat Dec 29 11:37:34 2018 +0000
Revision:
0:810679c5659e
To create, follow and escape from a virtual Laby. To have a digital twin of the bot

Who changed what in which revision?

UserRevisionLine numberNew contents of line
garphil 0:810679c5659e 1 #ifdef _LABYLIB_H_
garphil 0:810679c5659e 2 #else
garphil 0:810679c5659e 3 #define _LABYLIB_H_
garphil 0:810679c5659e 4 #define LABY_X 16
garphil 0:810679c5659e 5 #define LABY_Y 16
garphil 0:810679c5659e 6 #define LABY_UP 8
garphil 0:810679c5659e 7 #define LABY_DOWN 2
garphil 0:810679c5659e 8 #define LABY_LEFT 4
garphil 0:810679c5659e 9 #define LABY_RIGHT 6
garphil 0:810679c5659e 10
garphil 0:810679c5659e 11
garphil 0:810679c5659e 12 #define CAP(x,m,M) (x=max(m,min(x,M)))
garphil 0:810679c5659e 13 extern Serial pc_uart;
garphil 0:810679c5659e 14 extern Serial bt_uart;
garphil 0:810679c5659e 15
garphil 0:810679c5659e 16 class LabyLib {
garphil 0:810679c5659e 17 bool labyrinthe[LABY_X][LABY_Y]; // false = wall, true=empty
garphil 0:810679c5659e 18 char path[LABY_X][LABY_Y]; // false = wall, true=empty
garphil 0:810679c5659e 19 char position_x, position_y;
garphil 0:810679c5659e 20 char init_x, init_y;
garphil 0:810679c5659e 21 char size_x, size_y;
garphil 0:810679c5659e 22 char direction, init_direction; // 2=down,4=left,8=up,6=right
garphil 0:810679c5659e 23 char fill_line;
garphil 0:810679c5659e 24
garphil 0:810679c5659e 25 int max(int a, int b) {return a>b?a:b;}
garphil 0:810679c5659e 26 int min(int a, int b) {return a<b?a:b;}
garphil 0:810679c5659e 27
garphil 0:810679c5659e 28 public:
garphil 0:810679c5659e 29 LabyLib(char _size_x, char _size_y, char _position_x, char _position_y, char _direction) {
garphil 0:810679c5659e 30 size_x = _size_x+1;
garphil 0:810679c5659e 31 size_y = _size_y+1;
garphil 0:810679c5659e 32 position_x = _position_x;
garphil 0:810679c5659e 33 position_y = _position_y;
garphil 0:810679c5659e 34 init_direction=direction = _direction;
garphil 0:810679c5659e 35 fill_line = 1;
garphil 0:810679c5659e 36 CAP(size_x,0,LABY_X);
garphil 0:810679c5659e 37 CAP(size_y,0,LABY_Y);
garphil 0:810679c5659e 38 init_x=CAP(position_x,1,_size_x);
garphil 0:810679c5659e 39 init_y=CAP(position_y,1,_size_y);
garphil 0:810679c5659e 40 // pc_uart.printf("LABY init %d x %d, pos = %d,%d, direction = %d\n\r",size_x,size_y,position_x,position_y, direction);
garphil 0:810679c5659e 41
garphil 0:810679c5659e 42 if(direction!=LABY_UP && direction!=LABY_DOWN && direction!=LABY_LEFT && direction!=LABY_RIGHT) direction=LABY_UP;
garphil 0:810679c5659e 43
garphil 0:810679c5659e 44 int i,j;
garphil 0:810679c5659e 45 for(i=0;i<LABY_X;i++) {
garphil 0:810679c5659e 46 labyrinthe[i][0]=false;
garphil 0:810679c5659e 47 labyrinthe[i][size_x]=false;
garphil 0:810679c5659e 48 }
garphil 0:810679c5659e 49
garphil 0:810679c5659e 50 for(j=0;j<LABY_Y;j++) {
garphil 0:810679c5659e 51 labyrinthe[0][j]=false;
garphil 0:810679c5659e 52 labyrinthe[size_y][j]=false;
garphil 0:810679c5659e 53 }
garphil 0:810679c5659e 54
garphil 0:810679c5659e 55 for(i=1;i<size_x;i++)
garphil 0:810679c5659e 56 for(j=1;j<size_y;j++) {
garphil 0:810679c5659e 57 labyrinthe[i][j]=true;
garphil 0:810679c5659e 58 }
garphil 0:810679c5659e 59
garphil 0:810679c5659e 60 }
garphil 0:810679c5659e 61
garphil 0:810679c5659e 62 void FillLaby(unsigned int Line) { // Fill as binary 0x101001 means | | | 1 being wall
garphil 0:810679c5659e 63 if(fill_line>=size_y) return;
garphil 0:810679c5659e 64 unsigned int caseX = 1<<((size_x-2)*4);
garphil 0:810679c5659e 65 for(int i=1;i<size_x;i++) {
garphil 0:810679c5659e 66 labyrinthe[i][fill_line] = !(Line & caseX);
garphil 0:810679c5659e 67 caseX >>= 4;
garphil 0:810679c5659e 68 }
garphil 0:810679c5659e 69 fill_line++;
garphil 0:810679c5659e 70
garphil 0:810679c5659e 71 }
garphil 0:810679c5659e 72
garphil 0:810679c5659e 73 void printLaby() {
garphil 0:810679c5659e 74 pc_uart.printf("LABY %d x %d, pos = %d,%d, direction = %d\n\r",size_x,size_y,position_x,position_y, direction);
garphil 0:810679c5659e 75 for(int j=1;j<size_y;j++) {
garphil 0:810679c5659e 76 char l=0;
garphil 0:810679c5659e 77 for(int i=1;i<size_x;i++) {
garphil 0:810679c5659e 78 if(i==position_x && j==position_y) {
garphil 0:810679c5659e 79 switch(direction) {
garphil 0:810679c5659e 80 case LABY_UP:l='^';break;
garphil 0:810679c5659e 81 case LABY_DOWN:l='v';break;
garphil 0:810679c5659e 82 case LABY_RIGHT:l='>';break;
garphil 0:810679c5659e 83 case LABY_LEFT:l='<';break;
garphil 0:810679c5659e 84 default:l='/';break;
garphil 0:810679c5659e 85 }
garphil 0:810679c5659e 86 } else {
garphil 0:810679c5659e 87 if(labyrinthe[i][j]) l='.';
garphil 0:810679c5659e 88 else l='x';
garphil 0:810679c5659e 89 }
garphil 0:810679c5659e 90 pc_uart.putc(l);
garphil 0:810679c5659e 91 bt_uart.putc(l);
garphil 0:810679c5659e 92 }
garphil 0:810679c5659e 93 pc_uart.printf("----\n\r");
garphil 0:810679c5659e 94 bt_uart.printf("----\n\r");
garphil 0:810679c5659e 95 }
garphil 0:810679c5659e 96 }
garphil 0:810679c5659e 97 bool atStart() {
garphil 0:810679c5659e 98 if(position_x==init_x && position_y==init_y) return true;
garphil 0:810679c5659e 99 return false;
garphil 0:810679c5659e 100 }
garphil 0:810679c5659e 101
garphil 0:810679c5659e 102 void setCell(int x, int y, bool wall) { // 0 for cell, 1 for wall
garphil 0:810679c5659e 103 if(x<0 || x>=size_x) return;
garphil 0:810679c5659e 104 if(y<0 || y>=size_y) return;
garphil 0:810679c5659e 105 labyrinthe[x][y]=!wall;
garphil 0:810679c5659e 106 }
garphil 0:810679c5659e 107
garphil 0:810679c5659e 108 bool goUP() {
garphil 0:810679c5659e 109 bool value = labyrinthe[position_x][position_y-1];
garphil 0:810679c5659e 110 if(value) position_y--;
garphil 0:810679c5659e 111 return value;
garphil 0:810679c5659e 112 }
garphil 0:810679c5659e 113
garphil 0:810679c5659e 114 bool goDOWN() {
garphil 0:810679c5659e 115 bool value = labyrinthe[position_x][position_y+1];
garphil 0:810679c5659e 116 if(value) position_y++;
garphil 0:810679c5659e 117 return value;
garphil 0:810679c5659e 118 }
garphil 0:810679c5659e 119
garphil 0:810679c5659e 120 bool goLEFT() {
garphil 0:810679c5659e 121 bool value = labyrinthe[position_x-1][position_y];
garphil 0:810679c5659e 122 if(value) position_x--;
garphil 0:810679c5659e 123 return value;
garphil 0:810679c5659e 124 }
garphil 0:810679c5659e 125
garphil 0:810679c5659e 126 bool goRIGHT() {
garphil 0:810679c5659e 127 bool value = labyrinthe[position_x+1][position_y];
garphil 0:810679c5659e 128 if(value) position_x++;
garphil 0:810679c5659e 129 return value;
garphil 0:810679c5659e 130 }
garphil 0:810679c5659e 131
garphil 0:810679c5659e 132 bool turnLEFT() {
garphil 0:810679c5659e 133 static char leftT[4] = {6,2,8,4};
garphil 0:810679c5659e 134 direction = leftT[direction/2-1];
garphil 0:810679c5659e 135 return true;
garphil 0:810679c5659e 136 }
garphil 0:810679c5659e 137
garphil 0:810679c5659e 138 bool turnRIGHT() {
garphil 0:810679c5659e 139 static char leftT[4]={4,8,2,6};
garphil 0:810679c5659e 140 direction = leftT[direction/2-1];
garphil 0:810679c5659e 141 return true;
garphil 0:810679c5659e 142 }
garphil 0:810679c5659e 143
garphil 0:810679c5659e 144 bool turnBACK() {
garphil 0:810679c5659e 145 static char leftT[4]={8,6,4,2};
garphil 0:810679c5659e 146 direction = leftT[direction/2-1];
garphil 0:810679c5659e 147 return true;
garphil 0:810679c5659e 148 }
garphil 0:810679c5659e 149
garphil 0:810679c5659e 150 bool goForward() {
garphil 0:810679c5659e 151 switch(direction) {
garphil 0:810679c5659e 152 case LABY_UP:return goUP();
garphil 0:810679c5659e 153 case LABY_DOWN:return goDOWN();
garphil 0:810679c5659e 154 case LABY_RIGHT:return goRIGHT();
garphil 0:810679c5659e 155 case LABY_LEFT:return goLEFT();
garphil 0:810679c5659e 156 default:return goUP();
garphil 0:810679c5659e 157 }
garphil 0:810679c5659e 158 }
garphil 0:810679c5659e 159
garphil 0:810679c5659e 160 bool goBackward() {
garphil 0:810679c5659e 161 switch(direction) {
garphil 0:810679c5659e 162 case LABY_UP:return goDOWN();
garphil 0:810679c5659e 163 case LABY_DOWN:return goUP();
garphil 0:810679c5659e 164 case LABY_RIGHT:return goLEFT();
garphil 0:810679c5659e 165 case LABY_LEFT:return goRIGHT();
garphil 0:810679c5659e 166 default:return goUP();
garphil 0:810679c5659e 167 }
garphil 0:810679c5659e 168 }
garphil 0:810679c5659e 169
garphil 0:810679c5659e 170 unsigned int alignDirection(unsigned int target) {
garphil 0:810679c5659e 171 unsigned int crossDir[4][4] = {
garphil 0:810679c5659e 172 {8,6,4,2},
garphil 0:810679c5659e 173 {4,8,2,6},
garphil 0:810679c5659e 174 {6,2,8,4},
garphil 0:810679c5659e 175 {2,4,6,8},
garphil 0:810679c5659e 176 };
garphil 0:810679c5659e 177 return crossDir[direction/2-1][target/2-1];
garphil 0:810679c5659e 178 }
garphil 0:810679c5659e 179
garphil 0:810679c5659e 180
garphil 0:810679c5659e 181 unsigned int alignStart() {
garphil 0:810679c5659e 182 return alignDirection(init_direction);
garphil 0:810679c5659e 183 }
garphil 0:810679c5659e 184
garphil 0:810679c5659e 185 unsigned int goStart() { // assumption laby is linear
garphil 0:810679c5659e 186 // path = 0 => not checked
garphil 0:810679c5659e 187 // path = 1 => checked
garphil 0:810679c5659e 188 // path = 2 => to be checked
garphil 0:810679c5659e 189 // path = 8 => Target
garphil 0:810679c5659e 190 int i,j,value;
garphil 0:810679c5659e 191 bool found = false;
garphil 0:810679c5659e 192 for(i=1;i<size_x;i++)
garphil 0:810679c5659e 193 for(j=1;j<size_y;j++)
garphil 0:810679c5659e 194 path[i][j]=0;
garphil 0:810679c5659e 195 path[init_x][init_y]=2;
garphil 0:810679c5659e 196 path[position_x][position_y]=8;
garphil 0:810679c5659e 197 while(!found) {
garphil 0:810679c5659e 198 for(j=1;j<size_y;j++)
garphil 0:810679c5659e 199 for(i=1;i<size_x;i++) {
garphil 0:810679c5659e 200 if(path[i][j]==2) {
garphil 0:810679c5659e 201 // check Up, then right, then down, then left.
garphil 0:810679c5659e 202 // Check UP
garphil 0:810679c5659e 203 value = path[i][j-1]|!labyrinthe[i][j-1];
garphil 0:810679c5659e 204 if(value ==0) path[i][j-1]=2;
garphil 0:810679c5659e 205 else if(value == 8) return alignDirection(LABY_DOWN);
garphil 0:810679c5659e 206 // Check DOWN
garphil 0:810679c5659e 207 value = path[i][j+1]|!labyrinthe[i][j+1];
garphil 0:810679c5659e 208 if(value ==0) path[i][j+1]=2;
garphil 0:810679c5659e 209 else if(value == 8) return alignDirection(LABY_UP);
garphil 0:810679c5659e 210 // Check RIGHT
garphil 0:810679c5659e 211 value = path[i+1][j]|!labyrinthe[i+1][j];
garphil 0:810679c5659e 212 if(value ==0) path[i+1][j]=2;
garphil 0:810679c5659e 213 else if(value == 8) return alignDirection(LABY_LEFT);
garphil 0:810679c5659e 214 // Check LEFT
garphil 0:810679c5659e 215 value = path[i-1][j]|!labyrinthe[i-1][j];
garphil 0:810679c5659e 216 if(value ==0) path[i-1][j]=2;
garphil 0:810679c5659e 217 else if(value == 8) return alignDirection(LABY_RIGHT);
garphil 0:810679c5659e 218
garphil 0:810679c5659e 219 path[i][j]=1;
garphil 0:810679c5659e 220
garphil 0:810679c5659e 221 }
garphil 0:810679c5659e 222 }
garphil 0:810679c5659e 223 }
garphil 0:810679c5659e 224 return LABY_UP;
garphil 0:810679c5659e 225 }
garphil 0:810679c5659e 226
garphil 0:810679c5659e 227 };
garphil 0:810679c5659e 228
garphil 0:810679c5659e 229 #endif