zhaw_st16b_pes2_10
/
PES-Yanick2
ahah
Fork of PES_Official by
Sources/Robot.cpp
- Committer:
- beacon
- Date:
- 2017-04-19
- Revision:
- 4:67d7177c213f
- Parent:
- 3:fa61be4ecaa6
- Child:
- 5:1aaf5de776ff
File content as of revision 4:67d7177c213f:
#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 ) { 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){ //*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 (*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; //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