![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 4:67d7177c213f
- Parent:
- 3:fa61be4ecaa6
- Child:
- 5:1aaf5de776ff
--- a/Sources/Robot.cpp Wed Mar 22 13:33:07 2017 +0000 +++ b/Sources/Robot.cpp Wed Apr 19 12:23:52 2017 +0000 @@ -1,8 +1,9 @@ #include "Robot.h" +#include "Declarations.h" /* Work in progress -------------------------------------------- */ -Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage ) +Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS ) { this->powerSignal = enableSignal; this->left = left; @@ -14,10 +15,15 @@ this->leds = leds; this->FarbVoltage = FarbVoltage; + this->frontS = frontS; + this->leftS = leftS; + this->rightS = rightS; + + this->arm = arm; } -/* work in progress ------------------------------------------- */ +//Drive functions void Robot::drive() { //pwm determine what direction it goes. @@ -26,6 +32,14 @@ *right= 0.4f; } +void Robot::driveB() +{ + //pwm determine what direction it goes. + *powerSignal = 1; + *left= 0.4f; + *right= 0.6f; +} + void Robot::turnLeft() { @@ -35,6 +49,15 @@ } +void Robot::turnLeftS() +{ + + *powerSignal = 1; + *left= 0.45f; + *right= 0.45f; + +} + void Robot::turnRight() { *powerSignal = 1; @@ -42,6 +65,15 @@ *right= 0.6f; } +void Robot::turnRightS() +{ + + *powerSignal = 1; + *left= 0.55f; + *right= 0.55f; + +} + void Robot::turnAround(int left) { *powerSignal = 1; @@ -62,67 +94,238 @@ *right= 0.5f; } -void Robot::search(int* counter, Timer* t){ - int rando; + +//Functions that use the drive functions +void Robot::wallRight(int* counter, int* timer, int* lastAct){ + *counter += 1; + + if (*lastAct != 1){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 1; + } + + this->turnLeft(); +} + +void Robot::counterMax(int* counter, int* timer, int* lastAct, int* rando){ + if (*lastAct != 0){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 0; + } + + if (*rando == -1){ //If rando was unused, set a new number. + *rando = rand() % 2; + } + + if (this->sensors[FWD] < NEAR){ //While something is seen turn around. + this->turnAround(*rando); + } + + else{ + *rando = -1; + *counter = 0; + } +} + +void Robot::wallLeft(int* counter, int* timer, int* lastAct){ + *counter += 1; + + if (*lastAct != 2){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 2; + } + + this->turnRight(); +} + +void Robot::wallFront(int* counter, int* timer, int* lastAct){ + if (*lastAct != 3){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 3; + } + + *counter = MAX; //By setting the counter to MAX, next time it will go into the first if-statement (action 0). +} + + +void Robot::legoFront(int* counter, int* timer, int* lastAct, int* legoFound){ + //*counter += 1; + *legoFound = 0; + + if (*lastAct != 4){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 4; + } + + if (this->sensors[FWD] < NEAR){ + *legoFound = -1; + } + + else if (this->sensors[FWD_L] > 0.15f){ + this->drive(); + } + else{ + this->stop(); + *legoFound = 3; + } +} + +void Robot::legoRight(int* counter, int* timer, int* lastAct, int* legoFound){ + //*counter += 1; + *legoFound = 1; + + if (*lastAct != 5){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 5; + } + + if (this->sensors[FWD_L] > NEAR){ + this->turnRight(); + } + else{ + this->stop(); + *legoFound = 3; + } +} + +void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){ + //*counter += 1; + *legoFound = 2; - if (*counter >= 5) { - //t->reset(); + if (*lastAct != 6){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 6; + } + + if (this->sensors[FWD_L] > NEAR){ + this->turnLeft(); + } + else{ + this->stop(); + *legoFound = 3; + } +} + + +void Robot::nothingFound(int* counter, int* timer, int* lastAct){ + *counter = 0; + if (*lastAct != 7){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 7; + } + + this->drive(); +} + + +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(); + + 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 + + + if (*counter >= MAX/* && legoFound == -1*/) { //Robot is stuck turning left & right + counterMax(counter, timer, &lastAct, &rando); + } + + //Wall actions: + else if (this->sensors[RIGHT] < NEAR && legoFound == -1){ //Robot has spotted an obstacle on the right. + wallRight(counter, timer, &lastAct); + } + + else if (this->sensors[LEFT] < NEAR && legoFound == -1) { //Robot has spotted an obstacle on the left. + wallLeft(counter, timer, &lastAct); + } + + 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 && legoFound == -1 || legoFound == 0){ //There's a Lego in front. + legoFront(counter, timer, &lastAct, &legoFound); + } + + else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){ //There's a Lego on the right. + legoRight(counter, timer, &lastAct, &legoFound); + } + + 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; - rando = rand() % 2; - while (this->sensors[FWD] < NEAR){ + //Old! Check if still useful!{ + //check if this was the last action + //reset timer + + //dont add up counter + //use arm + //set legoFound to -1 + //} + } + + + //Nothing found + else { + nothingFound(counter, timer, &lastAct); + } +} + +void Robot::lego(int* counter, Timer* t){ + + if (this->sensors[RIGHT_L] < NEAR_LEGO){ + t->reset(); + + *counter += 1; + while (this->sensors[FWD] > NEAR_LEGO){ if ( t->read() > TIMEOUT ){ break; } - this->leds[FWD] = 1; - this->turnAround(rando); + this->turnRight(); } - this->leds[FWD] = 0; + this->stop(); } - else if (this->sensors[RIGHT] < NEAR){ + else if (this->sensors[LEFT] < NEAR_LEGO){ t-> reset(); *counter += 1; - while (this->sensors[RIGHT] < NEAR){ + while (this->sensors[FWD] > NEAR_LEGO){ if ( t->read() > TIMEOUT ){ break; } this->turnLeft(); - this->leds[RIGHT] = 1; } - this->leds[RIGHT] = 0; + this->stop(); } - else if (this->sensors[LEFT] < NEAR){ - t-> reset(); - - *counter += 1; - while (this->sensors[LEFT] < NEAR){ - if ( t->read() > TIMEOUT ){ - break; - } - - this->turnRight(); - this->leds[LEFT] = 1; + else if (this->sensors[FWD] < 0.12f){ + while (this->sensors[FWD] < 0.12f){ + this->driveB(); } - this->leds[LEFT] = 0; - } - - else if (this->sensors[FWD] < NEAR) { - t-> reset(); - - rando = rand() % 2; - while (this->sensors[FWD] < NEAR){ - if ( t->read() > TIMEOUT ){ - break; - } - - this->leds[FWD] = 1; - this->turnAround(rando); + while (this->sensors[FWD] < NEAR_LEGO){ + this->turnRight(); } - this->leds[FWD] = 0; + while(1){ + this->stop(); + } } else {