File content as of revision 18:3ee1b02ed3aa:

#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);

}

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

}


void Roboter::bandeAusweichen()         // Hindernissen ausweichen
{
    float x=0.13f;       // Distanz ab welcher sensoren reagieren sollen

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

    }

    while(sensors[0] < x && 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+0.02f)) {     // sensor links, roboter dreht nach rechts
        desiredSpeedLeft = v;
        desiredSpeedRight = v;

    }

}


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

    if(ii < 70) {                // Arm nach unten fahren
        servo2->SetPosition(2250);
    }
    if(ii > 69 && ii < 110) {      // gerade fahren
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
    }
    if(ii > 109 && ii < 160) {     // gerade fahren + Greifer schliessen
        desiredSpeedLeft = v;
        desiredSpeedRight = -v;
        servo1->SetPosition(500);
    }
    if(ii > 159 && ii < 230) {    // Arm nach oben fahren
        desiredSpeedLeft = 0;
        desiredSpeedRight = 0;
        servo2->SetPosition(700);
    }
    if(ii > 229 && ii < 280) {    // Greifer öffnen
        servo1->SetPosition(1500);
    }
    ii++;
    r = 5;                      // Rückegabewert

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

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

int Roboter::kreisFahren()
{
    int r;
    static int time1 = 0;
    if(time1 < 310) {       // 1s im Kreis drehen
        desiredSpeedLeft = v*1.7f;
        desiredSpeedRight = v*1.7f;
        time1 ++;
        r = 1;          // state
        wait(0.01f);
    } 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;
    }
    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;
}