File content as of revision 19:adae367247d4:

#include "Roboter.h"


Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver)

{
    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->servo1 = servo1;
    this->servo2 = servo2;
    this->counterLeft = counterLeft;
    this->counterRight = counterRight;
    this->enableMotorDriver = enableMotorDriver;

    initSpeedcontrol();

    t1.attach(this, &Roboter::speedCtrl, PERIOD);

}

int Roboter::readSensors()            // Sensoren auslesen
{
    int l = 0;
    float x = 0.12;                 // Distanz ab welcher sensoren reagieren sollen

    if(sensors[1] < x || sensors[5] < x) {            // Sensor rechts und links
        l = 6;
    } else {
        l = 1;
    }

    return l;

}


void Roboter::ausweichen1()         // Hindernissen ausweichen
{
    float x = 0.15;       // Distanz ab welcher sensoren reagieren sollen

    while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) {  // alle sensoren aktiv, roboter fährt nach hinten
        desiredSpeedLeft = -v;
        desiredSpeedRight = v;

    }

    while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links
        desiredSpeedLeft = -v;
        desiredSpeedRight = -v;

    }

    while(sensors[1] < x) {     // sensor rechts, roboter dreht nach links
        desiredSpeedLeft = -v;
        desiredSpeedRight = -v;

    }

    while(sensors[5] < x && sensors[1] > x) {     // sensor links, roboter dreht nach rechts
        desiredSpeedLeft = v;
        desiredSpeedRight = v;

    }

}

int Roboter::ausweichen2()         // Hindernissen ausweichen beim Anfahren des Klotzes
{
    float x = 0.12;                 // Distanz ab welcher sensoren reagieren sollen
    static int l = 0;
    static int r = 0;
    static int ii = 0;
    int s = 6;

    if(sensors[1] < x && l == 0) {
        r = 1;
    }

    if(sensors[5] < x && r == 0) {
        l = 1;
    }

    if(r == 1) {
        if(ii < 9000) {
            desiredSpeedLeft = -(v+30);
            desiredSpeedRight = v-10;
        }
        if(ii > 8999 && ii < 24000) {
            desiredSpeedLeft = v*2;
            desiredSpeedRight = -v*2;
        }
        if(ii > 23999 && ii < 33000) {
            desiredSpeedLeft = v*1.8f;
            desiredSpeedRight = v*1.8f;
        }
        ii++;
        if(ii > 32999) {
            desiredSpeedLeft = 0;
            desiredSpeedRight = 0;
            r = 0;
            ii = 0;
            s = 1;
        }
    }

    if(l == 1) {
        if(ii < 9000) {
            desiredSpeedLeft = -(v-10);
            desiredSpeedRight = v+30;
        }
        if(ii > 8999 && ii < 24000) {
            desiredSpeedLeft = v*2;
            desiredSpeedRight = -v*2;
        }
        if(ii > 23999 && ii < 33000) {
            desiredSpeedLeft = -v*1.8f;
            desiredSpeedRight = -v*1.8f;
        }
        ii++;
        if(ii > 32999) {
            desiredSpeedLeft = 0;
            desiredSpeedRight = 0;
            l = 0;
            ii = 0;
            s = 1;
        }
    }
    return s;
}


int Roboter::pickup()           // Klotz aufnehmen
{
    //static int anz = 0;
    static int ii = 0;
    int r;                      // Rückegabewert
    desiredSpeedLeft = 0;
    desiredSpeedRight = 0;

    if(ii < 7000) {                // Arm nach unten fahren
        servo2->SetPosition(2250);
    }
    if(ii > 6999 && ii < 12000) {      // gerade fahren
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
    }
    if(ii > 11999 && ii < 17000) {     // gerade fahren + Greifer schliessen
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
        servo1->SetPosition(500);
    }
    if(ii > 16999 && ii < 19000) {
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
    }
    if(ii > 18999 && ii < 26000) {    // Arm nach oben fahren
        desiredSpeedLeft = 0;
        desiredSpeedRight = 0;
        servo2->SetPosition(700);
    }
    if(ii > 25999 && ii < 31000) {    // Greifer öffnen
        servo1->SetPosition(1500);
    }
    ii++;
    //anz++;
    r = 5;                      // Rückegabewert

    if( ii > 30999 ) {
        r = 1;
        ii = 0;
    }
    /*if(anz == 31001) {
        r = 7;
        anz = 0;
    }*/
    return r;
}

void Roboter::anfahren(float cam)
{
    float speedValue;
    float maxWert = 20;
    if(cam == 100) {
        desiredSpeedLeft = -v/2;
        desiredSpeedRight = -v/2;
    }
    if(cam == 200) {
        desiredSpeedLeft = v/2;
        desiredSpeedRight = v/2;
    }
    if(cam > 100 && cam <200 ) {                  // links fahren, wenn wir Werte von 100 bis 199 haben
        cam = cam -99.0f;                           // cam nimmt die Werte von 1 bis 100 an
        speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
        desiredSpeedLeft = -speedValue;
        desiredSpeedRight = -speedValue;
    }
    if(cam > 200 && cam < 300) {                   // rechts fahren, wenn wir Werte von 200 bis 299 haben
        cam = cam -199.0f;                          // cam nimmt die Werte von 1 bis 100 an
        speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
        desiredSpeedLeft = speedValue;
        desiredSpeedRight = speedValue;
    }
    if(cam >= 300 && cam <400) {                   // gerade fahren, wenn wir Werte von 300 bis 399 haben
        cam = cam-299.0f;                           // cam nimmt die Werte von 1 bis 100 an
        speedValue = (v*2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
        desiredSpeedLeft = speedValue;
        desiredSpeedRight = -speedValue;
    }
}

int Roboter::geradeFahren()
{
    int r;
    static int time2 = 0;
    if(time2 < 100) {    // 1s gerade aus fahren
        desiredSpeedLeft = v*5;
        desiredSpeedRight = -v*5;
        time2 ++;
        r = 1;       // state
    } else {
        time2 = 0;
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 0.0f;
        r = 2;       //state
    }
    return r;
}

int Roboter::back()
{
    static int ii = 0;
    int s;

    if(ii < 3000) {
        desiredSpeedLeft = -v;
        desiredSpeedRight = v;
        s = 7;
    }
    ii++;

    if(ii > 2999) {
        desiredSpeedLeft = 0;
        desiredSpeedRight = 0;
        ii = 0;
        s = 1;
    }
    return s;
}

int Roboter::kreisFahren()
{
    int r;
    static int time1 = 0;
    if(time1 < 40) {       // 1s im Kreis drehen
        desiredSpeedLeft = v*1.7f;
        desiredSpeedRight = v*1.7f;
        time1 ++;
        r = 1;          // state
    } else {
        time1 = 0;
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 0.0f;
        r = 3;          // state
    }
    return r;
}

int Roboter::stateAuswahl(float cam, int temp)
{
    int s;
    if(cam >= 100.0f && cam < 400.0f ) {           // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399
        s = 4;
    }
    if(cam == 400.0f) {     // Roboter in Position
        s = 5;
    }
    if(cam == 0.0f) {
        s = temp;
    }
    if(cam == 1) {
        s = 1;
    }
    return s;


}

void Roboter::initSpeedcontrol()
{
    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
    pwmL->period(0.00005f); // Setzt die Periode auf 50 μs
    pwmR->period(0.00005f);
    *pwmL = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
    *pwmR = 0.5f; // Duty-Cycle von 50%

    // Initialisieren von lokalen Variabeln
    previousValueCounterLeft = counterLeft->read();
    previousValueCounterRight = counterRight->read();
    speedLeftFilter.setPeriod(PERIOD);
    speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
    speedRightFilter.setPeriod(PERIOD);
    speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);

    desiredSpeedLeft = 0.0f;
    desiredSpeedRight = 0.0f;
    actualSpeedLeft = 0.0f;
    actualSpeedRight = 0.0f;

    *enableMotorDriver = 1;
}

void Roboter::speedCtrl()
{
    // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
    short valueCounterLeft = counterLeft->read();
    short valueCounterRight = counterRight->read();
    short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
    short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;

    previousValueCounterLeft = valueCounterLeft;
    previousValueCounterRight = valueCounterRight;
    actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
    actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);

    // Berechnen der Motorspannungen Uout
    float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
    float voltageRight = KP*(desiredSpeedRight*correction-actualSpeedRight)+desiredSpeedRight*correction/KN;

    // Berechnen, Limitieren und Setzen der Duty-Cycle
    float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
    if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
    else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;

    *pwmL = dutyCycleLeft;
    float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
    if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
    else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;

    *pwmR = dutyCycleRight;
}