Revision 18:a158713a0049, committed 2017-05-19

Comitter:

EpicG10

Date:

Fri May 19 06:40:33 2017 +0000

Parent:

17:4e1be70bdedb

Commit message:

a

Changed in this revision

--- a/Sources/Robot.cpp	Thu May 18 14:27:09 2017 +0000
+++ b/Sources/Robot.cpp	Fri May 19 06:40:33 2017 +0000
@@ -57,8 +57,8 @@
 {
     
     *powerSignal = 1;
-    *left=  0.45f;
-    *right= 0.45f;
+    *left=  0.42f;
+    *right= 0.42f;
 
 }
 
@@ -73,8 +73,8 @@
 {
     
     *powerSignal = 1;
-    *left=  0.55f;
-    *right= 0.55f;
+    *left=  0.58f;
+    *right= 0.58f;
 
 }
 
@@ -264,7 +264,7 @@
     */
     
     
-        //printf("\n\rcurrent robot state:  %d", state);
+        printf("\n\rcurrent robot state:  %d", state);
     switch( state ){
         case neutral:
             if( counter > MAX ){
@@ -279,7 +279,7 @@
             else if( this->see(RIGHT) < NEAR ){
                 state = wallR;
             }
-            else if( this->see(FWD_L) < NEAR_LEGO + 0.015f ){
+            else if( this->see(FWD_L) < NEAR_LEGO + 0.03f ){
                 state = legoF;
             }
             else if( this->see(LEFT_L) < NEAR_LEGO ){
@@ -295,7 +295,7 @@
             break;
             
         case max: {
-            int time = 0;
+            int  time = 0;
             if( time < 15 && this->see(FWD) > NEAR ){
                 rando == -1 ? rando = rand() % 2 : rando = rando;
                 this->turnAround(rando);
@@ -346,40 +346,71 @@
                 state = wallF;
                 //stay = -1;
             }
-            else if( this->see(FWD_L) < 0.19f ){
+            else if( this->see(FWD_L) < 0.14f ){
                 this->driveBackSlowly();
             }
             else{
-                state = neutral;
+                //state = neutral;
                 //stay = -1;
                 counter = 0;
                 this->stop();
+                state=neutral;
                 return 1;
             }
             break;
         
         case legoL:
             counter++;
-            if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
-                this->turnLeft();
-            }
+            if(counter>MAX) state=neutral;
+            static int i=0;
+           if(i<4 && see(LEFT_L)< 0.15){
+                this->driveB();
+                i++;
+                }
             else{
-                state = neutral;
-                this->drive();
+            this->stop();
+                
+            if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){
+                this->turnLeftS();
+                this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0;
+            }
+            else if(see(FWD) < NEAR_LEGO + 0.05f ){
+                state= wallF;
+                }
+            else{
+                state = legoF;
+                i=0;
+                //this->drive();
                 //stay = 1;
             }
+            }
             break;
         
         case legoR:
+            if(counter>MAX) state=neutral;
             counter++;
-            if( this->see(FWD_L) > NEAR_LEGO + 0.05f ){
-                this->turnRight();
+            static int a=0;
+           if(a<4 && this->see(RIGHT_L)< 0.15){
+                this->driveB();
+                a++;
+                }
+            else{
+            this->stop();
+            
+            if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){
+                this->turnRightS();
+                this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0;
             }
+            else if(see(FWD) < NEAR_LEGO + 0.05f ){
+                state= wallF;
+                }
             else{
-                this->drive();
-                state = neutral;
+                //this->drive();
+                a=0;
+                state = legoF;
                 //stay = 1;
             }
+            }
             break;
         
     }

--- a/Sources/USsensor.cpp	Thu May 18 14:27:09 2017 +0000
+++ b/Sources/USsensor.cpp	Fri May 19 06:40:33 2017 +0000
@@ -21,8 +21,9 @@
         float dist = Usensor->get_dist_cm();
         static float distFiltered = dist;
         
-        distFiltered = 0.05f * distFiltered + 0.95f * dist;
-        return distFiltered / 100;
+        //distFiltered = 0.05f * distFiltered + 0.95f * dist;
+        printf("\n\rmeas= %.3f",distFiltered/100);
+        return dist / 100;
         //return dist/100.0f;
     }
     else{

--- a/Sources/main.cpp	Thu May 18 14:27:09 2017 +0000
+++ b/Sources/main.cpp	Fri May 19 06:40:33 2017 +0000
@@ -73,18 +73,13 @@
     //int done = 0;
     int color;
     
-    enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
+    enum states { search = 0, LeisteDown, turn, push, pos, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
 
     int state = search;
     
-                static float messung = 0;
+               // static float messung = 0;
                 
-    while( 1 ){
-        printf("\n\r%f", sam.see(FWD_L));
-        
-        wait(1.0f);
-            
-    }
+   
     
     while( 1 ) {
         
@@ -96,7 +91,7 @@
         //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
         
         
-        //printf("\n\rcurrent main state: %d", state);
+        printf("\n\rcurrent main state: %d", state);
         
         sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
         switch( state ) {
@@ -118,8 +113,8 @@
                 break;
             
             case turn:
-                static int i = 0;
-                if( i > 7 ){
+               /* static int i = 0;
+                if( i > 4 ){
                     sam.stop();
                     state = push;
                     i = 0;
@@ -127,36 +122,54 @@
                 else{
                     i++;
                     sam.turnRight();
-                }
+                }*/
+                state = push;
                 break;
                 
             case push:{
                 static int i = 0;
-                if( i > 5 ){
+                if( i > 6 ){
+                    sam.stop();
+                    i = 0;
+                    state = pos;
+                    timer = 0;
+                }
+                else{
+                    sam.drive();
+                    i++;
+                }
+                break;
+                }
+                
+            case pos:{
+                static int i = 0;
+                if( i > 4 ){
                     sam.stop();
                     i = 0;
                     state = backOff;
                     timer = 0;
                 }
                 else{
+                    sam.turnRightS();
+                    i++;
+                }
+            
+                break;
+            }
+            
+            case backOff:{
+                static int i = 0;
+                if( i > 2 ){
+                    sam.stop();
+                    i = 0;
+                    state = forward;
+                    timer = 0;
+                }
+                else{
                     sam.driveSlowly();
                     i++;
                 }
-                break;
-            }
-            
-            case backOff:{
-                static int i = 0;
-                if( i > 1 ){
-                    sam.stop();
-                    i = 0;
-                    state = forward;
-                    timer = 0;
-                }
-                else{
-                    sam.driveBackSlowly();
-                    i++;
-                }
+                //state = forward;
                 break;
             }