File content as of revision 21:69ee872b8ee9:

#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 lr = 1;
    float x = 0.1;                 // Distanz ab welcher sensoren reagieren sollen

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

}


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

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

    }

    while(sensors[0] < x) { // sensor vorne, roboter dreht nach links
        desiredSpeedLeft = -v*2;
        desiredSpeedRight = -v*2;

    }

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

    }

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

    }

}

int Roboter::ausweichen2()         // Hindernissen ausweichen beim Anfahren des Klotzes
{
    float x = 0.1;                 // 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 < 500) {                      // rückwärts fahren
            desiredSpeedLeft = -(v+30);
            desiredSpeedRight = v+30;
        }
        if(ii > 499 && ii < 900) {         // links drehen
            desiredSpeedLeft = -v*2;
            desiredSpeedRight = -v*2;
        }
        if(ii > 899 && ii < 1800) {         // gerade fahren
            desiredSpeedLeft = v*2;
            desiredSpeedRight = -v*2;
        }
        if(ii > 1799 && ii < 2300) {        // rechts drehen
            desiredSpeedLeft = v*2;
            desiredSpeedRight = v*2;
        }
        ii++;
        if(ii > 2299) {                     // rücksetzen
            desiredSpeedLeft = 0;
            desiredSpeedRight = 0;
            r = 0;
            ii = 0;
            s = 1;
        }
    }

    if(l == 1) {
        if(ii < 500) {                    // rückwärts fahren
            desiredSpeedLeft = -(v+30);
            desiredSpeedRight = v+30;
        }
        if(ii > 499 && ii < 900) {       // rechts drehen
            desiredSpeedLeft = v*2;
            desiredSpeedRight = v*2;
        }
        if(ii > 899 && ii < 1800) {      // gerade fahren
            desiredSpeedLeft = v*2;
            desiredSpeedRight = -v*2;
        }
        if(ii > 1799 && ii < 2300) {      // links drehen
            desiredSpeedLeft = -v*2;
            desiredSpeedRight = -v*2;
        }
        ii++;
        if(ii > 2299) {
            desiredSpeedLeft = 0;
            desiredSpeedRight = 0;
            l = 0;
            ii = 0;
            s = 1;
        }
    }
    return s;
}


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

    if(ii < 650) {                   // Arm nach unten fahren
        servo2->SetPosition(2250);
    }
    if(ii > 649 && ii < 1100) {      // gerade fahren
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
    }
    if(ii > 1099 && ii < 1500) {     // gerade fahren + Greifer schliessen
        if(ii < 1400) {
            desiredSpeedLeft = v;
            desiredSpeedRight = -v;
        }
        servo1->SetPosition(500);
    }
    if(ii > 1499 && ii < 1900) {     // rückwärts fahren
        desiredSpeedLeft = -v;
        desiredSpeedRight = v;
    }
    if(ii > 1899 && ii < 2550) {    // Arm nach oben fahren
        desiredSpeedLeft = 0;
        desiredSpeedRight = 0;
        servo2->SetPosition(700);
    }
    if(ii > 2549 && ii < 3000) {    // Greifer öffnen
        servo1->SetPosition(1500);
    }

    ii++;
    r = 5;                          // Rückegabewert

    if( ii > 2999 ) {
        r = 1;
        ii = 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) + (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) + (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*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's
        desiredSpeedLeft = speedValue;
        desiredSpeedRight = -speedValue;
    }
}

int Roboter::geradeFahren()
{
    int r;
    static int time2 = 0;
    if(time2 < 25) {    // 1s gerade aus fahren
        desiredSpeedLeft = v*4;
        desiredSpeedRight = -v*4;
        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 = 7;

    if(ii < 1000) {
        desiredSpeedLeft = -v;
        desiredSpeedRight = v;
    }

    if(ii > 999 && ii < 1800) {
        desiredSpeedLeft = v*2;
        desiredSpeedRight = v*2;
    }

    ii++;

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

    return s;
}

int Roboter::kreisFahren()
{
    int r;
    static int anzahl = 0;
    static int time1 = 0;

    if(time1 < 6 && anzahl%2 == 0) {       // 1s im Kreis drehen
        desiredSpeedLeft = v*2;
        desiredSpeedRight = v*2;
        time1 ++;
        r = 1;          // state
    } else {
        desiredSpeedLeft = -(v*2);
        desiredSpeedRight = -(v*2);
        time1 ++;
        r = 1;          // state
    }
    if(time1 == 6) {
        time1 = 0;
        anzahl ++;
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 0.0f;
        r = 3;          // state
    }
    return r;
}

int Roboter::stateAuswahl(float cam, int tempState, int temp)
{
    int s = 1;
    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 && temp != 5) {     // Roboter in Position
        s = 5;
    }
    if(cam == 400.0f && temp == 5) {     // Roboter immer noch beim selben Klotz -> back()
        s = 7;
    }
    if(cam < 1) {
        s = tempState;
    }
    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;
}