File content as of revision 5:24350e6fc9d7:

#include "mbed.h"
#include "IRSensor.h"
#include "MotorEncoder.h"
#include "LowpassFilter.h"
#include "Servo.h"
#include <cmath>

DigitalOut led(LED1);           //Zustands-LED: Grüne LED für Benutzer

//--------------------IRSensoren------------------------------------------------

AnalogIn distance(PB_1);        //Input der Distanzsensoren
DigitalOut enableSensor(PC_1);  //Aktivierung der IRSensoren
DigitalOut bit0(PH_1);          //Ansteuerung der Sensoren 0-5 mit 3 Bits
DigitalOut bit1(PC_2);
DigitalOut bit2(PC_3);
IRSensor sensors[6];            //Erstellt 6 IRSensor-Objekte als Array

DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };    //LED-Outputs der Sensoren

float sensorMittelwert[6];   //Array der 6 Sensorenwerte
float sensorTiefbass[6];
float sensorZentimeter[6];
int printfZaehler = 0;

//Titel printf()
void title()
{
    printf("\f"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
}

void sensorWerte()   //Rechnet Sensorwerte aus und speichert sie unter sensorZentimeter[]
{
    for(int j = 0; j < 25; j++) {                //Zählt 25 Sensorwerten pro Sensor zusammen
        for(int i = 0; i < 6; i++) {
            sensorMittelwert[i] += sensors[i].read();
        }
    }
    for(int i = 0; i < 6; i++) {
        sensorTiefbass[i] = sensorTiefbass[i]*0.75f + sensorMittelwert[i]*0.25f;    //Verrechnet den neuen Wert mit dem alten
        sensorMittelwert[i] = 0.0f;                                                 //Setzt die Sensorwerte auf NULL
        switch(i) { //Rechnet die Werte in Zentimeter
            case 0: //Mitte
                if(sensorTiefbass[i] < 2.97f) sensorZentimeter[i] = 80.0f;  //Grenzwert
                else sensorZentimeter[i] = 110.0f/pow((sensorTiefbass[i]-1.9f),0.41f)-27.0f;
                break;
            case 1: //UntenRechts
                if(sensorTiefbass[i] < 3.2f) sensorZentimeter[i] = 69.0f;   //Grenzwert
                else sensorZentimeter[i] = 87.0f/pow((sensorTiefbass[i]-2.2f),0.41f)-18.0f;
                break;
            case 3: //Links
                if(sensorTiefbass[i] < 3.67f) sensorZentimeter[i] = 80.0f;  //Grenzwert
                sensorZentimeter[i] = 80.0f/pow((sensorTiefbass[i]-3.1f),0.38f)-19.0f;
                break;
            case 4: //Rechts
                if(sensorTiefbass[i] < 3.53f) sensorZentimeter[i] = 80.0f;  //Grenzwert
                else sensorZentimeter[i] = 90.0f/pow((sensorTiefbass[i]-2.8f),0.4f)-22.0f;
                break;
            case 5: //UntenLinks
                if(sensorTiefbass[i] < 4.4f) sensorZentimeter[i] = 59.0f;   //Grenzwert
                else sensorZentimeter[i] = 115.0f/pow((sensorTiefbass[i]-2.5f),0.4f)-30.0f;
                break;
        }
    }
    printf("%f\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0]);   //Plottet die unteren Sensoren + Mitte-Oben
}

//--------------------Motor&Encoder---------------------------------------------

const float PERIOD = 0.002f;                    // period of control task, given in [s]
const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
const float LOWPASS_FILTER_FREQUENCY = 300.0f;  // frequency of lowpass filter for actual speed values, given in [rad/s]
const float KN = 40.0f;                         // speed constant of motor, given in [rpm/V]
const float KP = 0.2f;                          // speed controller gain, given in [V/rpm]
const float MAX_VOLTAGE = 12.0f;                // supply voltage for power stage in [V]
const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)

MotorEncoder counterLeft(PB_6, PB_7);
MotorEncoder counterRight(PA_6, PC_7);

LowpassFilter speedLeftFilter;
LowpassFilter speedRightFilter;

DigitalOut enableMotorDriver(PB_2);
PwmOut pwmLeft(PA_8);
PwmOut pwmRight(PA_9);

short previousValueCounterRight = 0;
short previousValueCounterLeft = 0;

float desiredSpeedLeft;     //Ansteuerung des linken Motors
float desiredSpeedRight;    //Ansteuerung des rechten Motors

float actualSpeedLeft;
float actualSpeedRight;

void 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-actualSpeedRight)+desiredSpeedRight/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;

    pwmLeft = 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;

    pwmRight = dutyCycleRight;
}

int active = 0;
float distanceStatic;
int geradeaus_langsam(float distance)   // Rückgabewert 1 -> distance erreicht
{
    if(active == 0) {
        active = 1;
        distanceStatic = distance;
        counterLeft.reset();
        counterRight.reset();
    }
    if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
        desiredSpeedLeft = 1.5 + 30;
        desiredSpeedRight = 1.5 - 30;
    } else {
        desiredSpeedLeft = 0.0;
        desiredSpeedRight = 0.0;
        active = 0;
        return 1;
    }
    return 0;
}

int zurueck_langsam(float distance)   // Rückgabewert 1 -> distance erreicht
{
    if(active == 0) {
        active = 1;
        distanceStatic = distance;
        counterLeft.reset();
        counterRight.reset();
    }
    if((counterRight.read() - counterLeft.read()) / 100.0f < distanceStatic) {
        desiredSpeedLeft = 1.5 - 50;
        desiredSpeedRight = 1.5 + 50;
    } else {
        desiredSpeedLeft = 0.0;
        desiredSpeedRight = 0.0;
        active = 0;
        return 1;
    }
    return 0;
}

int geradeaus_schnell(float distance)   // RRückgabewert 1 -> distance erreicht
{
    if(active == 0) {
        active = 1;
        distanceStatic = distance;
        counterLeft.reset();
        counterRight.reset();
    }
    if((counterLeft.read() - counterRight.read()) / 100.0f < distanceStatic) {
        desiredSpeedLeft = 1.5 + 90;
        desiredSpeedRight = 1.5 - 90;
    } else {
        desiredSpeedLeft = 0.0;
        desiredSpeedRight = 0.0;
        active = 0;
        return 1;
    }
    return 0;
}

//--------------------Ausweichen------------------------------------------------

int a = 0;
int statusAusweichen = 0;
int zaehlerAusweichen = 0;
int ausweichen()
{
    if(statusAusweichen == 0) {
        if(sensorZentimeter[0] < 10) statusAusweichen = 1;                                                  //statusAusweichen 1 -> Wand vorne -> 90° rechts
        else if(sensorZentimeter[0] + sensorZentimeter[3] + sensorZentimeter[4] < 70) statusAusweichen = 2; //statusAusweichen 2 -> Ecke -> 90° rechts
        else if(sensorZentimeter[3] < 10) {
            if(a > 0) statusAusweichen = 9;                 //Spezial
            else statusAusweichen = 3;                      //statusAusweichen 3 -> Wand links -> 30° rechts
        } else if(sensorZentimeter[4] < 10) {
            if(a > 0) statusAusweichen = 9;                 //Spezial
            else statusAusweichen = 4;                      //statusAusweichen 4 -> Wand rechts -> 30° links
        }
    }
    switch(statusAusweichen) {
        case 0: //kein Hindernis
            a = 0;
            return 1;               //Ausgewichen
        case 1: //Hindernis vorne
        case 2: //Ecke
            if(zaehlerAusweichen > 12) {
                statusAusweichen = 0;
                zaehlerAusweichen = 0;
                desiredSpeedLeft = 0;
                desiredSpeedRight = 0;
            } else {
                desiredSpeedLeft = 60;
                desiredSpeedRight = 60;
                zaehlerAusweichen++;
            }
            break;
        case 3: //Hindernis links
            if(zaehlerAusweichen > 4) {
                a++;
                statusAusweichen = 0;
                zaehlerAusweichen = 0;
                desiredSpeedLeft = 0;
                desiredSpeedRight = 0;
            } else {
                desiredSpeedLeft = 60;
                desiredSpeedRight = 60;
                zaehlerAusweichen++;
            }
            break;
        case 4: //Hindernis rechts
            if(zaehlerAusweichen > 4) {
                a++;
                statusAusweichen = 0;
                zaehlerAusweichen = 0;
                desiredSpeedLeft = 0;
                desiredSpeedRight = 0;
            } else {
                desiredSpeedLeft = -60;
                desiredSpeedRight = -60;
                zaehlerAusweichen++;
            }
            break;
        case 9: //Spezial   120°
            if(zaehlerAusweichen > 16) {
                a = 0;
                statusAusweichen = 0;
                zaehlerAusweichen = 0;
                desiredSpeedLeft = 0;
                desiredSpeedRight = 0;
            } else {
                desiredSpeedLeft = 60;
                desiredSpeedRight = 60;
                zaehlerAusweichen++;
            }
            break;
    }
    return 0;   //Am ausweichen...
}

//--------------------Servos----------------------------------------------------

Servo Arm(PA_10);
Servo Greifer(PC_4);
Servo Deckel(PB_8);
AnalogIn Red (PB_0);               //Farbauswertung Rot
AnalogIn Green (PA_4);             //Farbauswertung Grün

void aufheben()
{
    enableSensor = 0;
    desiredSpeedLeft = 0.0f;
    desiredSpeedRight = 0.0f;

    for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
        Greifer.SetPosition(pos);
        wait_ms(7);
    }
    for (int pos = 440; pos < 2300; pos += 25) {      //Arm Runter
        Arm.SetPosition(pos);
        wait_ms(7);
    }
    desiredSpeedLeft = 65.0f;
    desiredSpeedRight = -60.0f;
    wait_ms(400);
    for (int pos = 1400; pos > 800; pos -= 25) {      //Greifer Schliessen
        Greifer.SetPosition(pos);
        wait_ms(80);
    }
    desiredSpeedLeft = 0.0f;
    desiredSpeedRight = 0.0f;

    if (Green>Red) {
        desiredSpeedLeft = -35.0f;
        desiredSpeedRight = 30.0f;
        wait(1.0f);
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 00.0f;
        for (int pos = 2300; pos > 440; pos -= 30) {      //Arm heben
            Arm.SetPosition(pos);
            wait_ms(5);
        }
        wait(0.1f);
        for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
            Greifer.SetPosition(pos);
            wait_ms(3);
        }
    } else if(Red>Green) {
        for (int pos = 1300; pos < 2250; pos += 25) {    //Deckel schliessen
            Deckel.SetPosition(pos);
            wait_ms(2);
        }
        desiredSpeedLeft = -35.0f;
        desiredSpeedRight = 30.0f;
        wait(1.0f);
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 00.0f;
        for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
            Arm.SetPosition(pos);
            wait_ms(9);
        }
        for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
            Greifer.SetPosition(pos);
            wait_ms(4);
        }
        for (int pos = 440; pos < 700; pos += 25) {      //Arm Runter
            Arm.SetPosition(pos);
            wait_ms(5);
        }
        for (int pos = 2250; pos > 1300; pos -= 30) {     //Deckel öffnen
            Deckel.SetPosition(pos);
            wait_ms(5);
        }
        for (int pos = 700; pos > 440; pos -= 25) {      //Arm heben
            Arm.SetPosition(pos);
            wait_ms(5);
        }
    } else {
        for (int pos = 2300; pos > 1600; pos -= 25) {      //Arm heben um alfällig eingeklemmte Klötze von den Sensoren zu entfernen
            Arm.SetPosition(pos);
            wait_ms(5);
        }
        desiredSpeedLeft = -40.0f;
        desiredSpeedRight = 30.0f;
        wait(1.0f);
        desiredSpeedLeft = 0.0f;
        desiredSpeedRight = 0.0f;
        for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
            Greifer.SetPosition(pos);
            wait_ms(2);
        }
        for (int pos = 1600; pos > 440; pos -= 25) {      //Arm heben
            Arm.SetPosition(pos);
            wait_ms(5);
        }
    }
    enableSensor = 1;
}

//--------------------Status&Main-----------------------------------------------

enum RobotState {
    // START = 0,
    FAHREN = 0,
    AUSWEICHEN,
    VORSUCHEN,
    SUCHEN,
    DREHUNGBACK,
    ANFAHREN,
    LADEN
};


int main()
{

    // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
    pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
    pwmRight.period(0.00005f);
    pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
    pwmRight = 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;    //Initialisiere Geschwindigkeit auf 0
    desiredSpeedRight = 0.0f;   //Initialisiere Geschwindigkeit auf 0
    actualSpeedLeft = 0.0f;
    actualSpeedRight = 0.0f;

    Ticker t1;
    t1.attach( &speedCtrl, PERIOD);

    int status = 0; //status definiert aktuellen Programmcode (0: Startprogramm, 1: Suchen, 2: Ausrichten, 3: Anfahren, 4: Aufnehmen)

    //Servos
    Greifer.Enable(1500,20000);
    Arm.Enable (440,20000);
    Deckel.Enable(1500,20000);

    //SensorArrays
    int z = 0;  //Zähler
    int y = 0;
    float sensorLinks[500];
    float sensorOben[500];

    enableMotorDriver = 1;

    //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
    for( int i = 0; i < 6; i++) {
        sensors[i].init(&distance, &bit0, &bit1, &bit2, i);
        sensorMittelwert[i] = 0.0f;
        sensorTiefbass[i] = 0.0f;
    }
    enableSensor = 1;   //Aktiviert die IRSensoren

    status = FAHREN;

    int timer = 0;
    led = 0;

    while(1) {
        //Lese Sensorwerte, Tiefpass Initialisierung
        sensorWerte();
        timer++;
        wait(0.0125f);

        switch(status) {

                /*          case START: //Start Sequenz
                              status = FAHREN;
                              break;
                */
            case FAHREN:
                status = AUSWEICHEN;
                if(geradeaus_schnell(100)) {
                    status = VORSUCHEN;
                }
                break;

            case AUSWEICHEN:
                if(ausweichen()) status = FAHREN;   //Wenn fertig ausgewichen -> FAHREN
                break;

            case VORSUCHEN:
                led = 1;
                desiredSpeedLeft = 15.0f; //Drehung
                desiredSpeedRight = 15.0f;
                if(z<12) { //Schreibt die Sensorwerte in ein Array
                    if(sensorZentimeter[0] > 60) sensorOben[z] = 60;
                    else sensorOben[z] = sensorZentimeter[0];
                    sensorLinks[z] = sensorZentimeter[5];
                    z++;
                } else status = SUCHEN;
                break;

            case SUCHEN:
                //for(int i = 0; i < 10; i++)
                desiredSpeedLeft = 20.0f; //Drehung
                desiredSpeedRight = 20.0f;

                if(z < 500) {
                    if(sensorZentimeter[0] > 60) sensorOben[z] = 60; //ACHTUUUUNG Z <500 und sensorOben = 60
                    else sensorOben[z] = sensorZentimeter[0];
                    sensorLinks[z] = sensorZentimeter[5];
                    //Prüft Ausschlag && vergleicht ersten mit letztem Wert && prüft Zwischenwerte
                    if(3.0f * sensorLinks[z] > sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] + 13.5f && sensorLinks[z] + 1 > sensorLinks[z-12] && sensorLinks[z] >= sensorLinks[z-2] && sensorLinks[z] >= sensorLinks[z-3] && sensorLinks[z] >= sensorLinks[z-4] && sensorLinks[z-12] >= sensorLinks[z-10] && sensorLinks[z-12] >= sensorLinks[z-9] && sensorLinks[z-12] >= sensorLinks[z-8]) {
                //        if(sensorLinks[z-7] + sensorLinks[z-6] + sensorLinks[z-5] > 3 * 33.5f) {
                            //Prüft Hindernis
                            if(sensorLinks[z-5] + 3.5f < sensorOben[z-5] && sensorLinks[z-6] + 3.5f < sensorOben[z-6] && sensorLinks[z-7] + 3.5f < sensorOben[z-7]) {
                                status = DREHUNGBACK;
                                z--;
                                printf("\n\r");
                  //          }
                        }
                    }
                    z++;
                } else {
                    z = 0;
                    status = FAHREN;
                }
                break;

            case DREHUNGBACK:
                if(y < 5) {
                    desiredSpeedLeft = -25.0f;  //Drehung rückwärts
                    desiredSpeedRight = -25.0f;
                    y++;
                    /*                } else if {y < 30){
                                        desiredSpeedLeft = 0.0f;    //Stillstand für genauen Sensorwert
                                        desiredSpeedRight = 0.0f;
                                        y++;
                    */
                } else {
                    y = 0;
                    status = ANFAHREN;
                }
                break;

            case ANFAHREN:
                if(sensorLinks[z-6] < 15.0f) {
                    if(zurueck_langsam(15.0f - sensorLinks[z-6])) {
                        z = 0;
                        status = LADEN;
                    }
                } else if(geradeaus_langsam(sensorLinks[z-6] - 15.0f)) {  //-15cm Roboterarm-Offset
                    z = 0;
                    status = LADEN;
                }
                break;

            case LADEN:
                aufheben();
                desiredSpeedLeft = -50.0f;  //Drehung rückwärts
                desiredSpeedRight = -50.0f;
                wait(0.1f);
                desiredSpeedLeft = 0.0f;  //Stopp
                desiredSpeedRight = 0.0f;
                status = VORSUCHEN;
                break;

        }
    }
}