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:
- 9:5701616e3a2c
- Parent:
- 8:de6ca481b136
--- a/main3.cpp Wed Mar 07 10:24:57 2018 +0000
+++ b/main3.cpp Tue Mar 13 23:19:52 2018 +0000
@@ -3,16 +3,16 @@
#include "MSCFileSystem.h"
m3pi m3pi; // Initialise the m3pi
-
+char *replaceWord(const char *s, const char *oldW, const char *newW);
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");
+
Timer t;
Ticker tick1;
-int cpt=0;
char affichage[3]={0};
char maze[100]={0};
+char *optm;
BusOut myleds(LED1, LED2, LED3, LED4);
#define D_TERM 0.0
@@ -35,9 +35,11 @@
speed=0.3;
char chain[10];
-int v;
-
-unsigned short tabsensor[5];
+
+int j = 0;
+
+
+unsigned short tabsensor[5];
volatile unsigned char sensors;
volatile char flag10ms;
@@ -79,119 +81,121 @@
char result;
current_state();
switch(sensors) {
- case 0x00:
+ case 0x00:
// Deadend
// Back
m3pi.cls();
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 0x1C: case 0x18: case 0x10:
// Forward/Left or Left Only
- // m3pi.stop();
- step();
+ step();
current_state();
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);
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+
result = 3;
} else {
// Forward
m3pi.cls();
strcat(maze,"F");
- m3pi.locate(0,1);
- m3pi.printf(maze);
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+ step();
+
result = 1;
}
break;
case 0x07: case 0x03: case 0x01:
- // Forward/Right or Right Only -> RF
- // m3pi.stop();
+ // Forward/Right or Right Only
m3pi.cls();
strcat(maze,"R");
- //sprintf(affichage,"%d",cpt);
m3pi.locate(0,1);
m3pi.printf(maze);
- //m3pi.print("Right", 6);
+
step();
+
result = 4;
break;
case 0x1F:
- // 'T' or Intersection or End -> LR or RFL
- // m3pi.stop();
+ // 'T' or Intersection or End
step();
current_state();
- // m3pi.print("End", 4);
if (sensors == 0x1F) {
// End
m3pi.cls();
strcat(maze,"E");
- cpt++;
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+
+ m3pi.stop();
+
+ result = 5;
+ } else {
+ // 'T' -> Turn Right
+ m3pi.cls();
+ 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
+ step2();
+ current_state();
+ if (sensors == 0x1F) {
+ // End
+ m3pi.cls();
+ strcat(maze,"E");
m3pi.locate(0,1);
m3pi.printf(maze);
- result = 6;
+
+ m3pi.stop();
+
+ result = 5;
+ } else if ((sensors == 0x10) || (sensors == 0x18)) {
+ // Turn Left
+ m3pi.cls();
+ strcat(maze,"L");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+
+ step2();
+ step2();
+
+ result = 3;
+ } else if ((sensors == 0x14) || (sensors == 0x16) || (sensors == 0x06) || (sensors == 0x1C)) {
+ // Forward
+ m3pi.cls();
+ strcat(maze,"F");
+ m3pi.locate(0,1);
+ m3pi.printf(maze);
+
+ step2();
+ step2();
+
+ result = 1;
} else {
// Turn Right
m3pi.cls();
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;
@@ -217,11 +221,13 @@
// Send command to motors
m3pi.left_motor(left);
m3pi.right_motor(right);
+
result = 1;
break;
default:
// Faire rien
m3pi.stop();
+
result = 0;
break;
}
@@ -244,52 +250,49 @@
switch(command) {
case 2:
// Turn Back
- //m3pi.cls();
- //m3pi.print("Back", 5);
if(sensors != 0x01) {
m3pi.right(speed);
+
result = 2;
} else {
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 {
do{
current_state();
m3pi.left(0.4*speed);
- }while(sensors!=0x04);
- //wait(0.1);
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
m3pi.stop();
+
result = 1;
}
break;
case 4:
// Turn Right
- //m3pi.cls();
- //m3pi.print("Right", 6);
if(sensors != 0x01) {
m3pi.right(speed);
+
result = 4;
} else {
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;
@@ -298,55 +301,18 @@
}
}
-void optimiser(char s[])
-{
- char buffer[3];
- for(int i = 0; i < 1; i++)
- {
-
- }
+char 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;
}
-/* 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;
@@ -356,8 +322,8 @@
// 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]){
+ for (i = 0; s[i] != '\0'; i++) {
+ if (strstr(&s[i], oldW) == &s[i]) {
cnt++;
// Jumping to index after the old word.
@@ -369,9 +335,9 @@
result = (char *)malloc(i + cnt * (newWlen - oldWlen) + 1);
i = 0;
- while (*s){
+ while (*s) {
// compare the substring with the result
- if (strstr(s, oldW) == s){
+ if (strstr(s, oldW) == s) {
strcpy(&result[i], newW);
i += newWlen;
s += oldWlen;
@@ -383,24 +349,11 @@
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 *optimiser(char *str) {
char *buf1 = str;
char *buf2;
- while(find(buf1,'B'))
- {
+ while(find(buf1,'B')) {
buf2 = replaceWord(buf1, "RBR", "F");
buf1 = replaceWord(buf2, "RBL", "B");
buf2 = replaceWord(buf1, "RBF", "L");
@@ -418,18 +371,126 @@
return buf1;
}
-
+
+/* 1 -> PID
+ * 2 -> Turn back
+ * 3 -> Turn left
+ * 4 -> Turn right
+ */
+char labyrinth(char commande) {
+ if(commande==6) {
+ int result = 6;
+
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf(optm);
+ current_state();
+ switch(sensors) {
+ case 0x00: case 0x1C: case 0x18: case 0x10: case 0x07: case 0x03: case 0x1F: case 0x0F: case 0x1E:
+ switch (optm[j]) {
+ case 'B':
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf("B");
+ step();
+ do{
+ m3pi.right(speed);
+ current_state();
+ }while(sensors!=0x01);
+ do{
+ m3pi.right(0.4*speed);
+ current_state();
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
+ m3pi.stop();
+
+ break;
+ case 'R':
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf("R");
+ step();
+ do{
+ m3pi.right(speed);
+ current_state();
+ }while(sensors!=0x01);
+ do{
+ m3pi.right(0.4*speed);
+ current_state();
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
+ //wait(0.1);
+ m3pi.stop();
+
+ break;
+ case 'L':
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf("L");
+ step();
+ do{
+ m3pi.left(speed);
+ current_state();
+ }while(sensors!=0x10);
+ do{
+ m3pi.left(0.4*speed);
+ current_state();
+ }while(sensors!=0x04 && sensors!=0x0E && sensors!=0x0A && sensors!=0x1B && sensors!=0x1F);
+ //wait(0.1);
+ m3pi.stop();
+
+ break;
+ case 'F':
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf("F");
+ step();
+ break;
+ default:
+ result = 0;
+ break;
+ }
+ j++;
+ break;
+ case 0x04: case 0x0C: case 0x06: case 0x0E: case 0x02: case 0x08:
+ //PID
+ // Get the position of the line
+ current_pos_of_line = m3pi.line_position();
+ proportional = current_pos_of_line;
+ // Compute the derivate
+ derivate = current_pos_of_line - previous_pos_of_line;
+ // Compute the integral
+ integral = (integral+I_TERMO*proportional)/(1+I_TERMO);
+ // Remember the last postion
+ previous_pos_of_line = current_pos_of_line;
+ // Compute the power
+ power = (proportional*(P_TERM)) + (integral*(I_TERM)) + (derivate*(D_TERM));
+ // Compute new speeds
+ right = speed-(power*MAX);
+ left = speed+(power*MAX);
+ // Limits checks on motor control
+ right = (right>MAX ? MAX :(right<MIN ? MIN : right));
+ left = (left>MAX ? MAX :(left<MIN ? MIN : left));
+ // Send command to motors
+ m3pi.left_motor(left);
+ m3pi.right_motor(right);
+
+ break;
+ }
+ return result;
+ }
+}
+
+
+
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);
@@ -450,31 +511,21 @@
// 4 -> Right
command = turn(command);
break;
+ case 5:
+ // Optimisation
+ optm = optimiser(maze);
+ wait(5.0);
+ m3pi.cls();
+ m3pi.locate(0, 1);
+ m3pi.printf(optm);
+ command = 6;
+ break;
case 6:
+ // Labyrinth
command = labyrinth(command);
break;
+
}
}
}
-#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
-}
-
+}
\ No newline at end of file