File content as of revision 1:4e84271a70c6:

#include "Roboter.h"


Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR)
{

    sensors[0].init( distance, bit0, bit1, bit2, 0);
    sensors[1].init( distance, bit0, bit1, bit2, 1);
    sensors[2].init( distance, bit0, bit1, bit2, 2);
    sensors[3].init( distance, bit0, bit1, bit2, 3);
    sensors[4].init( distance, bit0, bit1, bit2, 4);
    sensors[5].init( distance, bit0, bit1, bit2, 5);

    this->pwmR = pwmR;
    this->pwmL = pwmL;

    pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
    pwmR->period(0.00005f);

}

float Roboter::readSensor1()
{
    return sensors[0].read();

}


void Roboter::bandeAusweichen()
{
    float offsetDir;
    float offsetLin;

    float x=0.13f;       // Distanz ab welcher sensoren reagieren sollen


    offsetDir = 0;
    offsetLin = 0.1;


    if(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
        offsetDir = -0.05;
        offsetLin = 0;
    }
    if(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
        offsetDir = -0.05;
        offsetLin = 0;
    }
    if(sensors[5] < x && sensors[1]>(x+0.02f)) {     // sensor links, roboter dreht nach rechts
        offsetDir = 0.05;
        offsetLin = 0;
    }

    *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
    *pwmR = 0.5f+offsetDir-offsetLin;

    wait(0.1);

}