File content as of revision 10:10bcb7fee9a6:

#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, Pixy& _pixy, PID_Control& _pid, Button& _onoff) : pwmLeft(_pwmLeft), pwmRight(_pwmRight), pixy(_pixy),  pid(_pid), onoff(_onoff)
{
    init(enable, bit0, bit1, bit2, distance, enableMotorDriver);
}

//Fahren::Fahren() {}

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

void Fahren::init(DigitalOut* enable, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, AnalogIn* distance, DigitalOut* enableMotorDriver)
{
    this->enable = enable;
    this->bit0 = bit0;
    this->bit1 = bit1;
    this->bit2 = bit2;
    this->distance = distance;
    this->enableMotorDriver = enableMotorDriver;
    state = 0;
    e = 0.0;
    diff = 0.0;
    

}

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

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

void Fahren::fahrInit()
{ 
    pwmLeft = 0.5;
    pwmRight = 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 (onoff.getState() != 0) {
        //pc->printf("left:%f\n\r", sensors[5].read());
    
    if(sensors[0]<=0.1 && sensors[1]<=0.12 && sensors[5]<=0.12) {
        //wenn hinten rechts => leichte linkskurve
        if(sensors[2]<=0.25) {
            pwmLeft = 0.35;
            pwmRight = 0.55;
            if (pc) {
                pc->printf("zurueck-rechtskurve\n");
            }
            state=zurueck_l;
        }

        if(sensors[4]<=0.25) {
            pwmLeft = 0.45;
            pwmRight = 0.65;
            if (pc) {
                pc->printf("zurueck-linkskurve\n");
            }
            state=zurueck_r;
        }
        if(sensors[4]>0.25 && sensors[2]>0.25) {
            pwmLeft = 0.4;
            pwmRight = 0.6;
            state=zurueck;
            if (pc) {
                pc->printf("zurueck-gerade\n");
            }
        }
    }
    if(sensors[5] <= wand) {
        
        pwmLeft = 0.35;
        pwmRight = 0.55;
        wait(0.8);
    }
    if (sensors[1] <= wand){
        pwmLeft = 0.45;
        pwmRight = 0.65;
        wait(0.8);
        }


    //plötzlicher Wand anschlag    ***************************************
    /* else if(sensors[1] <= wand){
         notfall1=1;
         pwmLeft = 0.3;
         pwmRight = 0.3;
         if (pc) {
             pc->printf("Notfall links drehen");}
     }

     else if(sensors[5] <= wand){
         notfall1=2;
         pwmLeft = 0.7;
         pwmRight = 0.7;
         if (pc) {
             pc->printf("Notfall rechts drehen");}
     }
    */


    // Wenn Front etwas sehen => drehen***********************************

    else if(sensors[0]<0.25 && sensors [1]<=wenden) {
        pwmLeft = 0.3;
        pwmRight = 0.3;
        if (pc) {
            pc->printf("drehen links\n\n\n");
        }
    }

    else if (sensors[0]<0.25 && sensors [5]<=wenden) {
        pwmLeft = 0.7;
        pwmRight = 0.7;
        if (pc) {
            pc->printf("drehen rechts\n\n\n");
        }
    }

    else if(sensors[0]<0.25) {
        if (rand()%2==0 && state != drehen) {
            pwmLeft = 0.3;
            pwmRight = 0.3;
            if (pc) {
                pc->printf("random drehen\n\n\n");
            }
        } else if (rand()%2 != 0 && state != drehen) {
            pwmLeft = 0.7;
            pwmRight = 0.7;
            if (pc) {
              pc->printf("random drehen\n\n\n");
            }
        }

        state=drehen;
    }
    //Wenn Front-Left etwas sehen => nach Rechts**************************
    else if(sensors[1] >= 0.08 && (sensors[5]<=wenden && sensors[5] >= 0.08)) {
        if (pc) {
            pc->printf("rechts\n");
        }
        pwmLeft = 0.65;
        pwmRight = 0.45;
        state=rechts;
    }

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

    //Wenn kein Sensor anspricht => PixyCam wird abgerufen*****************************
    else if(sensors[0]>=0.26) {
        if (pixy.getX()>=5 && pixy.getX()<=215){
            e = 160-pixy.getX();
            diff = pid.calc( e, 0.005f );
            pwmLeft = 0.65 - diff;
            pwmRight = 0.35 - diff;
        } else {
            pwmLeft = 0.65;
            pwmRight = 0.35;
        }
    }
    } else {
        pwmLeft = 0.5;
        pwmRight = 0.5;
    }
}