![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
hallo
Fork of PES1 by
Diff: Roboter.cpp
- Revision:
- 20:a90e5b54bf7b
- Parent:
- 19:adae367247d4
- Child:
- 21:69ee872b8ee9
diff -r adae367247d4 -r a90e5b54bf7b Roboter.cpp --- a/Roboter.cpp Fri May 12 12:25:34 2017 +0000 +++ b/Roboter.cpp Sat May 13 16:50:57 2017 +0000 @@ -27,16 +27,14 @@ int Roboter::readSensors() // Sensoren auslesen { - int l = 0; - float x = 0.12; // Distanz ab welcher sensoren reagieren sollen + int lr = 1; + float x = 0.13; // Distanz ab welcher sensoren reagieren sollen if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links - l = 6; - } else { - l = 1; + lr = 6; } - - return l; + //printf("%d",lr); + return lr; } @@ -46,26 +44,26 @@ float x = 0.15; // Distanz ab welcher sensoren reagieren sollen while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten - desiredSpeedLeft = -v; - desiredSpeedRight = v; + desiredSpeedLeft = -v*2; + desiredSpeedRight = v*2; } while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links - desiredSpeedLeft = -v; - desiredSpeedRight = -v; + desiredSpeedLeft = -v*2; + desiredSpeedRight = -v*2; } while(sensors[1] < x) { // sensor rechts, roboter dreht nach links - desiredSpeedLeft = -v; - desiredSpeedRight = -v; + desiredSpeedLeft = -v*2; + desiredSpeedRight = -v*2; } while(sensors[5] < x && sensors[1] > x) { // sensor links, roboter dreht nach rechts - desiredSpeedLeft = v; - desiredSpeedRight = v; + desiredSpeedLeft = v*2; + desiredSpeedRight = v*2; } @@ -73,7 +71,7 @@ int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes { - float x = 0.12; // Distanz ab welcher sensoren reagieren sollen + float x = 0.13; // Distanz ab welcher sensoren reagieren sollen static int l = 0; static int r = 0; static int ii = 0; @@ -88,20 +86,24 @@ } if(r == 1) { - if(ii < 9000) { + if(ii < 500) { // linksdrehend rückwärts fahren desiredSpeedLeft = -(v+30); - desiredSpeedRight = v-10; + desiredSpeedRight = v+30; } - if(ii > 8999 && ii < 24000) { + if(ii > 499 && ii < 1100) { // links drehen + desiredSpeedLeft = -v*2; + desiredSpeedRight = -v*2; + } + if(ii > 1099 && ii < 2900) { // gerade fahren desiredSpeedLeft = v*2; desiredSpeedRight = -v*2; } - if(ii > 23999 && ii < 33000) { - desiredSpeedLeft = v*1.8f; - desiredSpeedRight = v*1.8f; + if(ii > 2899 && ii < 3900) { // rechts drehen + desiredSpeedLeft = v*2; + desiredSpeedRight = v*2; } ii++; - if(ii > 32999) { + if(ii > 3899) { desiredSpeedLeft = 0; desiredSpeedRight = 0; r = 0; @@ -111,20 +113,24 @@ } if(l == 1) { - if(ii < 9000) { - desiredSpeedLeft = -(v-10); + if(ii < 500) { // rückwärts fahren + desiredSpeedLeft = -(v+30); desiredSpeedRight = v+30; } - if(ii > 8999 && ii < 24000) { + if(ii > 499 && ii < 1100) { // rechts drehen + desiredSpeedLeft = v*2; + desiredSpeedRight = v*2; + } + if(ii > 1099 && ii < 2900) { // gerade fahren desiredSpeedLeft = v*2; desiredSpeedRight = -v*2; } - if(ii > 23999 && ii < 33000) { - desiredSpeedLeft = -v*1.8f; - desiredSpeedRight = -v*1.8f; + if(ii > 2899 && ii < 3900) { // links drehen + desiredSpeedLeft = -v*2; + desiredSpeedRight = -v*2; } ii++; - if(ii > 32999) { + if(ii > 3899) { desiredSpeedLeft = 0; desiredSpeedRight = 0; l = 0; @@ -138,48 +144,46 @@ int Roboter::pickup() // Klotz aufnehmen { - //static int anz = 0; static int ii = 0; int r; // Rückegabewert desiredSpeedLeft = 0; desiredSpeedRight = 0; - if(ii < 7000) { // Arm nach unten fahren + if(ii < 650) { // Arm nach unten fahren servo2->SetPosition(2250); } - if(ii > 6999 && ii < 12000) { // gerade fahren + if(ii > 649 && ii < 1100) { // gerade fahren desiredSpeedLeft = v; desiredSpeedRight = -v; } - if(ii > 11999 && ii < 17000) { // gerade fahren + Greifer schliessen - desiredSpeedLeft = v; - desiredSpeedRight = -v; + if(ii > 1099 && ii < 1500) { // gerade fahren + Greifer schliessen + if(ii < 1400) { + desiredSpeedLeft = v; + desiredSpeedRight = -v; + } servo1->SetPosition(500); } - if(ii > 16999 && ii < 19000) { - desiredSpeedLeft = v; - desiredSpeedRight = -v; + if(ii > 1499 && ii < 1900) { // rückwärts fahren + desiredSpeedLeft = -v; + desiredSpeedRight = v; } - if(ii > 18999 && ii < 26000) { // Arm nach oben fahren + if(ii > 1899 && ii < 2550) { // Arm nach oben fahren desiredSpeedLeft = 0; desiredSpeedRight = 0; servo2->SetPosition(700); } - if(ii > 25999 && ii < 31000) { // Greifer öffnen + if(ii > 2549 && ii < 3000) { // Greifer öffnen servo1->SetPosition(1500); } + ii++; - //anz++; r = 5; // Rückegabewert - if( ii > 30999 ) { + if( ii > 2999 ) { r = 1; ii = 0; } - /*if(anz == 31001) { - r = 7; - anz = 0; - }*/ + return r; } @@ -197,19 +201,19 @@ } if(cam > 100 && cam <200 ) { // links fahren, wenn wir Werte von 100 bis 199 haben cam = cam -99.0f; // cam nimmt die Werte von 1 bis 100 an - speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = -speedValue; desiredSpeedRight = -speedValue; } if(cam > 200 && cam < 300) { // rechts fahren, wenn wir Werte von 200 bis 299 haben cam = cam -199.0f; // cam nimmt die Werte von 1 bis 100 an - speedValue = (v/2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = speedValue; } if(cam >= 300 && cam <400) { // gerade fahren, wenn wir Werte von 300 bis 399 haben cam = cam-299.0f; // cam nimmt die Werte von 1 bis 100 an - speedValue = (v*2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v*3) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = -speedValue; } @@ -219,9 +223,9 @@ { int r; static int time2 = 0; - if(time2 < 100) { // 1s gerade aus fahren - desiredSpeedLeft = v*5; - desiredSpeedRight = -v*5; + if(time2 < 10) { // 1s gerade aus fahren + desiredSpeedLeft = v*4; + desiredSpeedRight = -v*4; time2 ++; r = 1; // state } else { @@ -236,21 +240,27 @@ int Roboter::back() { static int ii = 0; - int s; + int s = 7; - if(ii < 3000) { + if(ii < 1000) { desiredSpeedLeft = -v; desiredSpeedRight = v; - s = 7; } + + if(ii > 999 && ii < 2000) { + desiredSpeedLeft = v*2; + desiredSpeedRight = v*2; + } + ii++; - if(ii > 2999) { + if(ii > 1999) { desiredSpeedLeft = 0; desiredSpeedRight = 0; ii = 0; s = 1; } + return s; } @@ -258,7 +268,7 @@ { int r; static int time1 = 0; - if(time1 < 40) { // 1s im Kreis drehen + if(time1 < 10) { // 1s im Kreis drehen desiredSpeedLeft = v*1.7f; desiredSpeedRight = v*1.7f; time1 ++; @@ -272,17 +282,20 @@ return r; } -int Roboter::stateAuswahl(float cam, int temp) +int Roboter::stateAuswahl(float cam, int tempState, int temp) { - int s; + int s = 1; if(cam >= 100.0f && cam < 400.0f ) { // Die Kamera erkennt ein grünen Klotz , Werte von 100 und 399 s = 4; } - if(cam == 400.0f) { // Roboter in Position + if(cam == 400.0f && temp != 5) { // Roboter in Position s = 5; } - if(cam == 0.0f) { - s = temp; + if(cam == 400.0f && temp == 5) { + s = 7; + } + if(cam < 1) { + s = tempState; } if(cam == 1) { s = 1;