Diff: MotorDriver.cpp

Revision:

15:31d09ee65cf1

Parent:

8:73c8188916dc

diff -r 0caa7b93af7a -r 31d09ee65cf1 MotorDriver.cpp
--- 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();
 }
 
+