Revision 2:365bf16abbf6, committed 2017-05-16

Comitter:

EHess

Date:

Tue May 16 14:14:08 2017 +0000

Parent:

1:d40ff07e2fe0

Commit message:

zdf

Changed in this revision

--- a/IRSensor.cpp	Wed May 10 09:14:12 2017 +0000
+++ b/IRSensor.cpp	Tue May 16 14:14:08 2017 +0000
@@ -11,24 +11,28 @@
 
 IRSensor::IRSensor() {}
 
-IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) {
+IRSensor::IRSensor(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+{
     this->distance = distance;  //Weist den Objektvariablen, die eingegebenen Werte zu
     this->bit0 = bit0;
     this->bit1 = bit1;
     this->bit2 = bit2;
     this->number = number;
+    this->filteredValue = 0.0f;
 }
 
 //Destruktor -> Löscht das IRSensor-Objekt
 IRSensor::~IRSensor() {}
 
 //Initialisiert nachträglich
-void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) {
+void IRSensor::init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
+{
     this->distance = distance;  //Weist den Objektvariablen, die eingegebenen Werte zu
     this->bit0 = bit0;
     this->bit1 = bit1;
     this->bit2 = bit2;
     this->number = number;
+    this->filteredValue = 0.0f;
 }
 
 //Distanzrechner
@@ -39,5 +43,13 @@
     *bit2 = (number >> 2) & 1;  //Vergleicht das dritte Bit von rechts
 
     float d = distance->read();
+
+    this->filteredValue = 0.99f* this->filteredValue + 0.01f * d;
     return d;
-}
\ No newline at end of file
+}
+
+float IRSensor::readFitered()
+{
+    return this->filteredValue;
+}
+

--- a/IRSensor.h	Wed May 10 09:14:12 2017 +0000
+++ b/IRSensor.h	Tue May 16 14:14:08 2017 +0000
@@ -13,6 +13,7 @@
         ~IRSensor();
         void init(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number);
         float read();
+        float readFitered();
 
     private:
         AnalogIn*       distance;
@@ -20,6 +21,7 @@
         DigitalOut*     bit1;
         DigitalOut*     bit2;
         int             number;
+        float           filteredValue;
 };
 
 #endif
\ No newline at end of file

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