Final version of Polulu M2 SETI

Dependencies:   FileSystem_POPS m3PI_TP_POPS_II2015v0 m3pi mbed

Fork of m3PI_TP_POPS_II2015v0 by Samir Bouaziz

Files at this revision

API Documentation at this revision

Comitter:
diogohideki
Date:
Tue Mar 13 23:19:52 2018 +0000
Parent:
8:de6ca481b136
Commit message:
Final

Changed in this revision

main3.cpp Show annotated file Show diff for this revision Revisions of this file
diff -r de6ca481b136 -r 5701616e3a2c main3.cpp
--- 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