![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 11:292bdbd85a9c
- Parent:
- 10:f76476943a6c
- Child:
- 13:6c1f8e35b242
diff -r f76476943a6c -r 292bdbd85a9c Sources/Robot.cpp --- 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 //}