![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
hallo
Fork of PES1 by
Diff: Roboter.cpp
- Revision:
- 19:adae367247d4
- Parent:
- 18:3ee1b02ed3aa
- Child:
- 20:a90e5b54bf7b
diff -r 3ee1b02ed3aa -r adae367247d4 Roboter.cpp --- a/Roboter.cpp Tue May 09 15:25:54 2017 +0000 +++ b/Roboter.cpp Fri May 12 12:25:34 2017 +0000 @@ -4,7 +4,6 @@ Roboter::Roboter(AnalogIn* distance, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, PwmOut* pwmL, PwmOut* pwmR, Servo* servo1, Servo* servo2, EncoderCounter* counterLeft, EncoderCounter* counterRight, DigitalOut* enableMotorDriver) { - sensors[0].init( distance, bit0, bit1, bit2, 0); sensors[1].init( distance, bit0, bit1, bit2, 1); sensors[2].init( distance, bit0, bit1, bit2, 2); @@ -26,24 +25,33 @@ } -float Roboter::readSensor1() // Sensoren auslesen +int Roboter::readSensors() // Sensoren auslesen { - return sensors[0].read(); + int l = 0; + float x = 0.12; // Distanz ab welcher sensoren reagieren sollen + + if(sensors[1] < x || sensors[5] < x) { // Sensor rechts und links + l = 6; + } else { + l = 1; + } + + return l; } -void Roboter::bandeAusweichen() // Hindernissen ausweichen +void Roboter::ausweichen1() // Hindernissen ausweichen { - float x=0.13f; // Distanz ab welcher sensoren reagieren sollen + float x = 0.15; // Distanz ab welcher sensoren reagieren sollen - while(sensors[0] < x && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten + while(sensors[0] < (x-0.02f) && sensors[1] < x && sensors[5] < x) { // alle sensoren aktiv, roboter fährt nach hinten desiredSpeedLeft = -v; desiredSpeedRight = v; } - while(sensors[0] < x && sensors[5] > x) { // sensor vorne, roboter dreht nach links + while(sensors[0] < (x-0.02f) && sensors[5] > x) { // sensor vorne, roboter dreht nach links desiredSpeedLeft = -v; desiredSpeedRight = -v; @@ -55,7 +63,7 @@ } - while(sensors[5] < x && sensors[1]>(x+0.02f)) { // sensor links, roboter dreht nach rechts + while(sensors[5] < x && sensors[1] > x) { // sensor links, roboter dreht nach rechts desiredSpeedLeft = v; desiredSpeedRight = v; @@ -63,41 +71,115 @@ } +int Roboter::ausweichen2() // Hindernissen ausweichen beim Anfahren des Klotzes +{ + float x = 0.12; // Distanz ab welcher sensoren reagieren sollen + static int l = 0; + static int r = 0; + static int ii = 0; + int s = 6; + + if(sensors[1] < x && l == 0) { + r = 1; + } + + if(sensors[5] < x && r == 0) { + l = 1; + } + + if(r == 1) { + if(ii < 9000) { + desiredSpeedLeft = -(v+30); + desiredSpeedRight = v-10; + } + if(ii > 8999 && ii < 24000) { + desiredSpeedLeft = v*2; + desiredSpeedRight = -v*2; + } + if(ii > 23999 && ii < 33000) { + desiredSpeedLeft = v*1.8f; + desiredSpeedRight = v*1.8f; + } + ii++; + if(ii > 32999) { + desiredSpeedLeft = 0; + desiredSpeedRight = 0; + r = 0; + ii = 0; + s = 1; + } + } + + if(l == 1) { + if(ii < 9000) { + desiredSpeedLeft = -(v-10); + desiredSpeedRight = v+30; + } + if(ii > 8999 && ii < 24000) { + desiredSpeedLeft = v*2; + desiredSpeedRight = -v*2; + } + if(ii > 23999 && ii < 33000) { + desiredSpeedLeft = -v*1.8f; + desiredSpeedRight = -v*1.8f; + } + ii++; + if(ii > 32999) { + desiredSpeedLeft = 0; + desiredSpeedRight = 0; + l = 0; + ii = 0; + s = 1; + } + } + return s; +} + int Roboter::pickup() // Klotz aufnehmen { + //static int anz = 0; static int ii = 0; int r; // Rückegabewert desiredSpeedLeft = 0; desiredSpeedRight = 0; - if(ii < 70) { // Arm nach unten fahren + if(ii < 7000) { // Arm nach unten fahren servo2->SetPosition(2250); } - if(ii > 69 && ii < 110) { // gerade fahren + if(ii > 6999 && ii < 12000) { // gerade fahren desiredSpeedLeft = v; desiredSpeedRight = -v; } - if(ii > 109 && ii < 160) { // gerade fahren + Greifer schliessen + if(ii > 11999 && ii < 17000) { // gerade fahren + Greifer schliessen desiredSpeedLeft = v; desiredSpeedRight = -v; servo1->SetPosition(500); } - if(ii > 159 && ii < 230) { // Arm nach oben fahren + if(ii > 16999 && ii < 19000) { + desiredSpeedLeft = v; + desiredSpeedRight = -v; + } + if(ii > 18999 && ii < 26000) { // Arm nach oben fahren desiredSpeedLeft = 0; desiredSpeedRight = 0; servo2->SetPosition(700); } - if(ii > 229 && ii < 280) { // Greifer öffnen + if(ii > 25999 && ii < 31000) { // Greifer öffnen servo1->SetPosition(1500); } ii++; + //anz++; r = 5; // Rückegabewert - if( ii > 279 ) { + if( ii > 30999 ) { r = 1; ii = 0; } + /*if(anz == 31001) { + r = 7; + anz = 0; + }*/ return r; } @@ -115,19 +197,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 + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v/2) + (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 + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v/2) + (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*1.5f + (cam * (maxWert /100.0f)); // Berechnung des speedValue's + speedValue = (v*2) + (cam * (maxWert /100.0f)); // Berechnung des speedValue's desiredSpeedLeft = speedValue; desiredSpeedRight = -speedValue; } @@ -137,12 +219,11 @@ { int r; static int time2 = 0; - if(time2 < 200) { // 1s gerade aus fahren - desiredSpeedLeft = v*3; - desiredSpeedRight = -v*3; + if(time2 < 100) { // 1s gerade aus fahren + desiredSpeedLeft = v*5; + desiredSpeedRight = -v*5; time2 ++; r = 1; // state - wait(0.01f); } else { time2 = 0; desiredSpeedLeft = 0.0f; @@ -152,16 +233,36 @@ return r; } +int Roboter::back() +{ + static int ii = 0; + int s; + + if(ii < 3000) { + desiredSpeedLeft = -v; + desiredSpeedRight = v; + s = 7; + } + ii++; + + if(ii > 2999) { + desiredSpeedLeft = 0; + desiredSpeedRight = 0; + ii = 0; + s = 1; + } + return s; +} + int Roboter::kreisFahren() { int r; static int time1 = 0; - if(time1 < 310) { // 1s im Kreis drehen + if(time1 < 40) { // 1s im Kreis drehen desiredSpeedLeft = v*1.7f; desiredSpeedRight = v*1.7f; time1 ++; r = 1; // state - wait(0.01f); } else { time1 = 0; desiredSpeedLeft = 0.0f; @@ -183,6 +284,9 @@ if(cam == 0.0f) { s = temp; } + if(cam == 1) { + s = 1; + } return s;