Diff: MotorDriver.cpp

Revision:

1:056cd61800e9

Child:

7:b5bf886ae13c

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver.cpp	Fri Apr 27 12:33:54 2018 +0000
@@ -0,0 +1,151 @@
+#include "MotorDriver.h"
+#include "Controller.h"
+#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);
+
+PwmOut pwmLeft(PA_8);
+PwmOut pwmRight(PA_9);
+
+EncoderCounter counterLeft(PB_6, PB_7);
+EncoderCounter counterRight(PA_6, PC_7);
+
+DigitalOut myled(LED2);
+
+Controller controller(pwmLeft, pwmRight, counterLeft, counterRight);
+
+void resetCounter(void);
+
+void startup (void)
+{
+    enableMotorDriver = 1; // Schaltet den Leistungstreiber ein
+    counterLeft.reset();
+    counterRight.reset();
+}
+
+void driveOne(int NumbField, int SpeedMode)
+
+{
+    int AnzahlZyklen = 0;
+    float distance_side;
+    float distance_front;
+    float CorrectFactor;
+    float Speed = 0.0f;
+
+    switch(SpeedMode) {
+        case 1:
+            Speed = 20.0f;
+            break;
+
+
+        case 2:
+            Speed = 40.0f;
+            break;
+
+        case 3:
+            Speed = 80.0f;
+            break;
+
+
+        default:
+
+            break;
+    }
+
+    //Fährt bis Anzahl umdrehungen erreicht sind
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+        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 - 7.8f;         //Distanz zwischen Position und Ideallinie wird berechnet
+        CorrectFactor = distance_side*0.8f;      //P Faktor des P-Reglers
+
+        //printf("%f\n",CorrectFactor);
+        if(abs(CorrectFactor) <= 5.0f) {    //erkennt Wand oder Lücke auf linker Seite
+            controller.setDesiredSpeedLeft((Speed * (-1.0f)) - CorrectFactor); //Bei Wand wird korrigiert
+            controller.setDesiredSpeedRight(Speed - CorrectFactor);
+        } else {
+            controller.setDesiredSpeedLeft(Speed * (-1.0f));                 //Bei Lücke wird nach Counter gefahren
+            controller.setDesiredSpeedRight(Speed);
+        }
+        
+        if(distance_front <= 3)
+        {
+            break;
+        }
+        
+        
+    }
+    stop();
+    printf("Das Programm wurde: %d durchlaufen\n", AnzahlZyklen);
+}
+
+int turnRight(int direction)
+{
+    controller.setDesiredSpeedLeft(15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
+    controller.setDesiredSpeedRight(15.0f);
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+        //do nothing
+    }
+    stop();
+    direction = direction + 1;
+    if(direction == 5)
+    {
+        direction = 1;
+    }
+    return direction;
+}
+
+int turnLeft(int direction)
+{
+    controller.setDesiredSpeedLeft(-15.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
+    controller.setDesiredSpeedRight(-15.0f);
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 590) {
+        //do nothing
+    }
+    stop();
+    direction = direction - 1;
+        if(direction == 0)
+    {
+        direction = 4;
+    }
+    return direction;
+}
+
+
+void stop(void)
+{
+    controller.setDesiredSpeedRight(0.0f);
+    controller.setDesiredSpeedLeft(0.0f);
+    resetCounter();
+
+}
+
+void resetCounter(void)
+{
+    controller.DetachTicker();
+    counterLeft.reset();
+    counterRight.reset();
+    controller.setDesiredSpeedRight(0.0f);
+    controller.setDesiredSpeedLeft(0.0f);
+    controller.AttachTicker();
+
+}
+
+void driveDist(int Distance, int Direction)
+{
+    controller.setDesiredSpeedLeft(-20.0f*Direction); // Drehzahl in [rpm]
+    controller.setDesiredSpeedRight(20.0f*Direction);
+
+    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
+        //Fährt bis Anzahl umdrehungen erreicht sind
+    }
+    stop();
+}
+