File content as of revision 0:e38b500d6e74:

#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
    
}