Diff: Roboter.cpp

Revision:

19:adae367247d4

Parent:

18:3ee1b02ed3aa

Child:

20:a90e5b54bf7b

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