sef
Dependencies: mbed
Fork of Bewegungen_mit_Sensor by
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
diff -r d40ff07e2fe0 -r 365bf16abbf6 IRSensor.cpp --- 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; +} +
diff -r d40ff07e2fe0 -r 365bf16abbf6 IRSensor.h --- 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
diff -r d40ff07e2fe0 -r 365bf16abbf6 main.cpp --- 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