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@9:5701616e3a2c, 2018-03-13 (annotated)

Committer:
diogohideki
Date:
Tue Mar 13 23:19:52 2018 +0000
Revision:
9:5701616e3a2c
Parent:
8:de6ca481b136 
Final

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