Diff: MotorDriver.cpp

Revision:

0:e38b500d6e74

Child:

1:4808f55970e8

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/MotorDriver.cpp	Thu Apr 19 11:31:49 2018 +0000
@@ -0,0 +1,145 @@
+#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);
+
+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 Speed)
+{
+    switch(Speed) {
+        case 1:
+
+            controller.setDesiredSpeedLeft(-30.0f); // Drehzahl in [rpm]
+            controller.setDesiredSpeedRight(30.0f);
+            printf("SpeedSet\n");
+            //Fährt bis Anzahl umdrehungen erreicht sind
+            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 1000*NumbField) {
+
+            }
+            printf("number erreicht");
+            stop();
+
+            break;
+
+
+        case 2:
+            controller.setDesiredSpeedLeft(-150.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
+            controller.setDesiredSpeedRight(150.0f);
+            //Fährt bis Anzahl umdrehungen erreicht sind
+            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
+
+            }
+            stop();
+
+            break;
+
+        case 3:
+            controller.setDesiredSpeedLeft(-20.0f); // Drehzahl in [rpm] bei 31/1 enspricht es ca. 374/min
+            controller.setDesiredSpeedRight(20.0f);
+            //Fährt bis Anzahl umdrehungen erreicht sind
+            while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 2000*NumbField) {
+                //do nothing
+            }
+            stop();
+
+            break;
+
+
+        default:
+
+            break;
+    }
+}
+
+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;
+    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;
+    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();
+}
+
+void orientate(void)
+{
+    while(ReadSensorValue(1) > ReadSensorValue(3))
+    {
+        driveDist(1,1);
+    }
+    while(ReadSensorValue(1) < ReadSensorValue(3))
+    {
+        driveDist(1,-1);
+    }
+    //Differenz Sensor zu Drehpunkt vorwärts fahren
+    
+}
+