Diogo Kawamoto / Mbed 2 deprecated m3PI_TP_SETI

Final version of Polulu M2 SETI

Dependencies:   FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed

Fork of m3PI_TP_POPS_II2015v0 by Samir Bouaziz

main3.cpp@8:de6ca481b136, 2018-03-07 (annotated)

Committer:
diogohideki
Date:
Wed Mar 07 10:24:57 2018 +0000
Revision:
8:de6ca481b136
Parent:
7:ca53c1bde37c
Child:
9:5701616e3a2c
before change 7 mars;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
bouaziz 0:398afdd73d9e 1 #include "mbed.h"
bouaziz 0:398afdd73d9e 2 #include "m3pi.h"
bouaziz 0:398afdd73d9e 3 #include "MSCFileSystem.h"
bouaziz 0:398afdd73d9e 4
bouaziz 0:398afdd73d9e 5 m3pi m3pi; // Initialise the m3pi
bouaziz 0:398afdd73d9e 6
bouaziz 0:398afdd73d9e 7 Serial xbee(p28,p27);
bouaziz 0:398afdd73d9e 8 DigitalOut resetxbee(p26);
bouaziz 0:398afdd73d9e 9 Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
diogohideki 8:de6ca481b136 10 //MSCFileSystem fs("fs");
diogohideki 4:cdd8e61cd8c7 11 Timer t;
diogohideki 4:cdd8e61cd8c7 12 Ticker tick1;
diogohideki 8:de6ca481b136 13 int cpt=0;
diogohideki 8:de6ca481b136 14 char affichage[3]={0};
diogohideki 8:de6ca481b136 15 char maze[100]={0};
bouaziz 0:398afdd73d9e 16 BusOut myleds(LED1, LED2, LED3, LED4);
bouaziz 0:398afdd73d9e 17
diogohideki 4:cdd8e61cd8c7 18 #define D_TERM 0.0
diogohideki 4:cdd8e61cd8c7 19 #define I_TERM 0.1
diogohideki 4:cdd8e61cd8c7 20 #define I_TERMO 0.1
diogohideki 4:cdd8e61cd8c7 21 #define P_TERM 0.9
diogohideki 4:cdd8e61cd8c7 22 #define MAX 0.3
diogohideki 4:cdd8e61cd8c7 23 #define MIN -0.2
diogohideki 4:cdd8e61cd8c7 24
diogohideki 8:de6ca481b136 25 #define seuil(x) (x>300 ? 1 : 0)
diogohideki 4:cdd8e61cd8c7 26
diogohideki 4:cdd8e61cd8c7 27 float current_pos_of_line,
diogohideki 4:cdd8e61cd8c7 28 previous_pos_of_line,
diogohideki 4:cdd8e61cd8c7 29 derivate,
diogohideki 4:cdd8e61cd8c7 30 proportional,
diogohideki 4:cdd8e61cd8c7 31 power,
diogohideki 4:cdd8e61cd8c7 32 integral,
diogohideki 4:cdd8e61cd8c7 33 left,
diogohideki 4:cdd8e61cd8c7 34 right,
diogohideki 4:cdd8e61cd8c7 35 speed=0.3;
diogohideki 4:cdd8e61cd8c7 36
diogohideki 4:cdd8e61cd8c7 37 char chain[10];
diogohideki 4:cdd8e61cd8c7 38 int v;
diogohideki 8:de6ca481b136 39
diogohideki 8:de6ca481b136 40 unsigned short tabsensor[5];
diogohideki 4:cdd8e61cd8c7 41 volatile unsigned char sensors;
diogohideki 4:cdd8e61cd8c7 42
diogohideki 4:cdd8e61cd8c7 43 volatile char flag10ms;
diogohideki 4:cdd8e61cd8c7 44
diogohideki 4:cdd8e61cd8c7 45 char command=1;
diogohideki 4:cdd8e61cd8c7 46
diogohideki 4:cdd8e61cd8c7 47 void inter1() {
diogohideki 4:cdd8e61cd8c7 48 flag10ms=1;
diogohideki 4:cdd8e61cd8c7 49 }
diogohideki 4:cdd8e61cd8c7 50
diogohideki 4:cdd8e61cd8c7 51 void current_state() {
diogohideki 4:cdd8e61cd8c7 52 unsigned char i;
diogohideki 4:cdd8e61cd8c7 53 sensors=0;
diogohideki 5:bc86a4fb4784 54 m3pi.calibrated_sensors(tabsensor);
diogohideki 4:cdd8e61cd8c7 55 for(i=0; i<5; i++) {
diogohideki 4:cdd8e61cd8c7 56 sensors = (sensors << 1) | seuil(tabsensor[i]);
diogohideki 4:cdd8e61cd8c7 57 }
diogohideki 4:cdd8e61cd8c7 58 }
diogohideki 4:cdd8e61cd8c7 59
diogohideki 4:cdd8e61cd8c7 60 void step() {
diogohideki 7:ca53c1bde37c 61 m3pi.forward(0.12);
diogohideki 8:de6ca481b136 62 wait(0.22);
diogohideki 8:de6ca481b136 63 }
diogohideki 8:de6ca481b136 64
diogohideki 8:de6ca481b136 65 void step2() {
diogohideki 8:de6ca481b136 66 m3pi.forward(0.12);
diogohideki 8:de6ca481b136 67 wait(0.06);
diogohideki 4:cdd8e61cd8c7 68 }
diogohideki 4:cdd8e61cd8c7 69
diogohideki 4:cdd8e61cd8c7 70 /*
diogohideki 4:cdd8e61cd8c7 71 * Results
diogohideki 4:cdd8e61cd8c7 72 * 1 -> PID
diogohideki 4:cdd8e61cd8c7 73 * 2 -> Turn back
diogohideki 4:cdd8e61cd8c7 74 * 3 -> Turn left
diogohideki 4:cdd8e61cd8c7 75 * 4 -> Turn right
diogohideki 4:cdd8e61cd8c7 76 */
diogohideki 4:cdd8e61cd8c7 77 char PIDf(char commande) {
diogohideki 4:cdd8e61cd8c7 78 if(commande==1) {
diogohideki 4:cdd8e61cd8c7 79 char result;
diogohideki 4:cdd8e61cd8c7 80 current_state();
diogohideki 4:cdd8e61cd8c7 81 switch(sensors) {
diogohideki 8:de6ca481b136 82 case 0x00:
diogohideki 4:cdd8e61cd8c7 83 // Deadend
diogohideki 5:bc86a4fb4784 84 // Back
diogohideki 5:bc86a4fb4784 85 m3pi.cls();
diogohideki 8:de6ca481b136 86 strcat(maze,"B");
diogohideki 8:de6ca481b136 87 cpt++;
diogohideki 8:de6ca481b136 88 //sprintf(affichage,"%d",cpt);
diogohideki 8:de6ca481b136 89 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 90 m3pi.printf(maze);
diogohideki 8:de6ca481b136 91 //m3pi.print("Case 7", 7);
diogohideki 4:cdd8e61cd8c7 92 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 93 result = 2;
diogohideki 4:cdd8e61cd8c7 94 break;
diogohideki 8:de6ca481b136 95 case 0x1C: case 0x18: case 0x10:
diogohideki 4:cdd8e61cd8c7 96 // Forward/Left or Left Only
diogohideki 8:de6ca481b136 97 // m3pi.stop();
diogohideki 8:de6ca481b136 98 step();
diogohideki 4:cdd8e61cd8c7 99 current_state();
diogohideki 8:de6ca481b136 100 if ((sensors == 0x00) || (sensors == 0x10)) {
diogohideki 8:de6ca481b136 101 // Turn Left
diogohideki 8:de6ca481b136 102 m3pi.cls();
diogohideki 8:de6ca481b136 103 strcat(maze,"L");
diogohideki 8:de6ca481b136 104 //sprintf(affichage,"%d",cpt);
diogohideki 8:de6ca481b136 105 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 106 m3pi.printf(maze);
diogohideki 8:de6ca481b136 107 //m3pi.print("Left", 5);
diogohideki 8:de6ca481b136 108 result = 3;
diogohideki 8:de6ca481b136 109 } else {
diogohideki 7:ca53c1bde37c 110 // Forward
diogohideki 5:bc86a4fb4784 111 m3pi.cls();
diogohideki 8:de6ca481b136 112 strcat(maze,"F");
diogohideki 8:de6ca481b136 113 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 114 m3pi.printf(maze);
diogohideki 7:ca53c1bde37c 115 result = 1;
diogohideki 4:cdd8e61cd8c7 116 }
diogohideki 4:cdd8e61cd8c7 117 break;
diogohideki 8:de6ca481b136 118 case 0x07: case 0x03: case 0x01:
diogohideki 4:cdd8e61cd8c7 119 // Forward/Right or Right Only -> RF
diogohideki 4:cdd8e61cd8c7 120 // m3pi.stop();
diogohideki 5:bc86a4fb4784 121 m3pi.cls();
diogohideki 8:de6ca481b136 122 strcat(maze,"R");
diogohideki 8:de6ca481b136 123 //sprintf(affichage,"%d",cpt);
diogohideki 8:de6ca481b136 124 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 125 m3pi.printf(maze);
diogohideki 8:de6ca481b136 126 //m3pi.print("Right", 6);
diogohideki 4:cdd8e61cd8c7 127 step();
diogohideki 4:cdd8e61cd8c7 128 result = 4;
diogohideki 4:cdd8e61cd8c7 129 break;
diogohideki 8:de6ca481b136 130 case 0x1F:
diogohideki 4:cdd8e61cd8c7 131 // 'T' or Intersection or End -> LR or RFL
diogohideki 4:cdd8e61cd8c7 132 // m3pi.stop();
diogohideki 4:cdd8e61cd8c7 133 step();
diogohideki 4:cdd8e61cd8c7 134 current_state();
diogohideki 8:de6ca481b136 135 // m3pi.print("End", 4);
diogohideki 4:cdd8e61cd8c7 136 if (sensors == 0x1F) {
diogohideki 4:cdd8e61cd8c7 137 // End
diogohideki 5:bc86a4fb4784 138 m3pi.cls();
diogohideki 8:de6ca481b136 139 strcat(maze,"E");
diogohideki 8:de6ca481b136 140 cpt++;
diogohideki 8:de6ca481b136 141 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 142 m3pi.printf(maze);
diogohideki 8:de6ca481b136 143 result = 6;
diogohideki 4:cdd8e61cd8c7 144 } else {
diogohideki 4:cdd8e61cd8c7 145 // Turn Right
diogohideki 5:bc86a4fb4784 146 m3pi.cls();
diogohideki 8:de6ca481b136 147 strcat(maze, "R");
diogohideki 8:de6ca481b136 148 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 149 m3pi.printf(maze);
diogohideki 8:de6ca481b136 150 result = 4;
diogohideki 8:de6ca481b136 151 }
diogohideki 8:de6ca481b136 152 break;
diogohideki 8:de6ca481b136 153 case 0x0F: case 0x1E:
diogohideki 8:de6ca481b136 154 // 'T' or Intersection or End -> LR or RFL
diogohideki 8:de6ca481b136 155 // m3pi.stop();
diogohideki 8:de6ca481b136 156 step2();
diogohideki 8:de6ca481b136 157 current_state();
diogohideki 8:de6ca481b136 158 // m3pi.print("End", 4);
diogohideki 8:de6ca481b136 159 if (sensors == 0x1F) {
diogohideki 8:de6ca481b136 160 // End
diogohideki 8:de6ca481b136 161 step2();
diogohideki 8:de6ca481b136 162 step2();
diogohideki 8:de6ca481b136 163 m3pi.cls();
diogohideki 8:de6ca481b136 164 strcat(maze,"E");
diogohideki 8:de6ca481b136 165 cpt++;
diogohideki 8:de6ca481b136 166 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 167 m3pi.printf(maze);
diogohideki 8:de6ca481b136 168 result = 0;
diogohideki 8:de6ca481b136 169 } else if ((sensors == 0x10) || (sensors == 0x18)) {
diogohideki 8:de6ca481b136 170 // Turn Left
diogohideki 8:de6ca481b136 171 step2();
diogohideki 8:de6ca481b136 172 step2();
diogohideki 8:de6ca481b136 173 m3pi.cls();
diogohideki 8:de6ca481b136 174 strcat(maze,"L");
diogohideki 8:de6ca481b136 175 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 176 m3pi.printf(maze);
diogohideki 8:de6ca481b136 177 result = 3;
diogohideki 8:de6ca481b136 178 } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) {
diogohideki 8:de6ca481b136 179 // Forward
diogohideki 8:de6ca481b136 180 step2();
diogohideki 8:de6ca481b136 181 step2();
diogohideki 8:de6ca481b136 182 m3pi.cls();
diogohideki 8:de6ca481b136 183 strcat(maze,"F");
diogohideki 8:de6ca481b136 184 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 185 m3pi.printf(maze);
diogohideki 8:de6ca481b136 186 result = 1;
diogohideki 8:de6ca481b136 187 } else {
diogohideki 8:de6ca481b136 188 // Turn Right
diogohideki 8:de6ca481b136 189 step2();
diogohideki 8:de6ca481b136 190 step2();
diogohideki 8:de6ca481b136 191 m3pi.cls();
diogohideki 8:de6ca481b136 192 strcat(maze, "R");
diogohideki 8:de6ca481b136 193 m3pi.locate(0,1);
diogohideki 8:de6ca481b136 194 m3pi.printf(maze);
diogohideki 4:cdd8e61cd8c7 195 result = 4;
diogohideki 4:cdd8e61cd8c7 196 }
diogohideki 4:cdd8e61cd8c7 197 break;
diogohideki 4:cdd8e61cd8c7 198 case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
diogohideki 4:cdd8e61cd8c7 199 //PID
diogohideki 4:cdd8e61cd8c7 200 // Get the position of the line
diogohideki 4:cdd8e61cd8c7 201 current_pos_of_line = m3pi.line_position();
diogohideki 4:cdd8e61cd8c7 202 proportional = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 203 // Compute the derivate
diogohideki 4:cdd8e61cd8c7 204 derivate = current_pos_of_line - previous_pos_of_line;
diogohideki 4:cdd8e61cd8c7 205 // Compute the integral
diogohideki 4:cdd8e61cd8c7 206 integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
diogohideki 4:cdd8e61cd8c7 207 // Remember the last postion
diogohideki 4:cdd8e61cd8c7 208 previous_pos_of_line = current_pos_of_line;
diogohideki 4:cdd8e61cd8c7 209 // Compute the power
diogohideki 4:cdd8e61cd8c7 210 power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
diogohideki 4:cdd8e61cd8c7 211 // Compute new speeds
diogohideki 4:cdd8e61cd8c7 212 right = speed-(power*MAX);
diogohideki 4:cdd8e61cd8c7 213 left = speed+(power*MAX);
diogohideki 4:cdd8e61cd8c7 214 // Limits checks on motor control
diogohideki 4:cdd8e61cd8c7 215 right = (right>MAX ? MAX :(right<MIN ? MIN : right));
diogohideki 4:cdd8e61cd8c7 216 left = (left>MAX ? MAX :(left<MIN ? MIN : left));
diogohideki 4:cdd8e61cd8c7 217 // Send command to motors
diogohideki 4:cdd8e61cd8c7 218 m3pi.left_motor(left);
diogohideki 4:cdd8e61cd8c7 219 m3pi.right_motor(right);
diogohideki 4:cdd8e61cd8c7 220 result = 1;
diogohideki 4:cdd8e61cd8c7 221 break;
diogohideki 4:cdd8e61cd8c7 222 default:
diogohideki 4:cdd8e61cd8c7 223 // Faire rien
diogohideki 4:cdd8e61cd8c7 224 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 225 result = 0;
diogohideki 4:cdd8e61cd8c7 226 break;
diogohideki 4:cdd8e61cd8c7 227 }
diogohideki 8:de6ca481b136 228 return result;
diogohideki 4:cdd8e61cd8c7 229 }
diogohideki 4:cdd8e61cd8c7 230 }
diogohideki 4:cdd8e61cd8c7 231
diogohideki 4:cdd8e61cd8c7 232
diogohideki 4:cdd8e61cd8c7 233 /*
diogohideki 4:cdd8e61cd8c7 234 * Results
diogohideki 4:cdd8e61cd8c7 235 * 1 -> PID
diogohideki 4:cdd8e61cd8c7 236 * 2 -> Turn back
diogohideki 4:cdd8e61cd8c7 237 * 3 -> Turn left
diogohideki 4:cdd8e61cd8c7 238 * 4 -> Turn right
diogohideki 4:cdd8e61cd8c7 239 */
diogohideki 4:cdd8e61cd8c7 240 char turn(char command) {
diogohideki 4:cdd8e61cd8c7 241 if(command > 1 && command < 5) {
diogohideki 4:cdd8e61cd8c7 242 char result;
diogohideki 4:cdd8e61cd8c7 243 current_state();
diogohideki 4:cdd8e61cd8c7 244 switch(command) {
diogohideki 4:cdd8e61cd8c7 245 case 2:
diogohideki 4:cdd8e61cd8c7 246 // Turn Back
diogohideki 8:de6ca481b136 247 //m3pi.cls();
diogohideki 8:de6ca481b136 248 //m3pi.print("Back", 5);
diogohideki 4:cdd8e61cd8c7 249 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 250 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 251 result = 2;
diogohideki 4:cdd8e61cd8c7 252 } else {
diogohideki 8:de6ca481b136 253 do{
diogohideki 8:de6ca481b136 254 current_state();
diogohideki 8:de6ca481b136 255 m3pi.right(0.4*speed);
diogohideki 8:de6ca481b136 256 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
diogohideki 8:de6ca481b136 257 //wait(0.12);
diogohideki 4:cdd8e61cd8c7 258 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 259 result = 1;
diogohideki 4:cdd8e61cd8c7 260 }
diogohideki 4:cdd8e61cd8c7 261 break;
diogohideki 4:cdd8e61cd8c7 262 case 3:
diogohideki 4:cdd8e61cd8c7 263 // Turn Left
diogohideki 8:de6ca481b136 264 //m3pi.cls();
diogohideki 8:de6ca481b136 265 //m3pi.print("Left", 5);
diogohideki 4:cdd8e61cd8c7 266 if(sensors != 0x10) {
diogohideki 4:cdd8e61cd8c7 267 m3pi.left(speed);
diogohideki 4:cdd8e61cd8c7 268 result = 3;
diogohideki 4:cdd8e61cd8c7 269 } else {
diogohideki 8:de6ca481b136 270 do{
diogohideki 8:de6ca481b136 271 current_state();
diogohideki 8:de6ca481b136 272 m3pi.left(0.4*speed);
diogohideki 8:de6ca481b136 273 }while(sensors!=0x04);
diogohideki 8:de6ca481b136 274 //wait(0.1);
diogohideki 4:cdd8e61cd8c7 275 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 276 result = 1;
diogohideki 4:cdd8e61cd8c7 277 }
diogohideki 4:cdd8e61cd8c7 278 break;
diogohideki 4:cdd8e61cd8c7 279 case 4:
diogohideki 4:cdd8e61cd8c7 280 // Turn Right
diogohideki 8:de6ca481b136 281 //m3pi.cls();
diogohideki 8:de6ca481b136 282 //m3pi.print("Right", 6);
diogohideki 4:cdd8e61cd8c7 283 if(sensors != 0x01) {
diogohideki 4:cdd8e61cd8c7 284 m3pi.right(speed);
diogohideki 4:cdd8e61cd8c7 285 result = 4;
diogohideki 4:cdd8e61cd8c7 286 } else {
diogohideki 8:de6ca481b136 287 do{
diogohideki 8:de6ca481b136 288 current_state();
diogohideki 8:de6ca481b136 289 m3pi.right(0.4*speed);
diogohideki 8:de6ca481b136 290 }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
diogohideki 8:de6ca481b136 291 //wait(0.1);
diogohideki 4:cdd8e61cd8c7 292 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 293 result = 1;
diogohideki 4:cdd8e61cd8c7 294 }
diogohideki 8:de6ca481b136 295 break;
diogohideki 4:cdd8e61cd8c7 296 }
diogohideki 4:cdd8e61cd8c7 297 return result;
diogohideki 4:cdd8e61cd8c7 298 }
diogohideki 4:cdd8e61cd8c7 299 }
diogohideki 4:cdd8e61cd8c7 300
diogohideki 8:de6ca481b136 301 void optimiser(char s[])
diogohideki 8:de6ca481b136 302 {
diogohideki 8:de6ca481b136 303 char buffer[3];
diogohideki 8:de6ca481b136 304 for(int i = 0; i < 1; i++)
diogohideki 8:de6ca481b136 305 {
diogohideki 8:de6ca481b136 306
diogohideki 8:de6ca481b136 307 }
diogohideki 8:de6ca481b136 308 }
diogohideki 8:de6ca481b136 309
diogohideki 8:de6ca481b136 310 /* 1 -> PID
diogohideki 8:de6ca481b136 311 * 2 -> Turn back
diogohideki 8:de6ca481b136 312 * 3 -> Turn left
diogohideki 8:de6ca481b136 313 * 4 -> Turn right
diogohideki 8:de6ca481b136 314 */
diogohideki 8:de6ca481b136 315 char labyrinth(char result)
diogohideki 8:de6ca481b136 316 {
diogohideki 8:de6ca481b136 317 if(result==6)
diogohideki 8:de6ca481b136 318 {
diogohideki 8:de6ca481b136 319 char s[100];
diogohideki 8:de6ca481b136 320 int j=0;
diogohideki 8:de6ca481b136 321 strcpy(s,maze);
diogohideki 8:de6ca481b136 322 optimiser(s);
diogohideki 8:de6ca481b136 323 current_state();
diogohideki 8:de6ca481b136 324 if((sensors == 0x00) ||(sensors == 0x1C) ||(sensors == 0x18) ||(sensors == 0x10) ||(sensors == 0x07) ||(sensors == 0x03) ||(sensors == 0x1F) ||(sensors == 0x0F) ||(sensors == 0x1E) )
diogohideki 8:de6ca481b136 325 {
diogohideki 8:de6ca481b136 326 switch (s[j])
diogohideki 8:de6ca481b136 327 {
diogohideki 8:de6ca481b136 328 case 'R':
diogohideki 8:de6ca481b136 329 result=4;
diogohideki 8:de6ca481b136 330 break;
diogohideki 8:de6ca481b136 331 case 'B':
diogohideki 8:de6ca481b136 332 result=2;
diogohideki 8:de6ca481b136 333 break;
diogohideki 8:de6ca481b136 334 case 'L':
diogohideki 8:de6ca481b136 335 result=3;
diogohideki 8:de6ca481b136 336 break;
diogohideki 8:de6ca481b136 337 case 'F':
diogohideki 8:de6ca481b136 338 result=1;
diogohideki 8:de6ca481b136 339 break;
diogohideki 8:de6ca481b136 340 default:
diogohideki 8:de6ca481b136 341 result=0;
diogohideki 8:de6ca481b136 342 break;
diogohideki 8:de6ca481b136 343 }
diogohideki 8:de6ca481b136 344 j++;
diogohideki 8:de6ca481b136 345 }
diogohideki 8:de6ca481b136 346
diogohideki 8:de6ca481b136 347 }
diogohideki 8:de6ca481b136 348 return result;
diogohideki 8:de6ca481b136 349 }
diogohideki 8:de6ca481b136 350
diogohideki 8:de6ca481b136 351 char *replaceWord(const char *s, const char *oldW, const char *newW) {
diogohideki 8:de6ca481b136 352 char *result;
diogohideki 8:de6ca481b136 353 int i, cnt = 0;
diogohideki 8:de6ca481b136 354 int newWlen = strlen(newW);
diogohideki 8:de6ca481b136 355 int oldWlen = strlen(oldW);
diogohideki 8:de6ca481b136 356
diogohideki 8:de6ca481b136 357 // Counting the number of times old word
diogohideki 8:de6ca481b136 358 // occur in the string
diogohideki 8:de6ca481b136 359 for (i = 0; s[i] != '\0'; i++){
diogohideki 8:de6ca481b136 360 if (strstr(&s[i], oldW) == &s[i]){
diogohideki 8:de6ca481b136 361 cnt++;
diogohideki 8:de6ca481b136 362
diogohideki 8:de6ca481b136 363 // Jumping to index after the old word.
diogohideki 8:de6ca481b136 364 i += oldWlen - 1;
diogohideki 8:de6ca481b136 365 }
diogohideki 8:de6ca481b136 366 }
diogohideki 8:de6ca481b136 367
diogohideki 8:de6ca481b136 368 // Making new string of enough length
diogohideki 8:de6ca481b136 369 result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1);
diogohideki 8:de6ca481b136 370
diogohideki 8:de6ca481b136 371 i = 0;
diogohideki 8:de6ca481b136 372 while (*s){
diogohideki 8:de6ca481b136 373 // compare the substring with the result
diogohideki 8:de6ca481b136 374 if (strstr(s, oldW) == s){
diogohideki 8:de6ca481b136 375 strcpy(&result[i], newW);
diogohideki 8:de6ca481b136 376 i += newWlen;
diogohideki 8:de6ca481b136 377 s += oldWlen;
diogohideki 8:de6ca481b136 378 } else
diogohideki 8:de6ca481b136 379 result[i++] = *s++;
diogohideki 8:de6ca481b136 380 }
diogohideki 8:de6ca481b136 381
diogohideki 8:de6ca481b136 382 result[i] = '\0';
diogohideki 8:de6ca481b136 383 return result;
diogohideki 8:de6ca481b136 384 }
diogohideki 8:de6ca481b136 385
diogohideki 8:de6ca481b136 386 int find(char str[], char c) {
diogohideki 8:de6ca481b136 387 unsigned char len = strlen(str);
diogohideki 8:de6ca481b136 388 int i;
diogohideki 8:de6ca481b136 389
diogohideki 8:de6ca481b136 390 for(i = 0; i < len; i++){
diogohideki 8:de6ca481b136 391 if(str[i] == c)
diogohideki 8:de6ca481b136 392 return 1;
diogohideki 8:de6ca481b136 393 }
diogohideki 8:de6ca481b136 394
diogohideki 8:de6ca481b136 395 return 0;
diogohideki 8:de6ca481b136 396 }
diogohideki 8:de6ca481b136 397
diogohideki 8:de6ca481b136 398 char *optimisation(char *str) {
diogohideki 8:de6ca481b136 399 char *buf1 = str;
diogohideki 8:de6ca481b136 400 char *buf2;
diogohideki 8:de6ca481b136 401
diogohideki 8:de6ca481b136 402 while(find(buf1,'B'))
diogohideki 8:de6ca481b136 403 {
diogohideki 8:de6ca481b136 404 buf2 = replaceWord(buf1, "RBR", "F");
diogohideki 8:de6ca481b136 405 buf1 = replaceWord(buf2, "RBL", "B");
diogohideki 8:de6ca481b136 406 buf2 = replaceWord(buf1, "RBF", "L");
diogohideki 8:de6ca481b136 407
diogohideki 8:de6ca481b136 408 buf1 = replaceWord(buf2, "LBR", "B");
diogohideki 8:de6ca481b136 409 buf2 = replaceWord(buf1, "LBL", "F");
diogohideki 8:de6ca481b136 410 buf1 = replaceWord(buf2, "LBF", "R");
diogohideki 8:de6ca481b136 411
diogohideki 8:de6ca481b136 412 buf2 = replaceWord(buf1, "FBR", "L");
diogohideki 8:de6ca481b136 413 buf1 = replaceWord(buf2, "FBL", "R");
diogohideki 8:de6ca481b136 414 buf2 = replaceWord(buf1, "FBF", "B");
diogohideki 8:de6ca481b136 415
diogohideki 8:de6ca481b136 416 buf1 = buf2;
diogohideki 8:de6ca481b136 417 }
diogohideki 8:de6ca481b136 418
diogohideki 8:de6ca481b136 419 return buf1;
diogohideki 8:de6ca481b136 420 }
diogohideki 8:de6ca481b136 421
bouaziz 0:398afdd73d9e 422 int main() {
diogohideki 8:de6ca481b136 423 #if 1
diogohideki 4:cdd8e61cd8c7 424 resetxbee=0;
bouaziz 0:398afdd73d9e 425 wait(0.01);
bouaziz 0:398afdd73d9e 426 resetxbee =1;
diogohideki 8:de6ca481b136 427
diogohideki 4:cdd8e61cd8c7 428 // FILE *p= fopen("/fs/tt.txt","a+");
bouaziz 0:398afdd73d9e 429 m3pi.sensor_auto_calibrate();
bouaziz 0:398afdd73d9e 430 wait(1.);
diogohideki 4:cdd8e61cd8c7 431 tick1.attach(&inter1,0.01);
diogohideki 8:de6ca481b136 432
diogohideki 4:cdd8e61cd8c7 433 // fprintf(p,"ecrire dans la cle USB\r\n");
diogohideki 4:cdd8e61cd8c7 434 // fclose(p);
diogohideki 4:cdd8e61cd8c7 435
bouaziz 0:398afdd73d9e 436 while(1) {
diogohideki 4:cdd8e61cd8c7 437 if(flag10ms==1) {
diogohideki 4:cdd8e61cd8c7 438 switch(command) {
diogohideki 4:cdd8e61cd8c7 439 case 0:
diogohideki 4:cdd8e61cd8c7 440 // Faire Rien
diogohideki 4:cdd8e61cd8c7 441 m3pi.stop();
diogohideki 4:cdd8e61cd8c7 442 break;
diogohideki 4:cdd8e61cd8c7 443 case 1:
diogohideki 4:cdd8e61cd8c7 444 // PID
diogohideki 4:cdd8e61cd8c7 445 command = PIDf(command);
diogohideki 4:cdd8e61cd8c7 446 break;
diogohideki 4:cdd8e61cd8c7 447 case 2: case 3: case 4:
diogohideki 4:cdd8e61cd8c7 448 // 2 -> Back
diogohideki 4:cdd8e61cd8c7 449 // 3 -> Left
diogohideki 4:cdd8e61cd8c7 450 // 4 -> Right
diogohideki 4:cdd8e61cd8c7 451 command = turn(command);
diogohideki 4:cdd8e61cd8c7 452 break;
diogohideki 8:de6ca481b136 453 case 6:
diogohideki 8:de6ca481b136 454 command = labyrinth(command);
diogohideki 8:de6ca481b136 455 break;
diogohideki 8:de6ca481b136 456 }
diogohideki 4:cdd8e61cd8c7 457 }
bouaziz 0:398afdd73d9e 458 }
diogohideki 8:de6ca481b136 459 #else
diogohideki 8:de6ca481b136 460 char str[] = "xxforxx";
diogohideki 8:de6ca481b136 461 char c[] = "xx";
diogohideki 8:de6ca481b136 462 char d[] = "y";
diogohideki 8:de6ca481b136 463
diogohideki 8:de6ca481b136 464 char *result = NULL;
diogohideki 8:de6ca481b136 465
diogohideki 8:de6ca481b136 466 // oldW string
diogohideki 8:de6ca481b136 467 m3pi.cls();
diogohideki 8:de6ca481b136 468 m3pi.printf("Old: %sn", str);
diogohideki 8:de6ca481b136 469
diogohideki 8:de6ca481b136 470 result = replaceWord(str, c, d);
diogohideki 8:de6ca481b136 471 m3pi.cls();
diogohideki 8:de6ca481b136 472 m3pi.printf("%s", result);
diogohideki 8:de6ca481b136 473
diogohideki 8:de6ca481b136 474 free(result);
diogohideki 8:de6ca481b136 475 return 0;
diogohideki 8:de6ca481b136 476
diogohideki 8:de6ca481b136 477
diogohideki 8:de6ca481b136 478 #endif
diogohideki 8:de6ca481b136 479 }
diogohideki 8:de6ca481b136 480