Diff: Sources/Robot.cpp

Revision:

18:a158713a0049

Parent:

17:4e1be70bdedb

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