Alexandre Salconi-Denis
/
ProjetOctopode
Programme de contrôle de l'octopode 4DOF, Theraphosa-Salconi.
CtrlBridge
- fonction quelquonque pour communiquer avec les module
- fonction quelquonque pour faire des recherche dans les module dispo
- autre fonction pour jouer avec MemRegistre
Version 1.2.0
- Ajout d'un mode de simulation pour tester le code avec seulement un contrôleur stm32
directive.cpp@37:f53e9f7a14df, 2015-09-29 (annotated)
- Committer:
- salco
- Date:
- Tue Sep 29 17:23:25 2015 +0000
- Revision:
- 37:f53e9f7a14df
- Parent:
- 36:474d4795aafa
- Child:
- 38:57d5f36a0333
modification de la fonction analiseMaze();
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
salco | 2:4c5b0f6d0a6b | 1 | |
salco | 2:4c5b0f6d0a6b | 2 | #include "directive.h" |
salco | 36:474d4795aafa | 3 | |
salco | 31:ed23647fd16e | 4 | Serial pc(SERIAL_TX, SERIAL_RX); |
salco | 28:ac5c6350ed9a | 5 | int tabIR[12][2]= { |
salco | 28:ac5c6350ed9a | 6 | {80, 400},{50, 600},{40, 700},{30, 900},{25,1100}, |
salco | 28:ac5c6350ed9a | 7 | {20,1300},{15,1600},{10,2400},{ 8,2700},{ 7,3000}, |
salco | 28:ac5c6350ed9a | 8 | { 6,3200},{ 5,3100} |
salco | 28:ac5c6350ed9a | 9 | }; |
salco | 28:ac5c6350ed9a | 10 | |
salco | 28:ac5c6350ed9a | 11 | |
salco | 28:ac5c6350ed9a | 12 | |
salco | 28:ac5c6350ed9a | 13 | |
salco | 28:ac5c6350ed9a | 14 | |
salco | 2:4c5b0f6d0a6b | 15 | |
salco | 2:4c5b0f6d0a6b | 16 | Directive::Directive():TTask(0)//on veux que cette tache sois exec toute les fois que TTaskGen sexecute. |
salco | 2:4c5b0f6d0a6b | 17 | { |
salco | 28:ac5c6350ed9a | 18 | /* initialize random seed: */ |
salco | 31:ed23647fd16e | 19 | srand (time(NULL)); |
salco | 33:c8b4e5a392e6 | 20 | myMaze = new Labyrinthe(); |
salco | 33:c8b4e5a392e6 | 21 | |
salco | 28:ac5c6350ed9a | 22 | myMaze->setDirection(UP); |
salco | 28:ac5c6350ed9a | 23 | //c=0; |
salco | 35:d81f6b927932 | 24 | ssc32 = new Serial(PB_6, PB_7); |
salco | 27:ea60d12dccdf | 25 | ctrDesPattes = new Faculter_motrice(ssc32/*pc*/); |
salco | 28:ac5c6350ed9a | 26 | m_valueCapteurUltrasonic = 0; |
salco | 28:ac5c6350ed9a | 27 | m_valueCapteurIR = 0; |
salco | 28:ac5c6350ed9a | 28 | m_valueCapteurProximiter = 0; |
salco | 28:ac5c6350ed9a | 29 | for(int i =0; i<10; i++)tableauDeCommange[i]= 0; |
salco | 28:ac5c6350ed9a | 30 | size_tableauDeCommange=0; |
salco | 36:474d4795aafa | 31 | debug("\n\r set explorer \n\r"); |
salco | 36:474d4795aafa | 32 | myMaze->setMyPos(Labyrinthe::explorer); |
salco | 35:d81f6b927932 | 33 | //myMaze->setC(EXPLORER,C5); |
salco | 19:0ac80045ba4c | 34 | //m_CtrlBridge = m_CtrlBridge->getInstance(); |
salco | 19:0ac80045ba4c | 35 | //a enlever de commentaire//m_ListDesModules = m_CtrlBridge->findModule(0,1,0,0); |
salco | 27:ea60d12dccdf | 36 | |
salco | 27:ea60d12dccdf | 37 | //Serial pc(USBTX, USBRX); |
salco | 27:ea60d12dccdf | 38 | |
salco | 27:ea60d12dccdf | 39 | //ssc32->set_flow_control(0); |
salco | 31:ed23647fd16e | 40 | |
salco | 35:d81f6b927932 | 41 | |
salco | 35:d81f6b927932 | 42 | //debug("\n\r directive Init"); |
salco | 27:ea60d12dccdf | 43 | m_ListDesModules = m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0); |
salco | 27:ea60d12dccdf | 44 | m_ListDesModules.append(m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0)); |
salco | 28:ac5c6350ed9a | 45 | |
salco | 27:ea60d12dccdf | 46 | m_capteurUltrasonic= m_CtrlBridge->findModule(0,CAPTEUR,ULTRASONIQUE,0x27/*0b100111*/); |
salco | 35:d81f6b927932 | 47 | if(!(m_capteurUltrasonic.empty())) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurUltrasonic found"); |
salco | 35:d81f6b927932 | 48 | else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurUltrasonic not found"); |
salco | 27:ea60d12dccdf | 49 | m_capteurIR= m_CtrlBridge->findModule(0,CAPTEUR,DISTANCE,0x27/*0b100111*/); |
salco | 35:d81f6b927932 | 50 | if(!(m_capteurIR.empty())) debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurIR found"); |
salco | 34:dfd82c744b26 | 51 | else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurIR not found"); |
salco | 27:ea60d12dccdf | 52 | m_capteurProximiter= m_CtrlBridge->findModule(0,CAPTEUR,PROXIMITEE,0x0E/*0b001110*/); |
salco | 36:474d4795aafa | 53 | if(!(m_capteurProximiter.empty()))debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurProximiter found\n\r"); |
salco | 36:474d4795aafa | 54 | else debug(DEBUG_DIRECTIVE_TEST,"\n\r m_capteurProximiter not found\n\r"); |
salco | 28:ac5c6350ed9a | 55 | |
salco | 28:ac5c6350ed9a | 56 | ctrDesPattes->moveUp(); |
salco | 28:ac5c6350ed9a | 57 | |
salco | 28:ac5c6350ed9a | 58 | bool getUp=true; |
salco | 28:ac5c6350ed9a | 59 | while((getUp)&& (! ctrDesPattes->isSeqComplet())) { |
salco | 28:ac5c6350ed9a | 60 | ctrDesPattes->exec(); |
salco | 28:ac5c6350ed9a | 61 | |
salco | 28:ac5c6350ed9a | 62 | if(ctrDesPattes->get_idFrame() >= 5) |
salco | 28:ac5c6350ed9a | 63 | getUp = false; |
salco | 28:ac5c6350ed9a | 64 | |
salco | 28:ac5c6350ed9a | 65 | } |
salco | 31:ed23647fd16e | 66 | |
salco | 28:ac5c6350ed9a | 67 | ctrDesPattes->calibre(); |
salco | 31:ed23647fd16e | 68 | |
salco | 35:d81f6b927932 | 69 | #ifdef DEBUG_DIRECTIVE_GRAPHICAL_INTERFACE |
salco | 35:d81f6b927932 | 70 | analiseMaze(); |
salco | 31:ed23647fd16e | 71 | wait(2); |
salco | 31:ed23647fd16e | 72 | debug("\x1B[2J"); //clear screen |
salco | 35:d81f6b927932 | 73 | /*debug("\x1B[25l");//hide cursor |
salco | 35:d81f6b927932 | 74 | debug("\x1B[;H");*/ //cursor default position |
salco | 31:ed23647fd16e | 75 | |
salco | 35:d81f6b927932 | 76 | //createSLbox(0,0,5,15,"Mode"); |
salco | 35:d81f6b927932 | 77 | /*createDLbox(0,0,10,20,"Menu");//(1,12,10,20,"test2"); |
salco | 31:ed23647fd16e | 78 | setText(1,3,"1) BrainControle"); |
salco | 35:d81f6b927932 | 79 | setText(1,4,"2) Show Maze");*/ |
salco | 31:ed23647fd16e | 80 | //createDLbox(16,0,5,15,"Stage"); |
salco | 31:ed23647fd16e | 81 | |
salco | 35:d81f6b927932 | 82 | |
salco | 31:ed23647fd16e | 83 | #endif |
salco | 2:4c5b0f6d0a6b | 84 | } |
salco | 2:4c5b0f6d0a6b | 85 | Directive::~Directive() |
salco | 2:4c5b0f6d0a6b | 86 | { |
salco | 11:496453b45e25 | 87 | |
salco | 3:e0d6e081a9d3 | 88 | if(ctrDesPattes) |
salco | 3:e0d6e081a9d3 | 89 | delete ctrDesPattes; |
salco | 19:0ac80045ba4c | 90 | if(ssc32) |
salco | 19:0ac80045ba4c | 91 | delete ssc32; |
salco | 31:ed23647fd16e | 92 | if(myMaze) |
salco | 28:ac5c6350ed9a | 93 | delete myMaze; |
salco | 2:4c5b0f6d0a6b | 94 | } |
salco | 3:e0d6e081a9d3 | 95 | void Directive::task(void)//ALL CODE HERE// |
salco | 2:4c5b0f6d0a6b | 96 | { |
salco | 31:ed23647fd16e | 97 | |
salco | 31:ed23647fd16e | 98 | #ifdef DEBUG_BOOT_GRAPHICAL_INTERFACE |
salco | 37:f53e9f7a14df | 99 | debug("\x1B[2J"); //clear screen |
salco | 37:f53e9f7a14df | 100 | debug("\x1B[25l");//hide cursor |
salco | 37:f53e9f7a14df | 101 | debug("\x1B[;H"); //cursor default position |
salco | 37:f53e9f7a14df | 102 | /* |
salco | 37:f53e9f7a14df | 103 | createDLbox(0,0,10,25,"BrainControle"); |
salco | 37:f53e9f7a14df | 104 | setText(1,3,"1) Move Up"); |
salco | 37:f53e9f7a14df | 105 | setText(1,4,"2) Move Down"); |
salco | 37:f53e9f7a14df | 106 | setText(1,5,"3) Move Left"); |
salco | 37:f53e9f7a14df | 107 | setText(1,6,"4) Move Right"); |
salco | 37:f53e9f7a14df | 108 | |
salco | 37:f53e9f7a14df | 109 | int idCommand; |
salco | 37:f53e9f7a14df | 110 | do{ |
salco | 37:f53e9f7a14df | 111 | if(pc.readable()){ |
salco | 37:f53e9f7a14df | 112 | idCommand = fgetc(pc) - '0'; |
salco | 37:f53e9f7a14df | 113 | switch(idCommand) |
salco | 37:f53e9f7a14df | 114 | { |
salco | 37:f53e9f7a14df | 115 | case 1: |
salco | 37:f53e9f7a14df | 116 | myMaze->goUp(); |
salco | 37:f53e9f7a14df | 117 | break; |
salco | 37:f53e9f7a14df | 118 | case 2: |
salco | 37:f53e9f7a14df | 119 | myMaze->goDown(); |
salco | 37:f53e9f7a14df | 120 | break; |
salco | 37:f53e9f7a14df | 121 | case 3: |
salco | 37:f53e9f7a14df | 122 | myMaze->goLeft(); |
salco | 37:f53e9f7a14df | 123 | break; |
salco | 37:f53e9f7a14df | 124 | case 4: |
salco | 37:f53e9f7a14df | 125 | myMaze->goRight(); |
salco | 37:f53e9f7a14df | 126 | break; |
salco | 37:f53e9f7a14df | 127 | } |
salco | 37:f53e9f7a14df | 128 | myMaze->setMyPos(Labyrinthe::explorer); |
salco | 37:f53e9f7a14df | 129 | |
salco | 37:f53e9f7a14df | 130 | string mymap = myMaze->showMap(); |
salco | 37:f53e9f7a14df | 131 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r Labyrinthe map: X:%02x Y:%02x \n\r",myMaze->getX(),myMaze->getY()); |
salco | 37:f53e9f7a14df | 132 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+6]); |
salco | 37:f53e9f7a14df | 133 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 134 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+3]); |
salco | 37:f53e9f7a14df | 135 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 136 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i]); |
salco | 37:f53e9f7a14df | 137 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 138 | |
salco | 37:f53e9f7a14df | 139 | #ifdef DBG_MAIN_DELAY_SEC |
salco | 37:f53e9f7a14df | 140 | debug("\n\r\n wait \n\r\n"); |
salco | 37:f53e9f7a14df | 141 | wait(DBG_MAIN_DELAY_SEC); |
salco | 37:f53e9f7a14df | 142 | #endif |
salco | 37:f53e9f7a14df | 143 | |
salco | 37:f53e9f7a14df | 144 | debug("\x1B[2J"); //clear screen |
salco | 37:f53e9f7a14df | 145 | debug("\x1B[;H"); //cursor default position |
salco | 37:f53e9f7a14df | 146 | createDLbox(0,0,10,25,"BrainControle"); |
salco | 37:f53e9f7a14df | 147 | setText(1,3,"1) Move Up"); |
salco | 37:f53e9f7a14df | 148 | setText(1,4,"2) Move Down"); |
salco | 37:f53e9f7a14df | 149 | setText(1,5,"3) Move Left"); |
salco | 37:f53e9f7a14df | 150 | setText(1,6,"4) Move Right"); |
salco | 37:f53e9f7a14df | 151 | } |
salco | 37:f53e9f7a14df | 152 | }while(1); */ |
salco | 31:ed23647fd16e | 153 | #endif |
salco | 36:474d4795aafa | 154 | |
salco | 28:ac5c6350ed9a | 155 | //debug(DEBUG_DIRECTIVE_TEST,"\n\rIn task directive"); |
salco | 37:f53e9f7a14df | 156 | #ifndef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 157 | if(ctrDesPattes->isSeqComplet()) { |
salco | 37:f53e9f7a14df | 158 | |
salco | 28:ac5c6350ed9a | 159 | if((tableauDeCommange[0] == 6) && (size_tableauDeCommange == 1)) { |
salco | 28:ac5c6350ed9a | 160 | //addTableauDeCommande(0x81); |
salco | 28:ac5c6350ed9a | 161 | //addTableauDeCommande(0x82); |
salco | 28:ac5c6350ed9a | 162 | //mabey//addTableauDeCommande(0x83); |
salco | 28:ac5c6350ed9a | 163 | addTableauDeCommande(0x84); |
salco | 28:ac5c6350ed9a | 164 | |
salco | 28:ac5c6350ed9a | 165 | //addTableauDeCommande(0x85); |
salco | 28:ac5c6350ed9a | 166 | //mabey//addTableauDeCommande(0x86); |
salco | 28:ac5c6350ed9a | 167 | addTableauDeCommande(0x87); |
salco | 28:ac5c6350ed9a | 168 | addTableauDeCommande(0x88); |
salco | 28:ac5c6350ed9a | 169 | } |
salco | 37:f53e9f7a14df | 170 | |
salco | 37:f53e9f7a14df | 171 | |
salco | 28:ac5c6350ed9a | 172 | if(tableauDeCommange[0] == 0) { |
salco | 28:ac5c6350ed9a | 173 | //pas sur que c'Est tout ce qui doit etre ici mais je vois pas quoi d'autre pour le moment. |
salco | 28:ac5c6350ed9a | 174 | |
salco | 28:ac5c6350ed9a | 175 | //////////////// |
salco | 28:ac5c6350ed9a | 176 | // Inspection // |
salco | 28:ac5c6350ed9a | 177 | //////////////// |
salco | 28:ac5c6350ed9a | 178 | //flag.append(1,0);//0x02); |
salco | 28:ac5c6350ed9a | 179 | /*for(int i=0; i<m_ListDesModules.length(); ++i) { |
salco | 28:ac5c6350ed9a | 180 | wait_us(300); |
salco | 28:ac5c6350ed9a | 181 | flag.clear(); |
salco | 28:ac5c6350ed9a | 182 | flag.append(1,7); |
salco | 28:ac5c6350ed9a | 183 | data.clear(); |
salco | 28:ac5c6350ed9a | 184 | debug("\n\r result: %d",m_CtrlBridge->send(m_ListDesModules.at(i),flag,data)); |
salco | 28:ac5c6350ed9a | 185 | |
salco | 28:ac5c6350ed9a | 186 | debug("\n\r flag: %d",flag[0]); |
salco | 28:ac5c6350ed9a | 187 | debug("\n\r data: %x",data[0]); |
salco | 28:ac5c6350ed9a | 188 | }*/ |
salco | 37:f53e9f7a14df | 189 | |
salco | 28:ac5c6350ed9a | 190 | updateModuleValue(); |
salco | 28:ac5c6350ed9a | 191 | |
salco | 28:ac5c6350ed9a | 192 | ////////////////////////////// |
salco | 28:ac5c6350ed9a | 193 | // Traitement du Labyrinthe // |
salco | 28:ac5c6350ed9a | 194 | ////////////////////////////// |
salco | 28:ac5c6350ed9a | 195 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -IRToCm(%02f): %02f",ADCTomv(m_valueCapteurIR),IRToCm(ADCTomv(m_valueCapteurIR))); |
salco | 28:ac5c6350ed9a | 196 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -ultrasonicToInch: %02f",ultrasonicToInch(m_valueCapteurUltrasonic)); |
salco | 28:ac5c6350ed9a | 197 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -m_valueCapteurProximiter: %02d",m_valueCapteurProximiter); |
salco | 37:f53e9f7a14df | 198 | #endif //DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 199 | analiseMaze(); |
salco | 28:ac5c6350ed9a | 200 | char nextCase=0; |
salco | 31:ed23647fd16e | 201 | switch(myMaze->getDirection()) { |
salco | 31:ed23647fd16e | 202 | case UP: |
salco | 31:ed23647fd16e | 203 | nextCase = myMaze->getC(myMaze->getX(),myMaze->getY()+1); |
salco | 31:ed23647fd16e | 204 | break; |
salco | 31:ed23647fd16e | 205 | |
salco | 31:ed23647fd16e | 206 | case DOWN: |
salco | 31:ed23647fd16e | 207 | nextCase = myMaze->getC(myMaze->getX(),myMaze->getY()-1); |
salco | 31:ed23647fd16e | 208 | break; |
salco | 31:ed23647fd16e | 209 | |
salco | 31:ed23647fd16e | 210 | case LEFT: |
salco | 31:ed23647fd16e | 211 | nextCase = myMaze->getC(myMaze->getX()-1,myMaze->getY()); |
salco | 31:ed23647fd16e | 212 | break; |
salco | 31:ed23647fd16e | 213 | |
salco | 31:ed23647fd16e | 214 | case RIGHT: |
salco | 31:ed23647fd16e | 215 | nextCase = myMaze->getC(myMaze->getX()+1,myMaze->getY()); |
salco | 31:ed23647fd16e | 216 | break; |
salco | 28:ac5c6350ed9a | 217 | } |
salco | 31:ed23647fd16e | 218 | |
salco | 31:ed23647fd16e | 219 | switch(nextCase ) { |
salco | 37:f53e9f7a14df | 220 | case Labyrinthe::vide : |
salco | 37:f53e9f7a14df | 221 | #ifdef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 37:f53e9f7a14df | 222 | debug("nextCase Vide\n\r"); |
salco | 37:f53e9f7a14df | 223 | #endif |
salco | 31:ed23647fd16e | 224 | debug(DEBUG_DIRECTIVE_TEST," Javance"); |
salco | 37:f53e9f7a14df | 225 | #ifndef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 226 | addTableauDeCommande(6); |
salco | 37:f53e9f7a14df | 227 | #endif |
salco | 31:ed23647fd16e | 228 | break; |
salco | 31:ed23647fd16e | 229 | |
salco | 37:f53e9f7a14df | 230 | case Labyrinthe::explorer :// ici pt amiliorer |
salco | 31:ed23647fd16e | 231 | debug(DEBUG_DIRECTIVE_TEST," Javance"); |
salco | 37:f53e9f7a14df | 232 | #ifndef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 233 | addTableauDeCommande(6); |
salco | 37:f53e9f7a14df | 234 | #endif |
salco | 31:ed23647fd16e | 235 | break; |
salco | 31:ed23647fd16e | 236 | |
salco | 36:474d4795aafa | 237 | case Labyrinthe::mur: |
salco | 31:ed23647fd16e | 238 | |
salco | 31:ed23647fd16e | 239 | switch(myMaze->getDirection()) { |
salco | 31:ed23647fd16e | 240 | case UP: |
salco | 31:ed23647fd16e | 241 | if(rand()%1) { |
salco | 31:ed23647fd16e | 242 | nextCase = myMaze->getC(myMaze->getX()-1,myMaze->getY()); |
salco | 36:474d4795aafa | 243 | if(nextCase != Labyrinthe::mur) { |
salco | 31:ed23647fd16e | 244 | } |
salco | 31:ed23647fd16e | 245 | nextCase = myMaze->getC(myMaze->getX()+1,myMaze->getY()); |
salco | 31:ed23647fd16e | 246 | } else { |
salco | 31:ed23647fd16e | 247 | nextCase = myMaze->getC(myMaze->getX()+1,myMaze->getY()+1); |
salco | 31:ed23647fd16e | 248 | } |
salco | 31:ed23647fd16e | 249 | break; |
salco | 31:ed23647fd16e | 250 | |
salco | 31:ed23647fd16e | 251 | case DOWN: |
salco | 31:ed23647fd16e | 252 | nextCase = myMaze->getC(myMaze->getX(),myMaze->getY()-1); |
salco | 31:ed23647fd16e | 253 | break; |
salco | 31:ed23647fd16e | 254 | |
salco | 31:ed23647fd16e | 255 | case LEFT: |
salco | 31:ed23647fd16e | 256 | nextCase = myMaze->getC(myMaze->getX()-1,myMaze->getY()); |
salco | 31:ed23647fd16e | 257 | break; |
salco | 31:ed23647fd16e | 258 | |
salco | 31:ed23647fd16e | 259 | case RIGHT: |
salco | 31:ed23647fd16e | 260 | nextCase = myMaze->getC(myMaze->getX()+1,myMaze->getY()); |
salco | 31:ed23647fd16e | 261 | break; |
salco | 31:ed23647fd16e | 262 | } |
salco | 31:ed23647fd16e | 263 | |
salco | 31:ed23647fd16e | 264 | break; |
salco | 31:ed23647fd16e | 265 | |
salco | 37:f53e9f7a14df | 266 | case Labyrinthe::pasExplorer : |
salco | 31:ed23647fd16e | 267 | debug(DEBUG_DIRECTIVE_TEST," Javance"); |
salco | 37:f53e9f7a14df | 268 | #ifndef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 31:ed23647fd16e | 269 | addTableauDeCommande(6); |
salco | 37:f53e9f7a14df | 270 | #endif |
salco | 31:ed23647fd16e | 271 | break; |
salco | 28:ac5c6350ed9a | 272 | } |
salco | 31:ed23647fd16e | 273 | |
salco | 37:f53e9f7a14df | 274 | #ifndef DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 275 | /*if((ultrasonicToInch(m_valueCapteurUltrasonic)< 1) && (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)) //capteur ultrasson embrouiller/imprecis |
salco | 28:ac5c6350ed9a | 276 | { |
salco | 28:ac5c6350ed9a | 277 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); |
salco | 28:ac5c6350ed9a | 278 | addTableauDeCommande(6); |
salco | 31:ed23647fd16e | 279 | } |
salco | 28:ac5c6350ed9a | 280 | else if(ultrasonicToInch(m_valueCapteurUltrasonic) < 10) { //plus proche que 10pouce |
salco | 28:ac5c6350ed9a | 281 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche"); |
salco | 28:ac5c6350ed9a | 282 | if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 10) { |
salco | 28:ac5c6350ed9a | 283 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); |
salco | 28:ac5c6350ed9a | 284 | addTableauDeCommande(7); |
salco | 28:ac5c6350ed9a | 285 | |
salco | 19:0ac80045ba4c | 286 | |
salco | 28:ac5c6350ed9a | 287 | //ici potentielle scaning |
salco | 28:ac5c6350ed9a | 288 | } else { |
salco | 28:ac5c6350ed9a | 289 | addTableauDeCommande(4); |
salco | 28:ac5c6350ed9a | 290 | addTableauDeCommande(4); |
salco | 28:ac5c6350ed9a | 291 | addTableauDeCommande(4); |
salco | 28:ac5c6350ed9a | 292 | } |
salco | 28:ac5c6350ed9a | 293 | } else { |
salco | 28:ac5c6350ed9a | 294 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Nothing:"); |
salco | 28:ac5c6350ed9a | 295 | addTableauDeCommande(6); |
salco | 31:ed23647fd16e | 296 | |
salco | 31:ed23647fd16e | 297 | |
salco | 28:ac5c6350ed9a | 298 | int randomNbr = rand() % 4; |
salco | 28:ac5c6350ed9a | 299 | switch(randomNbr) |
salco | 28:ac5c6350ed9a | 300 | { |
salco | 28:ac5c6350ed9a | 301 | case 1: |
salco | 28:ac5c6350ed9a | 302 | debug(DEBUG_DIRECTIVE_TEST," Javance"); |
salco | 28:ac5c6350ed9a | 303 | addTableauDeCommande(6); |
salco | 28:ac5c6350ed9a | 304 | break; |
salco | 28:ac5c6350ed9a | 305 | case 2: |
salco | 28:ac5c6350ed9a | 306 | debug(DEBUG_DIRECTIVE_TEST," Je tourne a gauche"); |
salco | 28:ac5c6350ed9a | 307 | addTableauDeCommande(4); |
salco | 28:ac5c6350ed9a | 308 | break; |
salco | 28:ac5c6350ed9a | 309 | case 3: |
salco | 28:ac5c6350ed9a | 310 | debug(DEBUG_DIRECTIVE_TEST," Je tourne a droite"); |
salco | 28:ac5c6350ed9a | 311 | addTableauDeCommande(5); |
salco | 28:ac5c6350ed9a | 312 | break; |
salco | 28:ac5c6350ed9a | 313 | default: |
salco | 28:ac5c6350ed9a | 314 | debug(DEBUG_DIRECTIVE_TEST," Jattend"); |
salco | 28:ac5c6350ed9a | 315 | break; |
salco | 28:ac5c6350ed9a | 316 | } |
salco | 31:ed23647fd16e | 317 | |
salco | 31:ed23647fd16e | 318 | |
salco | 28:ac5c6350ed9a | 319 | //addTableauDeCommande(6); |
salco | 28:ac5c6350ed9a | 320 | }*/ |
salco | 37:f53e9f7a14df | 321 | |
salco | 28:ac5c6350ed9a | 322 | /////////////// |
salco | 28:ac5c6350ed9a | 323 | // Mouvement // |
salco | 28:ac5c6350ed9a | 324 | /////////////// |
salco | 37:f53e9f7a14df | 325 | } |
salco | 37:f53e9f7a14df | 326 | else { |
salco | 37:f53e9f7a14df | 327 | |
salco | 28:ac5c6350ed9a | 328 | switch(tableauDeCommange[0]) { |
salco | 28:ac5c6350ed9a | 329 | case 11: |
salco | 28:ac5c6350ed9a | 330 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_calibre"); |
salco | 28:ac5c6350ed9a | 331 | ctrDesPattes->calibre(); |
salco | 28:ac5c6350ed9a | 332 | break; |
salco | 28:ac5c6350ed9a | 333 | case 2: |
salco | 28:ac5c6350ed9a | 334 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveUp"); |
salco | 28:ac5c6350ed9a | 335 | ctrDesPattes->moveUp(); |
salco | 28:ac5c6350ed9a | 336 | break; |
salco | 28:ac5c6350ed9a | 337 | case 3: |
salco | 28:ac5c6350ed9a | 338 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveDown"); |
salco | 28:ac5c6350ed9a | 339 | ctrDesPattes->moveDown(); |
salco | 28:ac5c6350ed9a | 340 | break; |
salco | 28:ac5c6350ed9a | 341 | case 4: |
salco | 28:ac5c6350ed9a | 342 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_turnLeft"); |
salco | 28:ac5c6350ed9a | 343 | ctrDesPattes->turnLeft(); |
salco | 28:ac5c6350ed9a | 344 | break; |
salco | 28:ac5c6350ed9a | 345 | case 5: |
salco | 28:ac5c6350ed9a | 346 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_trunRight"); |
salco | 28:ac5c6350ed9a | 347 | ctrDesPattes->trunRight(); |
salco | 28:ac5c6350ed9a | 348 | break; |
salco | 28:ac5c6350ed9a | 349 | case 6: |
salco | 28:ac5c6350ed9a | 350 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveFront"); |
salco | 28:ac5c6350ed9a | 351 | ctrDesPattes->moveFront(); |
salco | 28:ac5c6350ed9a | 352 | break; |
salco | 28:ac5c6350ed9a | 353 | case 7: |
salco | 28:ac5c6350ed9a | 354 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_moveBack"); |
salco | 28:ac5c6350ed9a | 355 | ctrDesPattes->moveBack(); |
salco | 28:ac5c6350ed9a | 356 | break; |
salco | 28:ac5c6350ed9a | 357 | /*case 8: |
salco | 28:ac5c6350ed9a | 358 | ctrDesPattes->moveBack(); |
salco | 28:ac5c6350ed9a | 359 | break;*/ |
salco | 28:ac5c6350ed9a | 360 | case 0x81: |
salco | 28:ac5c6350ed9a | 361 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 1"); |
salco | 28:ac5c6350ed9a | 362 | ctrDesPattes->repositioner(1); |
salco | 28:ac5c6350ed9a | 363 | break; |
salco | 28:ac5c6350ed9a | 364 | case 0x82: |
salco | 28:ac5c6350ed9a | 365 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 2"); |
salco | 28:ac5c6350ed9a | 366 | ctrDesPattes->repositioner(2); |
salco | 28:ac5c6350ed9a | 367 | break; |
salco | 28:ac5c6350ed9a | 368 | case 0x83: |
salco | 28:ac5c6350ed9a | 369 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 3"); |
salco | 28:ac5c6350ed9a | 370 | ctrDesPattes->repositioner(3); |
salco | 28:ac5c6350ed9a | 371 | break; |
salco | 28:ac5c6350ed9a | 372 | case 0x84: |
salco | 28:ac5c6350ed9a | 373 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 4"); |
salco | 28:ac5c6350ed9a | 374 | ctrDesPattes->repositioner(4); |
salco | 28:ac5c6350ed9a | 375 | break; |
salco | 28:ac5c6350ed9a | 376 | case 0x85: |
salco | 28:ac5c6350ed9a | 377 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 5"); |
salco | 28:ac5c6350ed9a | 378 | ctrDesPattes->repositioner(5); |
salco | 28:ac5c6350ed9a | 379 | break; |
salco | 28:ac5c6350ed9a | 380 | case 0x86: |
salco | 28:ac5c6350ed9a | 381 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 6"); |
salco | 28:ac5c6350ed9a | 382 | ctrDesPattes->repositioner(6); |
salco | 28:ac5c6350ed9a | 383 | break; |
salco | 28:ac5c6350ed9a | 384 | case 0x87: |
salco | 28:ac5c6350ed9a | 385 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 7"); |
salco | 28:ac5c6350ed9a | 386 | ctrDesPattes->repositioner(7); |
salco | 28:ac5c6350ed9a | 387 | break; |
salco | 28:ac5c6350ed9a | 388 | case 0x88: |
salco | 28:ac5c6350ed9a | 389 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -set_Repositioner 8"); |
salco | 28:ac5c6350ed9a | 390 | ctrDesPattes->repositioner(8); |
salco | 28:ac5c6350ed9a | 391 | break; |
salco | 28:ac5c6350ed9a | 392 | } |
salco | 28:ac5c6350ed9a | 393 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -TAB:"); |
salco | 28:ac5c6350ed9a | 394 | for(int i =0; i<10; i++) debug(DEBUG_DIRECTIVE_TEST,"[%02x]",tableauDeCommange[i]); |
salco | 28:ac5c6350ed9a | 395 | for(int i =0; i<9; i++)tableauDeCommange[i]= tableauDeCommange[i+1]; |
salco | 28:ac5c6350ed9a | 396 | tableauDeCommange[9]=0; |
salco | 28:ac5c6350ed9a | 397 | if(size_tableauDeCommange != 0) size_tableauDeCommange--; |
salco | 28:ac5c6350ed9a | 398 | } |
salco | 37:f53e9f7a14df | 399 | |
salco | 37:f53e9f7a14df | 400 | } else { |
salco | 28:ac5c6350ed9a | 401 | ctrDesPattes->exec(); |
salco | 28:ac5c6350ed9a | 402 | } |
salco | 37:f53e9f7a14df | 403 | #endif //DEBUG_DIRECTIVE_LEG_DISABLE |
salco | 28:ac5c6350ed9a | 404 | //////////////////// |
salco | 28:ac5c6350ed9a | 405 | // Update memoire // |
salco | 28:ac5c6350ed9a | 406 | //////////////////// |
salco | 28:ac5c6350ed9a | 407 | |
salco | 28:ac5c6350ed9a | 408 | //////////// |
salco | 28:ac5c6350ed9a | 409 | // Autre? // |
salco | 28:ac5c6350ed9a | 410 | //////////// |
salco | 28:ac5c6350ed9a | 411 | |
salco | 28:ac5c6350ed9a | 412 | |
salco | 28:ac5c6350ed9a | 413 | //debug(DEBUG_DIRECTIVE_TEST,"\n\rOut task directive"); |
salco | 37:f53e9f7a14df | 414 | #ifdef DEBUG_BOOT_GRAPHICAL_INTERFACE |
salco | 37:f53e9f7a14df | 415 | #ifdef DBG_MAIN_DELAY_SEC |
salco | 37:f53e9f7a14df | 416 | debug("\n\r\n wait \n\r\n"); |
salco | 37:f53e9f7a14df | 417 | wait(DBG_MAIN_DELAY_SEC); |
salco | 37:f53e9f7a14df | 418 | #endif |
salco | 37:f53e9f7a14df | 419 | #endif |
salco | 28:ac5c6350ed9a | 420 | } |
salco | 28:ac5c6350ed9a | 421 | double Directive::IRToCm (double miliVolt) |
salco | 28:ac5c6350ed9a | 422 | { |
salco | 28:ac5c6350ed9a | 423 | double result=0; |
salco | 28:ac5c6350ed9a | 424 | |
salco | 28:ac5c6350ed9a | 425 | if(miliVolt < tabIR[0][1])result = tabIR[0][0]; |
salco | 28:ac5c6350ed9a | 426 | else if(miliVolt > tabIR[11][1])result = tabIR[11][0]; |
salco | 28:ac5c6350ed9a | 427 | |
salco | 28:ac5c6350ed9a | 428 | for(int i =1; (result==0)&&(i<12); i++) { |
salco | 28:ac5c6350ed9a | 429 | if((tabIR[i-1][1]<=miliVolt) && (tabIR[i][1]>miliVolt)) { |
salco | 28:ac5c6350ed9a | 430 | result=tabIR[i-1][0]+((tabIR[i][0]-tabIR[i-1][0])*((tabIR[i-1][1]-miliVolt)/(tabIR[i-1][1]-tabIR[i][1]))); |
salco | 28:ac5c6350ed9a | 431 | } |
salco | 28:ac5c6350ed9a | 432 | } |
salco | 28:ac5c6350ed9a | 433 | return result; |
salco | 28:ac5c6350ed9a | 434 | } |
salco | 28:ac5c6350ed9a | 435 | void Directive::addTableauDeCommande(unsigned char value) |
salco | 28:ac5c6350ed9a | 436 | { |
salco | 28:ac5c6350ed9a | 437 | if(size_tableauDeCommange!=10) { |
salco | 28:ac5c6350ed9a | 438 | tableauDeCommange[size_tableauDeCommange]=value; |
salco | 28:ac5c6350ed9a | 439 | size_tableauDeCommange++; |
salco | 28:ac5c6350ed9a | 440 | |
salco | 28:ac5c6350ed9a | 441 | } |
salco | 28:ac5c6350ed9a | 442 | } |
salco | 28:ac5c6350ed9a | 443 | |
salco | 28:ac5c6350ed9a | 444 | void Directive::updateModuleValue(void) |
salco | 28:ac5c6350ed9a | 445 | { |
salco | 28:ac5c6350ed9a | 446 | string flag,data; |
salco | 28:ac5c6350ed9a | 447 | flag.clear(); |
salco | 28:ac5c6350ed9a | 448 | data.clear(); |
salco | 28:ac5c6350ed9a | 449 | flag.append(1,7); |
salco | 28:ac5c6350ed9a | 450 | if(m_CtrlBridge->send(m_capteurUltrasonic[0],flag,data)) |
salco | 28:ac5c6350ed9a | 451 | m_valueCapteurUltrasonic = data[0]; |
salco | 28:ac5c6350ed9a | 452 | |
salco | 28:ac5c6350ed9a | 453 | |
salco | 28:ac5c6350ed9a | 454 | flag.clear(); |
salco | 28:ac5c6350ed9a | 455 | data.clear(); |
salco | 28:ac5c6350ed9a | 456 | flag.append(1,7); |
salco | 28:ac5c6350ed9a | 457 | if(m_CtrlBridge->send(m_capteurIR[0],flag,data)) { |
salco | 28:ac5c6350ed9a | 458 | if(data.size() == 2) { |
salco | 28:ac5c6350ed9a | 459 | m_valueCapteurIR = data[0]; |
salco | 28:ac5c6350ed9a | 460 | m_valueCapteurIR = (m_valueCapteurIR<<8) + data[1]; |
salco | 28:ac5c6350ed9a | 461 | } else |
salco | 28:ac5c6350ed9a | 462 | m_valueCapteurIR = data[0]; |
salco | 28:ac5c6350ed9a | 463 | } |
salco | 28:ac5c6350ed9a | 464 | |
salco | 28:ac5c6350ed9a | 465 | flag.clear(); |
salco | 28:ac5c6350ed9a | 466 | data.clear(); |
salco | 28:ac5c6350ed9a | 467 | flag.append(1,7); |
salco | 28:ac5c6350ed9a | 468 | if(m_CtrlBridge->send(m_capteurProximiter[0],flag,data)) |
salco | 28:ac5c6350ed9a | 469 | m_valueCapteurProximiter = data[0]; |
salco | 28:ac5c6350ed9a | 470 | } |
salco | 28:ac5c6350ed9a | 471 | |
salco | 28:ac5c6350ed9a | 472 | void Directive::analiseMaze(void) |
salco | 28:ac5c6350ed9a | 473 | { |
salco | 28:ac5c6350ed9a | 474 | char areaLatt =0; |
salco | 28:ac5c6350ed9a | 475 | char areaVert =0; |
salco | 28:ac5c6350ed9a | 476 | string mymap = myMaze->showMap(); |
salco | 28:ac5c6350ed9a | 477 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r Labyrinthe map: X:%02x Y:%02x \n\r",myMaze->getX(),myMaze->getY()); |
salco | 37:f53e9f7a14df | 478 | |
salco | 37:f53e9f7a14df | 479 | debug("Direction: "); |
salco | 37:f53e9f7a14df | 480 | switch(myMaze->getDirection()) { |
salco | 37:f53e9f7a14df | 481 | case UP: |
salco | 37:f53e9f7a14df | 482 | debug(DEBUG_DIRECTIVE_LABYRINTH,"Up \n\r"); |
salco | 37:f53e9f7a14df | 483 | break; |
salco | 37:f53e9f7a14df | 484 | case DOWN: |
salco | 37:f53e9f7a14df | 485 | debug(DEBUG_DIRECTIVE_LABYRINTH,"Down \n\r"); |
salco | 37:f53e9f7a14df | 486 | break; |
salco | 37:f53e9f7a14df | 487 | case LEFT: |
salco | 37:f53e9f7a14df | 488 | debug(DEBUG_DIRECTIVE_LABYRINTH,"Left \n\r"); |
salco | 37:f53e9f7a14df | 489 | break; |
salco | 37:f53e9f7a14df | 490 | case RIGHT: |
salco | 37:f53e9f7a14df | 491 | debug(DEBUG_DIRECTIVE_LABYRINTH,"Right \n\r"); |
salco | 37:f53e9f7a14df | 492 | break; |
salco | 37:f53e9f7a14df | 493 | } |
salco | 37:f53e9f7a14df | 494 | |
salco | 36:474d4795aafa | 495 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+6]); |
salco | 28:ac5c6350ed9a | 496 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 31:ed23647fd16e | 497 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+3]); |
salco | 28:ac5c6350ed9a | 498 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 36:474d4795aafa | 499 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i]); |
salco | 28:ac5c6350ed9a | 500 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 31:ed23647fd16e | 501 | |
salco | 31:ed23647fd16e | 502 | |
salco | 37:f53e9f7a14df | 503 | #ifndef DEBUG_DIRECTIVE_LABYRINTH_USER_CTRL |
salco | 31:ed23647fd16e | 504 | if((ultrasonicToInch(m_valueCapteurUltrasonic)< 1) && (IRToCm(ADCTomv(m_valueCapteurIR)) <= 80)) { //capteur ultrasson embrouiller/imprecis |
salco | 31:ed23647fd16e | 505 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Capteur Ultrasson brouiller"); |
salco | 31:ed23647fd16e | 506 | //addTableauDeCommande(6); |
salco | 31:ed23647fd16e | 507 | } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=24) { //plus proche que 10pouce |
salco | 31:ed23647fd16e | 508 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); |
salco | 31:ed23647fd16e | 509 | areaLatt =2; |
salco | 31:ed23647fd16e | 510 | areaVert =2; |
salco | 31:ed23647fd16e | 511 | } else if(ultrasonicToInch(m_valueCapteurUltrasonic) >=12) { //plus proche que 10pouce |
salco | 31:ed23647fd16e | 512 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Objet proche 2pi"); |
salco | 31:ed23647fd16e | 513 | areaLatt =1; |
salco | 31:ed23647fd16e | 514 | areaVert =1; |
salco | 31:ed23647fd16e | 515 | } |
salco | 37:f53e9f7a14df | 516 | debug(DEBUG_DIRECTIVE_TEST,": %i",ultrasonicToInch(m_valueCapteurUltrasonic)); |
salco | 37:f53e9f7a14df | 517 | |
salco | 31:ed23647fd16e | 518 | if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 60) { |
salco | 31:ed23647fd16e | 519 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); |
salco | 31:ed23647fd16e | 520 | areaVert =2; |
salco | 31:ed23647fd16e | 521 | } else if(IRToCm(ADCTomv(m_valueCapteurIR)) <= 30) { |
salco | 31:ed23647fd16e | 522 | debug(DEBUG_DIRECTIVE_TEST,"\n\r -Confirmation IR"); |
salco | 31:ed23647fd16e | 523 | areaVert =1; |
salco | 31:ed23647fd16e | 524 | } |
salco | 37:f53e9f7a14df | 525 | debug(DEBUG_DIRECTIVE_TEST,": %i",IRToCm(ADCTomv(m_valueCapteurIR)); |
salco | 37:f53e9f7a14df | 526 | #endif |
salco | 31:ed23647fd16e | 527 | |
salco | 37:f53e9f7a14df | 528 | #ifdef DEBUG_DIRECTIVE_LABYRINTH_USER_CTRL |
salco | 37:f53e9f7a14df | 529 | debug("\n\r---analise maze(manualy CTRL)---\n\r"); |
salco | 37:f53e9f7a14df | 530 | int idCommand = -1; |
salco | 37:f53e9f7a14df | 531 | do{ |
salco | 37:f53e9f7a14df | 532 | debug("\n\r Enter areaVert [0,1,2]: "); idCommand = fgetc(pc) - '0'; |
salco | 37:f53e9f7a14df | 533 | debug(" %i",idCommand); |
salco | 37:f53e9f7a14df | 534 | }while((idCommand < 0) && (idCommand > 2)); |
salco | 37:f53e9f7a14df | 535 | areaVert = idCommand; |
salco | 37:f53e9f7a14df | 536 | |
salco | 37:f53e9f7a14df | 537 | do{ |
salco | 37:f53e9f7a14df | 538 | debug("\n\r Enter areaLatt [0,1,2]: "); idCommand = fgetc(pc) - '0'; |
salco | 37:f53e9f7a14df | 539 | debug(" %i",idCommand); |
salco | 37:f53e9f7a14df | 540 | }while((idCommand < 0) && (idCommand > 2)); |
salco | 37:f53e9f7a14df | 541 | areaLatt = idCommand; |
salco | 31:ed23647fd16e | 542 | |
salco | 37:f53e9f7a14df | 543 | debug("\n\r"); |
salco | 37:f53e9f7a14df | 544 | #endif |
salco | 37:f53e9f7a14df | 545 | //changer lalgoritme ici |
salco | 37:f53e9f7a14df | 546 | |
salco | 37:f53e9f7a14df | 547 | /* |
salco | 37:f53e9f7a14df | 548 | /---\ |
salco | 37:f53e9f7a14df | 549 | |?M?| //vert 0 |
salco | 37:f53e9f7a14df | 550 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 551 | |...| |
salco | 37:f53e9f7a14df | 552 | \---/ |
salco | 31:ed23647fd16e | 553 | |
salco | 37:f53e9f7a14df | 554 | /-M-\ |
salco | 37:f53e9f7a14df | 555 | |?V?| //vert 1 |
salco | 37:f53e9f7a14df | 556 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 557 | |...| |
salco | 37:f53e9f7a14df | 558 | \---/ |
salco | 37:f53e9f7a14df | 559 | |
salco | 37:f53e9f7a14df | 560 | /?M?\ |
salco | 37:f53e9f7a14df | 561 | |VVV| //vert 1 |
salco | 37:f53e9f7a14df | 562 | |.X.| //latt 1 |
salco | 37:f53e9f7a14df | 563 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 564 | \---/ |
salco | 31:ed23647fd16e | 565 | |
salco | 37:f53e9f7a14df | 566 | /?M?\ |
salco | 37:f53e9f7a14df | 567 | |VVV| //vert 1 |
salco | 37:f53e9f7a14df | 568 | |.X.| //latt 2 |
salco | 37:f53e9f7a14df | 569 | |...| // cas impossible |
salco | 37:f53e9f7a14df | 570 | \---/ |
salco | 31:ed23647fd16e | 571 | |
salco | 37:f53e9f7a14df | 572 | /?V?\ |
salco | 37:f53e9f7a14df | 573 | |?V?| //vert 2 |
salco | 37:f53e9f7a14df | 574 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 575 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 576 | \---/ |
salco | 37:f53e9f7a14df | 577 | |
salco | 37:f53e9f7a14df | 578 | /?V?\ |
salco | 37:f53e9f7a14df | 579 | |VVV| //vert 2 |
salco | 37:f53e9f7a14df | 580 | |.X.| //latt 1 |
salco | 37:f53e9f7a14df | 581 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 582 | \---/ |
salco | 31:ed23647fd16e | 583 | |
salco | 37:f53e9f7a14df | 584 | /VVV\ |
salco | 37:f53e9f7a14df | 585 | |VVV| //vert 2 |
salco | 37:f53e9f7a14df | 586 | |.X.| //latt 2 |
salco | 37:f53e9f7a14df | 587 | |...| |
salco | 37:f53e9f7a14df | 588 | \---/ |
salco | 37:f53e9f7a14df | 589 | */ |
salco | 37:f53e9f7a14df | 590 | //vertical==0 latt==0 //mur devant |
salco | 37:f53e9f7a14df | 591 | //vertical==1 latt==1 |
salco | 37:f53e9f7a14df | 592 | //////////////////////////////////// |
salco | 37:f53e9f7a14df | 593 | //345 415 |
salco | 37:f53e9f7a14df | 594 | //012 203| numerotation des case |
salco | 37:f53e9f7a14df | 595 | /////////////////////////////////// |
salco | 37:f53e9f7a14df | 596 | signed char fieldX[6]={0,0,0,0,0,0}; |
salco | 37:f53e9f7a14df | 597 | signed char fieldY[6]={0,0,0,0,0,0}; |
salco | 37:f53e9f7a14df | 598 | bool fieldSet = false; |
salco | 31:ed23647fd16e | 599 | |
salco | 37:f53e9f7a14df | 600 | switch(myMaze->getDirection()) |
salco | 37:f53e9f7a14df | 601 | { |
salco | 37:f53e9f7a14df | 602 | case UP: |
salco | 37:f53e9f7a14df | 603 | fieldX[0] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 604 | fieldY[0] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 605 | fieldX[1] = myMaze->getX(); |
salco | 37:f53e9f7a14df | 606 | fieldY[1] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 607 | fieldX[2] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 608 | fieldY[2] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 609 | fieldX[3] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 610 | fieldY[3] = myMaze->getY()+2; |
salco | 37:f53e9f7a14df | 611 | fieldX[4] = myMaze->getX(); |
salco | 37:f53e9f7a14df | 612 | fieldY[4] = myMaze->getY()+2; |
salco | 37:f53e9f7a14df | 613 | fieldX[5] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 614 | fieldY[5] = myMaze->getY()+2; |
salco | 37:f53e9f7a14df | 615 | break; |
salco | 37:f53e9f7a14df | 616 | |
salco | 37:f53e9f7a14df | 617 | case DOWN: |
salco | 37:f53e9f7a14df | 618 | fieldX[0] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 619 | fieldY[0] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 620 | fieldX[1] = myMaze->getX(); |
salco | 37:f53e9f7a14df | 621 | fieldY[1] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 622 | fieldX[2] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 623 | fieldY[2] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 624 | fieldX[3] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 625 | fieldY[3] = myMaze->getY()-2; |
salco | 37:f53e9f7a14df | 626 | fieldX[4] = myMaze->getX(); |
salco | 37:f53e9f7a14df | 627 | fieldY[4] = myMaze->getY()-2; |
salco | 37:f53e9f7a14df | 628 | fieldX[5] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 629 | fieldY[5] = myMaze->getY()-2; |
salco | 37:f53e9f7a14df | 630 | break; |
salco | 37:f53e9f7a14df | 631 | |
salco | 37:f53e9f7a14df | 632 | case LEFT: |
salco | 37:f53e9f7a14df | 633 | fieldX[0] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 634 | fieldY[0] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 635 | fieldX[1] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 636 | fieldY[1] = myMaze->getY(); |
salco | 37:f53e9f7a14df | 637 | fieldX[2] = myMaze->getX()-1; |
salco | 37:f53e9f7a14df | 638 | fieldY[2] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 639 | fieldX[3] = myMaze->getX()-2; |
salco | 37:f53e9f7a14df | 640 | fieldY[3] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 641 | fieldX[4] = myMaze->getX()-2; |
salco | 37:f53e9f7a14df | 642 | fieldY[4] = myMaze->getY(); |
salco | 37:f53e9f7a14df | 643 | fieldX[5] = myMaze->getX()-2; |
salco | 37:f53e9f7a14df | 644 | fieldY[5] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 645 | break; |
salco | 37:f53e9f7a14df | 646 | |
salco | 37:f53e9f7a14df | 647 | case RIGHT: |
salco | 37:f53e9f7a14df | 648 | fieldX[0] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 649 | fieldY[0] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 650 | fieldX[1] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 651 | fieldY[1] = myMaze->getY(); |
salco | 37:f53e9f7a14df | 652 | fieldX[2] = myMaze->getX()+1; |
salco | 37:f53e9f7a14df | 653 | fieldY[2] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 654 | fieldX[3] = myMaze->getX()+2; |
salco | 37:f53e9f7a14df | 655 | fieldY[3] = myMaze->getY()-1; |
salco | 37:f53e9f7a14df | 656 | fieldX[4] = myMaze->getX()+2; |
salco | 37:f53e9f7a14df | 657 | fieldY[4] = myMaze->getY(); |
salco | 37:f53e9f7a14df | 658 | fieldX[5] = myMaze->getX()+2; |
salco | 37:f53e9f7a14df | 659 | fieldY[5] = myMaze->getY()+1; |
salco | 37:f53e9f7a14df | 660 | break; |
salco | 37:f53e9f7a14df | 661 | } |
salco | 37:f53e9f7a14df | 662 | if(areaVert >= areaLatt) |
salco | 37:f53e9f7a14df | 663 | { |
salco | 37:f53e9f7a14df | 664 | switch(areaVert) |
salco | 37:f53e9f7a14df | 665 | { |
salco | 37:f53e9f7a14df | 666 | case 0: |
salco | 37:f53e9f7a14df | 667 | /* |
salco | 37:f53e9f7a14df | 668 | /---\ |
salco | 37:f53e9f7a14df | 669 | |?M?| //vert 0 |
salco | 37:f53e9f7a14df | 670 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 671 | |...| |
salco | 37:f53e9f7a14df | 672 | \---/ |
salco | 37:f53e9f7a14df | 673 | */ |
salco | 37:f53e9f7a14df | 674 | myMaze->setC(Labyrinthe::mur,fieldX[1],fieldY[1]); |
salco | 37:f53e9f7a14df | 675 | fieldSet = true; |
salco | 28:ac5c6350ed9a | 676 | break; |
salco | 37:f53e9f7a14df | 677 | case 1: |
salco | 37:f53e9f7a14df | 678 | /* |
salco | 37:f53e9f7a14df | 679 | /-M-\ |
salco | 37:f53e9f7a14df | 680 | |?V?| //vert 1 |
salco | 37:f53e9f7a14df | 681 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 682 | |...| |
salco | 37:f53e9f7a14df | 683 | \---/ |
salco | 37:f53e9f7a14df | 684 | */ |
salco | 37:f53e9f7a14df | 685 | myMaze->setC(Labyrinthe::vide,fieldX[1],fieldY[1]); |
salco | 37:f53e9f7a14df | 686 | myMaze->setC(Labyrinthe::mur,fieldX[4],fieldY[4]); |
salco | 37:f53e9f7a14df | 687 | fieldSet = true; |
salco | 37:f53e9f7a14df | 688 | |
salco | 37:f53e9f7a14df | 689 | if(areaLatt == 1) |
salco | 37:f53e9f7a14df | 690 | { |
salco | 37:f53e9f7a14df | 691 | /* |
salco | 37:f53e9f7a14df | 692 | /?M?\ |
salco | 37:f53e9f7a14df | 693 | |VVV| //vert 1 |
salco | 37:f53e9f7a14df | 694 | |.X.| //latt 1 |
salco | 37:f53e9f7a14df | 695 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 696 | \---/ |
salco | 37:f53e9f7a14df | 697 | */ |
salco | 37:f53e9f7a14df | 698 | myMaze->setC(Labyrinthe::vide,fieldX[0],fieldY[0]); |
salco | 37:f53e9f7a14df | 699 | myMaze->setC(Labyrinthe::vide,fieldX[2],fieldY[2]); |
salco | 28:ac5c6350ed9a | 700 | } |
salco | 28:ac5c6350ed9a | 701 | break; |
salco | 37:f53e9f7a14df | 702 | case 2: |
salco | 37:f53e9f7a14df | 703 | fieldSet = true; |
salco | 37:f53e9f7a14df | 704 | switch(areaLatt) |
salco | 37:f53e9f7a14df | 705 | { |
salco | 37:f53e9f7a14df | 706 | case 0: |
salco | 37:f53e9f7a14df | 707 | /* |
salco | 37:f53e9f7a14df | 708 | /?V?\ |
salco | 37:f53e9f7a14df | 709 | |?V?| //vert 2 |
salco | 37:f53e9f7a14df | 710 | |.X.| //latt 0 |
salco | 37:f53e9f7a14df | 711 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 712 | \---/ |
salco | 37:f53e9f7a14df | 713 | */ |
salco | 37:f53e9f7a14df | 714 | myMaze->setC(Labyrinthe::vide,fieldX[1],fieldY[1]); |
salco | 37:f53e9f7a14df | 715 | myMaze->setC(Labyrinthe::vide,fieldX[4],fieldY[4]); |
salco | 37:f53e9f7a14df | 716 | break; |
salco | 37:f53e9f7a14df | 717 | case 1: |
salco | 37:f53e9f7a14df | 718 | /* |
salco | 37:f53e9f7a14df | 719 | /?V?\ |
salco | 37:f53e9f7a14df | 720 | |VVV| //vert 2 |
salco | 37:f53e9f7a14df | 721 | |.X.| //latt 1 |
salco | 37:f53e9f7a14df | 722 | |...| // objet possiblement a proximiter |
salco | 37:f53e9f7a14df | 723 | \---/ |
salco | 37:f53e9f7a14df | 724 | */ |
salco | 37:f53e9f7a14df | 725 | for(int i = 0;i<3;i++)myMaze->setC(Labyrinthe::vide,fieldX[i],fieldY[i]); |
salco | 37:f53e9f7a14df | 726 | myMaze->setC(Labyrinthe::vide,fieldX[4],fieldY[4]); |
salco | 37:f53e9f7a14df | 727 | break; |
salco | 37:f53e9f7a14df | 728 | case 2: |
salco | 37:f53e9f7a14df | 729 | /* |
salco | 37:f53e9f7a14df | 730 | /VVV\ |
salco | 37:f53e9f7a14df | 731 | |VVV| //vert 2 |
salco | 37:f53e9f7a14df | 732 | |.X.| //latt 2 |
salco | 37:f53e9f7a14df | 733 | |...| |
salco | 37:f53e9f7a14df | 734 | \---/ |
salco | 37:f53e9f7a14df | 735 | */ |
salco | 37:f53e9f7a14df | 736 | for(int i = 0;i<6;i++)myMaze->setC( Labyrinthe::vide,fieldX[i],fieldY[i]); |
salco | 37:f53e9f7a14df | 737 | break; |
salco | 37:f53e9f7a14df | 738 | } |
salco | 28:ac5c6350ed9a | 739 | break; |
salco | 37:f53e9f7a14df | 740 | } |
salco | 37:f53e9f7a14df | 741 | } |
salco | 37:f53e9f7a14df | 742 | |
salco | 37:f53e9f7a14df | 743 | if(fieldSet == false)// on a un cas impossible |
salco | 37:f53e9f7a14df | 744 | { |
salco | 37:f53e9f7a14df | 745 | myMaze->setC(Labyrinthe::error,fieldX[1],fieldY[1]); |
salco | 37:f53e9f7a14df | 746 | } |
salco | 31:ed23647fd16e | 747 | |
salco | 37:f53e9f7a14df | 748 | //////////////////////////////////// |
salco | 31:ed23647fd16e | 749 | |
salco | 31:ed23647fd16e | 750 | |
salco | 37:f53e9f7a14df | 751 | #ifdef DEBUG_DIRECTIVE_LABYRINTH_USER_CTRL |
salco | 37:f53e9f7a14df | 752 | mymap = myMaze->showMap(); |
salco | 37:f53e9f7a14df | 753 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+6]); |
salco | 37:f53e9f7a14df | 754 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 755 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i+3]); |
salco | 37:f53e9f7a14df | 756 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 757 | for(int i=0; i<3; i++) debug(DEBUG_DIRECTIVE_LABYRINTH," [%02X] ",mymap[i]); |
salco | 37:f53e9f7a14df | 758 | debug(DEBUG_DIRECTIVE_LABYRINTH,"\n\r"); |
salco | 37:f53e9f7a14df | 759 | |
salco | 37:f53e9f7a14df | 760 | debug("\n\r-------------------\n\r"); |
salco | 37:f53e9f7a14df | 761 | #endif |
salco | 2:4c5b0f6d0a6b | 762 | } |