File content as of revision 4:85b8b4aa97a3:

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

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

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
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


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();
        }
        //wait( 0.001f );
    }
    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\t%f\n\r", sensorZentimeter[5], sensorZentimeter[0], sensorZentimeter[1]);   //Plottet die unteren Sensoren + Mitte-Oben

    printfZaehler++;
//    if(printfZaehler % 120 == 0){
//        wait(3.5f);
//    }
    if(printfZaehler % 40 == 0) title();  //Erstellt nach 40 Zeilen eine neue Seite
}


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

DigitalOut my_led(LED1);

short previousValueCounterRight = 0;
short previousValueCounterLeft = 0;

float desiredSpeedLeft;
float desiredSpeedRight;

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;
}

enum RobotState {
    START = 0,
    VORSUCHEN,
    SUCHEN,
    LADEN,
    SONST

};
int aufheben (){
      
      enableSensor=0;
      desiredSpeedLeft =65.0f; //50 RPM
      desiredSpeedRight = -60.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 = 18.0f; 
        desiredSpeedRight = -15.0f;
    wait_ms(400);
       for (int pos = 1400; pos > 800; pos -= 25) {      //Greifer Schliessen
          Greifer.SetPosition(pos); 
          wait_ms(80); 
      }
      
    if (Green>Red){
            for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
          Arm.SetPosition(pos); 
          wait_ms(8); 
      } 
      
          for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
          Greifer.SetPosition(pos);  
          wait_ms(2);
      } 
 }
    else if(Red>Green) {
       
       for (int pos = 1300; pos < 2250; pos += 25) {    //Deckel schliessen
          Deckel.SetPosition(pos); 
          wait_ms(2); 
      } 
    
      for (int pos = 2300; pos > 440; pos -= 25) {      //Arm heben
          Arm.SetPosition(pos); 
          wait_ms(7); 
      } 
      wait_ms(50);
          for (int pos = 800; pos < 1400; pos += 25) {  //Greifer öffnen
          Greifer.SetPosition(pos);  
          wait_ms(2);
      }
       for (int pos = 440; pos < 700; pos += 25) {      //Arm Runter 
         Arm.SetPosition(pos);  
          wait_ms(5);
      }
       for (int pos = 2250; pos > 1300; pos -= 25) {     //Deckel öffnen
          Deckel.SetPosition(pos); 
          wait_ms(5); 
      } 
       for (int pos = 700; pos > 440; pos -= 25) {      //Arm heben
          Arm.SetPosition(pos); 
          wait_ms(5); 
      } 
    }
    return 0;
}

int main()
{   
    Greifer.Enable(1500,20000);
    Arm.Enable (1500,20000);
    Deckel.Enable(1500,20000);
    // 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;
    desiredSpeedRight = 0.0f;
    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)

    //SensorArrays
    int z = 0;  //Zähler
    float sensorLinks[100];
    float sensorOben[100];
    float sensorRechts[100];

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

    int timer = 0;
    my_led = 0;

    while(1) {
        //lese sensorwerte, tiefpass initialisierung
        sensorWerte();
        ++timer;
        wait(0.025f);

//DISPLAY STATE 

        //while(status == 1){
        switch(status) {

                //start sequenz
            case START:
                // desiredSpeedLeft = 15.0f; //Drehung
                //  desiredSpeedRight = 15.0f;

                //roboter dreh links
                if( timer > 40 )
                    status = 1;
                break;

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

                //for(int i = 0; i < 10; i++)
                desiredSpeedLeft = 25.0f; //Drehung
                desiredSpeedRight = 25.0f;

                if(z < 100) {
                    sensorOben[z] = sensorZentimeter[0];
                    sensorLinks[z] = sensorZentimeter[5];
                    sensorRechts[z] = sensorZentimeter[1];
                    //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]) {
                        //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 = 3;
                            z=0;
                        }
                    }
                    z++;
                }
                else{
                    z = 0;
                    status = 4;
                }

                break;
            }
            case 3:
                desiredSpeedLeft = -15.0f;
                desiredSpeedRight = -15.0f;
                wait(0.2f);
                aufheben();
                wait(1.0f);
                status = VORSUCHEN;
                break;
            case 4:

                desiredSpeedLeft = 11.75f; //Stillstand
                desiredSpeedRight = -15.0f;
                wait(2.0f);
                status = VORSUCHEN;
                break;

        }
    }
}