Diff: Sources/Robot.cpp

Revision:

11:292bdbd85a9c

Parent:

10:f76476943a6c

Child:

13:6c1f8e35b242

--- a/Sources/Robot.cpp	Wed May 03 13:54:51 2017 +0000
+++ b/Sources/Robot.cpp	Sat May 06 13:33:23 2017 +0000
@@ -168,6 +168,7 @@
         this->drive();
     }
     else{
+        this->stop();
         *found = 1;
     }
 }
@@ -220,69 +221,178 @@
 }
 
 
-void Robot::search(int* counter, int* timer, int* found){
-    
-    static int rando = -1;          //Rando will be used, to randomize turnAround()
-                                    //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
+int Robot::search(int* timer){
+    enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
     
-    static int lastAct = -1;        //Is used, to check if the same action in Robot::search() was made multiple times.
-                                    //-1 := unused; x != -1 := action x was called last.
-                                    
-    static int legoFound = -1;      //Is used to determine, on what side the lego was found.
-                                    //-1 := unused; 0:= front; 1:= right, 2:= left.
-                                    
-    *timer += 1;                    //timer holds the time in 0.1s
+    static int state = neutral;
     
-    
-    if (*counter >= MAX) {          //Robot is stuck turning left & right
-        counterMax(counter, timer, &lastAct, &rando);
-    }
+    static int counter = 0;     //counter is used blabla
     
-    //Wall actions:
-    else if (this->sensors[RIGHT] < NEAR && legoFound == -1){   //Robot has spotted an obstacle on the right.
-        wallRight(counter, timer, &lastAct);
-    }
+    static int rando = -1;
     
-    else if (this->sensors[LEFT] < NEAR && legoFound == -1) {   //Robot has spotted an obstacle on the left.
-        wallLeft(counter, timer, &lastAct);
-    }
+    //static int lastAct = 0;
     
-    else if (this->sensors[FWD] < NEAR && legoFound == -1)  {   //Robot has spotted an obstacle infront
-        wallFront(counter, timer, &lastAct);
-    }
-    
-    //Lego actions:
-    else if (this->sensors[FWD_L] < NEAR_LEGO + 0.03f && legoFound == -1 || legoFound == 0){ //There's a Lego in front.
-        legoFront(counter, timer, &lastAct, &legoFound, found);
-    }
-
-    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){      //There's a Lego on the right.
-        legoRight(counter, timer, &lastAct, &legoFound);
+    switch( state ){
+        case neutral:
+            if( counter > MAX ){
+                state = max;
+            }
+            /*else if( this->sensors[FWD] < NEAR ){
+                state = wallF;
+            }*/
+            else if( this->sensors[LEFT] < NEAR ){
+                state = wallL;
+            }
+            else if( this->sensors[RIGHT] < NEAR ){
+                state = wallR;
+            }
+            else if( this->sensors[FWD_L] < NEAR ){
+                state = legoF;
+            }
+            else if( this->sensors[LEFT_L] < NEAR ){
+                state = legoL;
+            }
+            else if( this->sensors[RIGHT_L] < NEAR ){
+                state = legoR;
+            }
+            else{
+                this->drive();
+                counter = 0;
+            }
+            break;
+            
+        case max: {
+            int time = 0;
+            if( time < 15 && this->sensors[FWD] > NEAR ){
+                rando == -1 ? rando = rand() % 2 : rando = rando;
+                this->turnAround(rando);
+            }
+            else{
+                state = neutral;
+                counter = 0;
+                rando = -1;
+            }
+            break;
+        }
+        
+        case wallF:
+            counter++;
+            if( this->sensors[FWD] < NEAR ){
+                rando == -1 ? rando = rand() % 2 : rando = rando;
+                this->turnAround(rando);
+            }
+            else{
+                state = neutral;
+                rando = -1;
+            }
+            break;
+        
+        case wallL:
+            counter++;
+            if( this->sensors[LEFT] < NEAR ){
+                this->turnRight();
+            }
+            else{
+                state = neutral;
+            }
+            break;
+            
+        case wallR:
+            counter++;
+            if( this->sensors[RIGHT] < NEAR ){
+                this->turnLeft();
+            }
+            else{
+                state = neutral;
+            }
+            break;
+        
+        case legoF:
+            //counter++;
+            if( this->sensors[FWD_L] > 0.15f ){
+                this->drive();
+            }
+            else{
+                state = neutral;
+                counter = 0;
+                this->stop();
+                return 1;
+            }
+            break;
+        
+        case legoL:
+            counter++;
+            if( this->sensors[FWD_L] < NEAR_LEGO+0.05f ){
+                this->turnLeft();
+            }
+            else{
+                state = legoF;
+            }
+            break;
+        
+        case legoR:
+            counter++;
+            if( this->sensors[FWD_L] < NEAR_LEGO+0.05f ){
+                this->turnRight();
+            }
+            else{
+                state = legoF;
+            }
+            break;
+        
     }
-    
-    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){       //There's a Lego on the left.
-        legoLeft(counter, timer, &lastAct, &legoFound);
-    }
-    
-    //Lego is ready to be grabbed.
-    else if(legoFound == 3){
-        *found = 1;             //When found is set to 1, main will call arms functions
-        *counter = 0;
-        *timer = 0;
-        legoFound = -1;
-    }
-    
-    
-    //Nothing found
-    else {
-        nothingFound(counter, timer, &lastAct);
-    }
+    return 0;
 }
 
 int Robot::getErrorMotor(){
     return 0; //errorMotor;
 }
 
+/*int Robot::searchh(int* timer){
+    enum states {neutral = 0, max, wallF, wallL, wallR, legoF, legoL, legoR };
+    
+    static int state = neutral;
+    
+    static int counter = 0;     //counter is used blabla
+    
+    //static int lastAct = 0;
+    
+    switch( state ){
+        case neutral:
+            if( counter > MAX ){
+                state = max;
+            }
+            else if( this->sensora[FWD] < NEAR ){
+                state = wallF;
+                counter = 0;
+            }
+            else if( this->sensors[LEFT] < NEAR ){
+                state = wallL;
+                counter = 0;
+            }
+            else if( this->sensors[RIGHT] < NEAR ){
+                state = wallR;
+                counter = 0;
+            }
+            else if( this->sensors[FWD_L] < NEAR ){
+                state = legoF;
+                counter = 0;
+            }
+            else if( this->sensors[LEFT_L] < NEAR ){
+                state = legoL;
+                counter = 0;
+            }
+            else if( this->sensors[RIGHT_L] < NEAR ){
+                state = legoR;
+                counter = 0;
+            }
+            break;
+            
+        case max:
+            
+            break;
+  */          
+
 //reset
 //}