File content as of revision 1:4808f55970e8:

#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;
    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 = readSensorValue(1);      //Distanz zur linken Wand wird gemessen
        AnzahlZyklen = AnzahlZyklen+1;      //Anzahl Regelvorgänge werden gezählt
        distance = distance - 7.8f;         //Distanz zwischen Position und Ideallinie wird berechnet
        CorrectFactor = distance*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);
        }
    }
    stop();
    printf("Das Programm wurde: %d durchlaufen", 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;
    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();
}