File content as of revision 3:017c85c4b14b:

#include "mbed.h"
#include "cstdlib"
#include <cmath>
#include "Fahren.h"


Fahren::Fahren(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight)
{
    init(enable, bit0, bit1, bit2, distance, enableMotorDriver, pwmLeft, pwmRight);
}

Fahren::Fahren() {}

/**
 * Deletes the IRSensor object.
 */
Fahren::~Fahren() {}

void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver, PwmOut* pwmLeft, PwmOut* pwmRight)
{
    this->enable = enable;
    this->bit0 = bit0;
    this->bit1 = bit1;
    this->bit2 = bit2;
    this->distance = distance;
    this->enableMotorDriver = enableMotorDriver;
    this->pwmLeft = pwmLeft;
    this->pwmRight = pwmRight;
    state = 0;
}

void Fahren::setSerialOutput(Serial *pc)
{
    this->pc = pc;
}

int Fahren::getState() {
    return state;
}

void Fahren::fahrInit() {
    pwmLeft->write(0.5);
    pwmRight->write(0.5);
    
    for( int ii = 0; ii<6; ++ii) {
        sensors[ii].init(distance, bit0, bit1, bit2, ii);

        enable->write(1);


        pwmLeft->period(0.00005);
        pwmRight->period(0.00005);

        enableMotorDriver->write(1);
    }
}

void Fahren::fahrRutine() {
    if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
        //wenn hinten rechts => leichte linkskurve
        if(sensors[2]<=0.25) {
            pwmLeft->write(0.35);
            pwmRight->write(0.55);
            if (pc) {
                pc->printf("zurueck-linkskurve\n");}
            state=zurueck_l;
        }

        if(sensors[4]<=0.25) {
            pwmLeft->write(0.35);
            pwmRight->write(0.55);
            if (pc) {
                pc->printf("zurueck-rechtskurve\n");}
            state=zurueck_r;
        }
        if(sensors[4]>=0.25 && sensors[2]>=0.25) {
            pwmLeft->write(0.4);
            pwmRight->write(0.6);
            state=zurueck;
            if (pc) {
                pc->printf("zurueck-gerade\n");}
        }
    }
      
    
     //plötzlicher Wand anschlag    ***************************************
    else if(sensors[1] <= wand ){
        pwmLeft->write(0.3);
        pwmRight->write(0.3);
        if (pc) {  
            pc->printf("links drehen");}
    }    
    else if(sensors[5] <= wand ){
        pwmLeft->write(0.7);
        pwmRight->write(0.7);
        if (pc) {  
            pc->printf("rechts drehen");}
    }
    
    
    // Wenn Front etwas sehen => drehen***********************************
    
    else if(sensors[0]<0.25 && sensors [1]<=wenden) {  
        pwmLeft->write(0.3);
        pwmRight->write(0.3);
        if (pc) {  
            pc->printf("drehen links\n\n\n");}
    }  
                    
    else if (sensors[0]<0.25 && sensors [5]<=wenden){    
        pwmLeft->write(0.7);
        pwmRight->write(0.7);
        if (pc) { 
            pc->printf("drehen rechts\n\n\n");}
               }
               
    else if(sensors[0]<0.25) {
    if (rand()%2==0 && state != drehen) {
            pwmLeft->write(0.3);
            pwmRight->write(0.3);
             pc->printf("random drehen\n\n\n");}
        } else if (rand()%2 != 0 && state != drehen) {
            pwmLeft->write(0.7);
            pwmRight->write(0.7);
             pc->printf("random drehen\n\n\n");}
        }
    state=drehen;
    }
    //Wenn Front-Left etwas sehen => nach Rechts**************************
    else if(sensors[5]<=wenden) {
        if (pc) {
            pc->printf("rechts\n");}
        pwmLeft->write(0.65);
        pwmRight->write(0.45);
        state=rechts;
    }

    // Wenn Front-Right etwas sehen => Links*******************************
    else if(sensors[1]<=wenden) {
        if(pc) {
            pc->printf("Links\n");}
        pwmLeft->write(0.55);
        pwmRight->write(0.35);
        state=links;
    }

    //Wenn kein Sensor anspricht => gerade aus*****************************
    else if(sensors[0]>=0.26) {
        if(pc) {
            pc->printf("gerade\n");}
        pwmLeft->write(0.65);   
        pwmRight->write(0.35);      
        state=gerade;
    }
    else {
        if(pc) {
            pc->printf("gerade2\n\r");}
        }
}