File content as of revision 9:d9e46f9c9e40:

#include "Roboter.h"


Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* s1, Servo* s2,DigitalIn* switch1, DigitalIn* switch2)

{

    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;
    this->s1 = s1;
    this->s2 = s2;
    this->switch1 = switch1;
    this->switch2 = switch2;


}

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;

    while(sensors[0] < x && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
        offsetLin = 0.1f;
        *pwmL = 0.5f+offsetDir-offsetLin; // Setzt die Duty-Cycle auf 50%
        *pwmR = 0.5f+offsetDir+offsetLin;

    }

    while(sensors[0].read() < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links
        offsetDir = -0.05;
        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
        *pwmR = 0.5f+offsetDir-offsetLin;

    }

    while(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
        offsetDir = -0.05;
        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
        *pwmR = 0.5f+offsetDir-offsetLin;

    }

    while(sensors[5] < x && sensors[1]>(x+0.02f)) {     // sensor links, roboter dreht nach rechts
        offsetDir = 0.08;
        *pwmL = 0.5f+offsetDir+offsetLin; // Setzt die Duty-Cycle auf 50%
        *pwmR = 0.5f+offsetDir-offsetLin;

    }

}




void Roboter::pickup ()
{



}