Diogo Kawamoto
Final version of Polulu M2 SETI
Dependencies: FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed
Fork of
m3PI_TP_POPS_II2015v0
by 
Samir Bouaziz
Diff: main3.cpp
- Revision:
- 8:de6ca481b136
- Parent:
- 7:ca53c1bde37c
- Child:
- 9:5701616e3a2c
diff -r ca53c1bde37c -r de6ca481b136 main3.cpp
--- a/main3.cpp Tue Mar 06 08:35:43 2018 +0000
+++ b/main3.cpp Wed Mar 07 10:24:57 2018 +0000
@@ -7,10 +7,12 @@
Serial xbee(p28,p27);
DigitalOut resetxbee(p26);
Serial pc(USBTX, USBRX); // For debugging and pc messages, uses commented out to prevent hanging
-//MSCFileSystem fs("fs");
+//MSCFileSystem fs("fs");
Timer t;
Ticker tick1;
-
+int cpt=0;
+char affichage[3]={0};
+char maze[100]={0};
BusOut myleds(LED1, LED2, LED3, LED4);
#define D_TERM 0.0
@@ -20,7 +22,7 @@
#define MAX 0.3
#define MIN -0.2
-#define seuil(x) (x>400 ? 1 : 0)
+#define seuil(x) (x>300 ? 1 : 0)
float current_pos_of_line,
previous_pos_of_line,
@@ -34,8 +36,8 @@
char chain[10];
int v;
-
-unsigned short tabsensor[5];
+
+unsigned short tabsensor[5];
volatile unsigned char sensors;
volatile char flag10ms;
@@ -57,8 +59,12 @@
void step() {
m3pi.forward(0.12);
- wait(0.18);
- // m3pi.stop();
+ wait(0.22);
+}
+
+void step2() {
+ m3pi.forward(0.12);
+ wait(0.06);
}
/*
@@ -73,62 +79,119 @@
char result;
current_state();
switch(sensors) {
- case 0x00:
+ case 0x00:
// Deadend
// Back
m3pi.cls();
- m3pi.print("Back", 5);
+ strcat(maze,"B");
+ cpt++;
+ //sprintf(affichage,"%d",cpt);
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ //m3pi.print("Case 7", 7);
m3pi.stop();
result = 2;
break;
- case 0x18: case 0x1C: case 0x1E:
+ case 0x1C: case 0x18: case 0x10:
// Forward/Left or Left Only
- step();
+ // m3pi.stop();
+ step();
current_state();
- if ((sensors == 0x04) || (sensors == 0x06) || (sensors == 0x0C) || (sensors == 0x14) || (sensors == 0x16) || (sensors == 0x07)) {
+ if ((sensors == 0x00) || (sensors == 0x10)) {
+ // Turn Left
+ m3pi.cls();
+ strcat(maze,"L");
+ //sprintf(affichage,"%d",cpt);
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ //m3pi.print("Left", 5);
+ result = 3;
+ } else {
// Forward
m3pi.cls();
- m3pi.print("Forw2", 5);
+ strcat(maze,"F");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
result = 1;
- } else if (sensors == 0x1F) {
- // End
- m3pi.cls();
- m3pi.print("The End2", 8);
- result = 0;
- } else if((sensors == 0x00) || (sensors == 0x10) || (sensors == 0x18)) {
- // Turn Left
- m3pi.cls();
- m3pi.print("Left", 5);
- result = 3;
- } else {
- // Turn Right
- m3pi.cls();
- m3pi.print("Right2", 6);
- result = 4;
}
break;
- case 0x07: case 0x03:
+ case 0x07: case 0x03: case 0x01:
// Forward/Right or Right Only -> RF
// m3pi.stop();
m3pi.cls();
- m3pi.print("Right", 6);
+ strcat(maze,"R");
+ //sprintf(affichage,"%d",cpt);
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ //m3pi.print("Right", 6);
step();
result = 4;
break;
- case 0x1F: case 0x0F:
+ case 0x1F:
// 'T' or Intersection or End -> LR or RFL
// m3pi.stop();
step();
current_state();
+ // m3pi.print("End", 4);
if (sensors == 0x1F) {
// End
m3pi.cls();
- m3pi.print("The End", 8);
- result = 0;
+ strcat(maze,"E");
+ cpt++;
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ result = 6;
} else {
// Turn Right
m3pi.cls();
- m3pi.print("Inter", 6);
+ strcat(maze, "R");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ result = 4;
+ }
+ break;
+ case 0x0F: case 0x1E:
+ // 'T' or Intersection or End -> LR or RFL
+ // m3pi.stop();
+ step2();
+ current_state();
+ // m3pi.print("End", 4);
+ if (sensors == 0x1F) {
+ // End
+ step2();
+ step2();
+ m3pi.cls();
+ strcat(maze,"E");
+ cpt++;
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ result = 0;
+ } else if ((sensors == 0x10) || (sensors == 0x18)) {
+ // Turn Left
+ step2();
+ step2();
+ m3pi.cls();
+ strcat(maze,"L");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ result = 3;
+ } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) {
+ // Forward
+ step2();
+ step2();
+ m3pi.cls();
+ strcat(maze,"F");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ result = 1;
+ } else {
+ // Turn Right
+ step2();
+ step2();
+ m3pi.cls();
+ strcat(maze, "R");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
result = 4;
}
break;
@@ -162,7 +225,7 @@
result = 0;
break;
}
- return result;
+ return result;
}
}
@@ -181,56 +244,192 @@
switch(command) {
case 2:
// Turn Back
+ //m3pi.cls();
+ //m3pi.print("Back", 5);
if(sensors != 0x01) {
m3pi.right(speed);
result = 2;
} else {
- m3pi.right(0.12);
- wait(0.12);
+ do{
+ current_state();
+ m3pi.right(0.4*speed);
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
+ //wait(0.12);
m3pi.stop();
result = 1;
}
break;
case 3:
// Turn Left
+ //m3pi.cls();
+ //m3pi.print("Left", 5);
if(sensors != 0x10) {
m3pi.left(speed);
result = 3;
} else {
- m3pi.left(0.12);
- wait(0.1);
+ do{
+ current_state();
+ m3pi.left(0.4*speed);
+ }while(sensors!=0x04);
+ //wait(0.1);
m3pi.stop();
result = 1;
}
break;
case 4:
// Turn Right
+ //m3pi.cls();
+ //m3pi.print("Right", 6);
if(sensors != 0x01) {
m3pi.right(speed);
result = 4;
} else {
- m3pi.right(0.12);
- wait(0.1);
+ do{
+ current_state();
+ m3pi.right(0.4*speed);
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
+ //wait(0.1);
m3pi.stop();
result = 1;
}
- break;
+ break;
}
return result;
}
}
-
+void optimiser(char s[])
+{
+ char buffer[3];
+ for(int i = 0; i < 1; i++)
+ {
+
+ }
+}
+
+/* 1 -> PID
+ * 2 -> Turn back
+ * 3 -> Turn left
+ * 4 -> Turn right
+ */
+char labyrinth(char result)
+{
+ if(result==6)
+ {
+ char s[100];
+ int j=0;
+ strcpy(s,maze);
+ optimiser(s);
+ current_state();
+ if((sensors == 0x00) ||(sensors == 0x1C) ||(sensors == 0x18) ||(sensors == 0x10) ||(sensors == 0x07) ||(sensors == 0x03) ||(sensors == 0x1F) ||(sensors == 0x0F) ||(sensors == 0x1E) )
+ {
+ switch (s[j])
+ {
+ case 'R':
+ result=4;
+ break;
+ case 'B':
+ result=2;
+ break;
+ case 'L':
+ result=3;
+ break;
+ case 'F':
+ result=1;
+ break;
+ default:
+ result=0;
+ break;
+ }
+ j++;
+ }
+
+ }
+ return result;
+}
+
+char *replaceWord(const char *s, const char *oldW, const char *newW) {
+ char *result;
+ int i, cnt = 0;
+ int newWlen = strlen(newW);
+ int oldWlen = strlen(oldW);
+
+ // Counting the number of times old word
+ // occur in the string
+ for (i = 0; s[i] != '\0'; i++){
+ if (strstr(&s[i], oldW) == &s[i]){
+ cnt++;
+
+ // Jumping to index after the old word.
+ i += oldWlen - 1;
+ }
+ }
+
+ // Making new string of enough length
+ result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1);
+
+ i = 0;
+ while (*s){
+ // compare the substring with the result
+ if (strstr(s, oldW) == s){
+ strcpy(&result[i], newW);
+ i += newWlen;
+ s += oldWlen;
+ } else
+ result[i++] = *s++;
+ }
+
+ result[i] = '\0';
+ return result;
+}
+
+int find(char str[], char c) {
+ unsigned char len = strlen(str);
+ int i;
+
+ for(i = 0; i < len; i++){
+ if(str[i] == c)
+ return 1;
+ }
+
+ return 0;
+}
+
+char *optimisation(char *str) {
+ char *buf1 = str;
+ char *buf2;
+
+ while(find(buf1,'B'))
+ {
+ buf2 = replaceWord(buf1, "RBR", "F");
+ buf1 = replaceWord(buf2, "RBL", "B");
+ buf2 = replaceWord(buf1, "RBF", "L");
+
+ buf1 = replaceWord(buf2, "LBR", "B");
+ buf2 = replaceWord(buf1, "LBL", "F");
+ buf1 = replaceWord(buf2, "LBF", "R");
+
+ buf2 = replaceWord(buf1, "FBR", "L");
+ buf1 = replaceWord(buf2, "FBL", "R");
+ buf2 = replaceWord(buf1, "FBF", "B");
+
+ buf1 = buf2;
+ }
+
+ return buf1;
+}
+
int main() {
+#if 1
resetxbee=0;
wait(0.01);
resetxbee =1;
-
+
// FILE *p= fopen("/fs/tt.txt","a+");
m3pi.sensor_auto_calibrate();
wait(1.);
tick1.attach(&inter1,0.01);
-
+
// fprintf(p,"ecrire dans la cle USB\r\n");
// fclose(p);
@@ -251,7 +450,31 @@
// 4 -> Right
command = turn(command);
break;
- }
+ case 6:
+ command = labyrinth(command);
+ break;
+ }
}
}
-}
\ No newline at end of file
+#else
+ char str[] = "xxforxx";
+ char c[] = "xx";
+ char d[] = "y";
+
+ char *result = NULL;
+
+ // oldW string
+ m3pi.cls();
+ m3pi.printf("Old: %sn", str);
+
+ result = replaceWord(str, c, d);
+ m3pi.cls();
+ m3pi.printf("%s", result);
+
+ free(result);
+ return 0;
+
+
+#endif
+}
+