File content as of revision 15:31d09ee65cf1:

#include "MotorDriver.h"
#include "Controller.h"
#include "ReadSensor.h"
#include "mbed.h"

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 Motortreiber ein
    counterLeft.reset();   //Counter reseten
    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) {         //Geschwindigkeit einstellen
        case 1:
            Speed = 20.0f;
            break;


        case 2:
            Speed = 40.0f;
            break;

        case 3:
            Speed = 60.0f;
            break;


        default:

            break;
    }

    //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
        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
        }
        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) <= 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 {
            controller.setDesiredSpeedRight(Speed * (-1.0f));                 //Bei Lücke wird nach Counter gefahren
            controller.setDesiredSpeedLeft(Speed);
        }
        
        if(distance_front <= 3)
        {
            break;
        }
        
        
    }
    stop();

}
//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)<= 580) {
        //do nothing
    }
    stop();
    direction = direction + 1;
    if(direction == 5)
    {
        direction = 1;
    }
    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)<= 580) {
        //do nothing
    }
    stop();
    direction = direction - 1;
        if(direction == 0)
    {
        direction = 4;
    }
    return direction;
}

//Roboter anhalten
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.setDesiredSpeedRight(-20.0f*Direction); // Drehzahl in [rpm]
    controller.setDesiredSpeedLeft(20.0f*Direction);

    while(((abs(counterLeft.read())+abs(counterRight.read()))/2)<= 100*Distance) {
        //Fährt bis Anzahl umdrehungen erreicht sind
    }
    stop();
}