Diff: main.cpp

Revision:

2:365bf16abbf6

Parent:

1:d40ff07e2fe0

--- a/main.cpp	Wed May 10 09:14:12 2017 +0000
+++ b/main.cpp	Tue May 16 14:14:08 2017 +0000
@@ -2,6 +2,7 @@
 #include "IRSensor.h"
 #include "MotorEncoder.h"
 #include "LowpassFilter.h"
+#include <cmath>
 
 DigitalOut led(LED1);           //Zustands-LED: Grüne LED für Benutzer
 
@@ -14,10 +15,59 @@
 
 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  <    \t\t   -   \t\t    >\n\r"); //"\f" Setzt den Cursor an den Anfang der nächsten Seite
+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
 }
 
 
@@ -81,8 +131,19 @@
     pwmRight = dutyCycleRight;
 }
 
-int main(){
-    
+enum RobotState {
+    START = 0,
+    VORSUCHEN,
+    SUCHEN,
+    LADEN,
+    SONST
+
+};
+
+
+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);
@@ -105,13 +166,15 @@
     Ticker t1;
     t1.attach( &speedCtrl, PERIOD);
 
-    desiredSpeedLeft = 15.0f; //50 RPM
-    desiredSpeedRight = 15.0f; //50 RPM
+    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;
-    
-    float sensorMittelwert[6];   //Array der 6 Sensorenwerte
-    float sensorTiefbass[6];
-    int zaehler = 0;
 
     //Initialisiert Distanzsensoren und setzt sensorValue und sensorTiefbass auf NULL
     for( int i = 0; i < 6; i++) {
@@ -121,24 +184,87 @@
     }
     enableSensor = 1;   //Aktiviert die IRSensoren
 
+    status = START;
+
+    int timer = 0;
+    my_led = 0;
+
     while(1) {
-        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();
+        //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 = 15.0f; //Drehung
+                desiredSpeedRight = 15.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;
             }
-            wait( 0.001f );
+            case SUCHEN: {
+                my_led = 1;
+
+                //for(int i = 0; i < 10; i++)
+                desiredSpeedLeft = 15.0f; //Drehung
+                desiredSpeedRight = 15.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 = -20.0f; //Stillstand
+                desiredSpeedRight = 15.0f;
+                wait(2.0f);
+                status = VORSUCHEN;
+                break;
+            case 4:
+
+                desiredSpeedLeft = 15.0f; //Stillstand
+                desiredSpeedRight = -20.0f;
+                wait(2.0f);
+                status = VORSUCHEN;
+                break;
+
         }
-        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
-        }
-        
-        printf("%f\t%f\t%f\n\r", sensorTiefbass[5], sensorTiefbass[0], sensorTiefbass[1]);   //Plottet die unteren Sensoren + Mitte-Oben
-        
-        zaehler++;
-        if(zaehler % 120 == 0){
-            wait(3.5f);
-        }
-        if(zaehler % 40 == 0) title();  //Erstellt nach 40 Zeilen eine neue Seite
     }
 }
\ No newline at end of file