Revision 15:31d09ee65cf1, committed 2018-07-04

Comitter:

wengefa1

Date:

Wed Jul 04 09:14:37 2018 +0000

Parent:

14:0caa7b93af7a

Commit message:

Schalter f?r Moduswechsel implementiert

Changed in this revision

--- a/MotorDriver.cpp	Wed May 23 11:50:58 2018 +0000
+++ b/MotorDriver.cpp	Wed Jul 04 09:14:37 2018 +0000
@@ -3,9 +3,6 @@
 #include "ReadSensor.h"
 #include "mbed.h"
 
-#define WheelDiameter 3;
-#define ratio 0.04;
-
 DigitalOut enableMotorDriver(PB_2);
 DigitalIn motorDriverFault(PB_14);
 DigitalIn motorDriverWarning(PB_15);
@@ -24,8 +21,8 @@
 
 void startup (void)
 {
-    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
-    counterLeft.reset();
+    enableMotorDriver = 1; //Schaltet den Motortreiber ein
+    counterLeft.reset();   //Counter reseten
     counterRight.reset();
 }
 
@@ -38,7 +35,7 @@
     float CorrectFactor;
     float Speed = 0.0f;
 
-    switch(SpeedMode) {
+    switch(SpeedMode) {         //Geschwindigkeit einstellen
         case 1:
             Speed = 20.0f;
             break;
@@ -60,14 +57,25 @@
 
     //Fährt bis Anzahl umdrehungen erreicht sind
     while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 990*NumbField) {
+        distance_front = readSensorValue(2);    //Distanz zur vorderen Wand wird gemessen
+        if(readSensorValue(1) < 15.0f)
+        {
         distance_side= readSensorValue(1);      //Distanz zur linken Wand wird gemessen
-        distance_front = readSensorValue(2);    //Distanz zur vorderen Wand wird gemessen
         AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
         distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
-        CorrectFactor = distance_side*1.0f;      //P Faktor des P-Reglers
+        CorrectFactor = distance_side*2.0f;      //P Faktor des P-Reglers
+        }
+        else
+        {
+        distance_side= readSensorValue(3);      //Distanz zur rechten Wand wird gemessen
+        AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
+        distance_side = distance_side - 6.9f;         //Distanz zwischen Position und Ideallinie wird berechnet
+        CorrectFactor = distance_side*-2.0f;      //P Faktor des P-Reglers
+        }
+
 
         //printf("%f\n",CorrectFactor);
-        if(abs(CorrectFactor) <= 5.0f) {    //erkennt Wand oder Lücke auf linker Seite
+        if(abs(CorrectFactor) <= 16.0f) {    //erkennt Wand oder Lücke auf linker Seite
             controller.setDesiredSpeedLeft((Speed) - CorrectFactor); //Bei Wand wird korrigiert
             controller.setDesiredSpeedRight((Speed * (-1.0f)) - CorrectFactor);
         } else {
@@ -83,14 +91,14 @@
         
     }
     stop();
-    //printf("Das Programm wurde: %d durchlaufen\n", AnzahlZyklen);
+
 }
-
-int turnRight(int direction)
+//Rechts abbiegen
+int turnRight(int direction)                
 {
     controller.setDesiredSpeedLeft(40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
     controller.setDesiredSpeedRight(40.0f);
-    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
         //do nothing
     }
     stop();
@@ -101,12 +109,12 @@
     }
     return direction;
 }
-
+//links abbigen
 int turnLeft(int direction)
 {
     controller.setDesiredSpeedRight(-40.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
     controller.setDesiredSpeedLeft(-40.0f);
-    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 580) {
         //do nothing
     }
     stop();
@@ -118,7 +126,7 @@
     return direction;
 }
 
-
+//Roboter anhalten
 void stop(void)
 {
     controller.setDesiredSpeedRight(0.0f);
@@ -149,3 +157,4 @@
     stop();
 }
 
+

--- a/main.cpp	Wed May 23 11:50:58 2018 +0000
+++ b/main.cpp	Wed Jul 04 09:14:37 2018 +0000
@@ -7,32 +7,41 @@
 #include "AutoDrive.h"
 #include "RouteCalculation.h"
 
-
+DigitalIn ModeSwitch (PB_0);
 char* route;
 
 // inizialisieren der Karte
 int map[20][10];
-
+bool mappingpassed = false;
 
 
 
-int main(){
+int main()
+{
     printf("startup");
     startup();
-    wait(5);
-    
-    // Mapping Modus
-    printf("mapping");
-    mapping(map);
-    
-    
-    // Autodrive Modus
-    printf("Route wird berechnet");
-    route = RouteCalculation(map);
-    printf("Berechnung Abgeschlossen");
-    //wait(20);
-    //printf("strecke wird abgefahren");
-    //autoDrive(route);
-    
+    wait(10);
+
+    while(1) {
+        // Mapping Modus
+        if (ModeSwitch == 0) {               //Modus Schalter abfrage
+            wait(0.5);                       //Schalter entprellen
+            if (mappingpassed == false){     //Mapping kann nicht zwei mal nacheinander abgefahren werden, autoDrive muss dazwischen aufgerufen werden
+                printf("mapping");
+                mapping(map);
+                mappingpassed = true;
+            }
+        } else {                             //Modus berechnen und Abfahren 
+            wait(0.5);                       //Schalter entprellen
+            // Autodrive Modus
+            printf("Route wird berechnet");
+            //route = RouteCalculation(map);
+            printf("Berechnung Abgeschlossen");
+            mappingpassed = false; 
+            //wait(20);
+            //printf("strecke wird abgefahren");
+            //autoDrive(route);
+        }
+    }
 }