![](/media/cache/group/default_image.jpg.50x50_q85.jpg)
vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/Robot.cpp
- Revision:
- 9:ac362674c480
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/Robot.cpp Tue May 02 08:39:35 2017 +0000 @@ -0,0 +1,348 @@ +#include "Robot.h" +#include "Declarations.h" + +/* Work in progress -------------------------------------------- */ + +Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, Servo* Arm ) +{ + this->powerSignal = enableSignal; + this->left = left; + this->right = right; + + this->left->period(0.00005f); + this->right->period(0.00005f); + + this->leds = leds; + + this->FarbVoltage = FarbVoltage; + this->frontS = frontS; + this->leftS = leftS; + this->rightS = rightS; + + this->Arm = Arm; + +} + +//Drive functions +void Robot::drive() +{ + //pwm determine what direction it goes. + *powerSignal = 1; + *left= 0.6f; + *right= 0.4f; +} + +void Robot::driveB() +{ + //pwm determine what direction it goes. + *powerSignal = 1; + *left= 0.4f; + *right= 0.6f; +} + +void Robot::turnLeft() +{ + + *powerSignal = 1; + *left= 0.4f; + *right= 0.4f; + +} + +void Robot::turnLeftS() +{ + + *powerSignal = 1; + *left= 0.45f; + *right= 0.45f; + +} + +void Robot::turnRight() +{ + *powerSignal = 1; + *left= 0.6f; + *right= 0.6f; +} + +void Robot::turnRightS() +{ + + *powerSignal = 1; + *left= 0.55f; + *right= 0.55f; + +} + +void Robot::turnAround(int left) +{ + *powerSignal = 1; + + if (left) { + turnLeft(); + } + + else { + turnRight(); + } +} + +void Robot::stop() +{ + *powerSignal = 1; + *left= 0.5f; + *right= 0.5f; +} + + +//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, int* found){ + //*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){ //If Sam sees a wall turn around + *legoFound = -1; + *counter = MAX; //setting counter to MAX will couse sam to turnAround + } + + if (this->sensors[FWD_L] > 0.16f){ + this->drive(); + } + else{ + *found = 1; + } +} + +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] > 0.22f){ + this->turnRight(); + } + else{ + this->stop(); + *legoFound = -1; + } +} + +void Robot::legoLeft(int* counter, int* timer, int* lastAct, int* legoFound){ + //*counter += 1; + *legoFound = 2; + + if (*lastAct != 6){ //If this wasn't the last called action, reset the timer. + *timer = 0; + *lastAct = 6; + } + + if (this->sensors[FWD_L] > 0.22f){ + this->turnLeft(); + } + else{ + this->stop(); + *legoFound = -1; + } +} + + +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) { //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, 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); + } + + 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; + + //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->turnRight(); + } + this->stop(); + } + + else if (this->sensors[LEFT] < NEAR_LEGO){ + t-> reset(); + + *counter += 1; + while (this->sensors[FWD] > NEAR_LEGO){ + if ( t->read() > TIMEOUT ){ + break; + } + + this->turnLeft(); + } + this->stop(); + } + + else if (this->sensors[FWD] < 0.12f){ + while (this->sensors[FWD] < 0.12f){ + this->driveB(); + } + while (this->sensors[FWD] < NEAR_LEGO){ + this->turnRight(); + } + while(1){ + this->stop(); + } + } + + else { + *counter = 0; + this->drive(); + } +} + +//void Robot::init(){ +// Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii); +//} + +//Remember to set +//DigitalOut powerMotor(PB_2) = 1; +//DigitalIn errorMotor(PB_14); +//if (errorMotor){ +//reset +//} + +//Add management for Overpower!! Pin PB_15 \ No newline at end of file