![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
Drehen mit Halt und offset um zum Klotz zurück drehen. (Kann nur ein klotz aufheben)
Fork of DrehungMitStopp by
Diff: main.cpp
- Revision:
- 2:365bf16abbf6
- Parent:
- 1:d40ff07e2fe0
- Child:
- 3:5483d7c18c34
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