Diff: Mapping.cpp

Revision:

8:73c8188916dc

Parent:

6:a1fd0f1374e6

Child:

9:5802ccfcc984

diff -r b5bf886ae13c -r 73c8188916dc Mapping.cpp
--- a/Mapping.cpp	Mon Apr 30 13:55:37 2018 +0000
+++ b/Mapping.cpp	Mon May 07 16:30:51 2018 +0000
@@ -22,6 +22,8 @@
 //  int wait(int time); Systemfunktion für Pausen
 
 SDFileSystem sd(PB_5, PB_4, PB_3, PB_10, "sd"); //mosi, miso, sclk, cs
+InterruptIn LineSens1(PC_2);
+InterruptIn LineSens2(PC_3);
 
 void mapping(void){ //------------------------------------------------------------------------------------------------------ Mapping
 
@@ -37,7 +39,7 @@
 }
 }
 
-int speed = 1;
+int speed = 2;
 
 int startrichtung = 4;
 
@@ -87,21 +89,25 @@
     driveOne(1,speed); 
     X = X-2;
         // debug
-    printf("\nIf erster schritt\n");
+    //printf("\nIf erster schritt\n");
 }
 else{
     currentDirection = turnRight(currentDirection);
-    wait(pause);
+    //wait(pause);
     driveOne(1,speed);
     Y = Y-2;
-    printf("\nElse erster schritt\n");
+    //printf("\nElse erster schritt\n");
 }
-
+//Interrupt für Ziellinie
+LineSens1.rise(&setLine1);
+LineSens2.rise(&setLine2);  
+    
 
 // Farhralgorithmus
 
 while (drive == 1){
-//wait(pause);    
+//wait(pause);  
+
     // Variabeln zurücksetzen
      P_straight = 1;
      P_rightTurn = 1;
@@ -147,7 +153,7 @@
         // Kartografierung Feld nach hinten
         map[X][Y] = 1;
             // debug
-    printf("case 1 \n");
+    //printf("case 1 \n");
         break;
 
 // --------------------------------------------------------------------------- 2 Ausrichtung Rechts
@@ -184,7 +190,7 @@
         // Kartografierung Feld nach hinten
         map[X-1][Y] = 1;
             // debug
-    printf("case 2 \n");
+    //printf("case 2 \n");
         break;
 // --------------------------------------------------------------------------- 3 Ausrichtung Unten
         case 3:
@@ -220,7 +226,7 @@
         // Kartografierung Feld nach hinten
         map[X-1][Y-1] = 1;
             // debug
-    printf("case 3: S = %d\n",s);
+    //printf("case 3: S = %d\n",s);
         break;
 // --------------------------------------------------------------------------- 4 Ausrichtung Links
     case 4:
@@ -256,48 +262,51 @@
         // Kartografierung Feld nach hinten
         map[X-1][Y-1] = 1;
          // debug
-    printf("case 4\n");
+    //printf("case 4\n");
 
         break;
-    default: printf("Error: Direction out of definition\n");
+    default: //printf("Error: Direction out of definition\n");
     }
     // debug
     if (P_straight == 0){
-        printf("Geradeaus frei");
+        //printf("Geradeaus frei");
         }
          if (P_rightTurn == 0){
-        printf("Rechts frei");
+        //printf("Rechts frei");
         }
     if (P_leftTurn == 0){
-        printf("Links frei");
+        //printf("Links frei");
         }    
     //printf("Gerade %d\nRechts %d\nLinks %d\n", P_straight, P_rightTurn, P_leftTurn );
       
-    printf("Momentane Richtung = %d\n", currentDirection );
+    //printf("Momentane Richtung = %d\n", currentDirection );
 
 
     // Fahren gem. Rechtsfahralgorithmus
+    if (P_straight ==1){
+        driveOne(1,1);
+        }
 
     if (P_rightTurn == 0) {
         currentDirection = turnRight(currentDirection);
         //wait(pause);
-        printf("Ich biege rechts ab\n");
+        //printf("Ich biege rechts ab\n");
         driveOne(1,speed);
         //myled = 0;
         } else{
             if (P_straight == 0){
                 driveOne(1,speed); 
-                 printf("Ich fahre gerade aus\n");
+                 //printf("Ich fahre gerade aus\n");
                 //myled = 0;
             } else{
                 if(P_leftTurn == 0){
                     currentDirection = turnLeft(currentDirection);
                     //wait(pause);
-                     printf("Ich biege links ab\n");
+                     //printf("Ich biege links ab\n");
                     driveOne(1,speed);
                     //myled = 0;
                     } else{
-                         printf("Ich wende!!!\n");
+                         //printf("Ich wende!!!\n");
                         currentDirection = turnRight(currentDirection);
                         //wait(pause);
                         currentDirection = turnRight(currentDirection);
@@ -308,7 +317,7 @@
             }
         }
         
-       printf("Neue Richtung = %d\n", currentDirection ); 
+       //printf("Neue Richtung = %d\n", currentDirection ); 
     // Aktualisieren der Position:  
     
     switch (currentDirection){
@@ -377,7 +386,7 @@
     int k;
 
     //Perform a write test
-    printf("\nWriting to SD card...");
+    //printf("\nWriting to SD card...");
     FILE *fp = fopen("/sd/map.csv", "w");
     if (fp != NULL) {
         for (i = 0; i < 9; i++){
@@ -386,9 +395,9 @@
             }
             fprintf(fp,"\n\r");
         }
-        printf("Matrix has been sucessfully saved in map.csv\n\r");
+        //printf("Matrix has been sucessfully saved in map.csv\n\r");
     } else {
-        printf("Save-process failed!\n\r");
+        //printf("Save-process failed!\n\r");
     }
 
     //Unmount the filesystem