c
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Official by
Diff: Sources/Robot.cpp
- Revision:
- 0:15a8480061e8
diff -r 000000000000 -r 15a8480061e8 Sources/Robot.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/Robot.cpp Mon May 22 11:24:46 2017 +0000 @@ -0,0 +1,368 @@ +#include "Robot.h" +#include "Declarations.h" + +/* Work in progress -------------------------------------------- */ + +Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, Servo* Leiste, Ultraschall* USsensor, Pixy* pixy) +{ + this->left = left; + this->right = right; + this->powerSignal = powerSignal; + //this->errorMotor = errorMotor; + + 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; + this->Greifer = Greifer; + this->Leiste = Leiste; + this->USsensor = USsensor; + + this->pixy = pixy; + +} + +//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::setLeft(float pwm){ + *powerSignal = 1; + *left = pwm; +} + +void Robot::setRight(float pwm){ + *powerSignal = 1; + *right = pwm; +} + +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; +} + +void Robot::driveSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->drive(); + } + else{ + this->stop(); + } +} + +void Robot::driveBackSlowly(){ + static int i = 0; + i++; + if( i % 2 ){ + this->driveB(); + } + else{ + this->stop(); + } +} + +/* +//Functions that use the drive functions +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::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::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->driveSlowly(); + } + else{ + this->stop(); + *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(); +}*/ + + +int Robot::search(int* timer){ + enum states {neutral = 0, counterMax, wallF, wallL, wallR }; + + static int state = neutral; + + static int rando = -1; + + static int lastAct = 0; + + int oldx = this->pixy->getX(); + + static int counter = 0; + + /* + this->sensors[FWD_L] < NEAR ? this->leds[4] = 1 : this->leds[4] = 0; + this->sensors[RIGHT_L] < NEAR ? this->leds[RIGHT_L] = 1 : this->leds[RIGHT_L] = 0; + this->sensors[LEFT_L] < NEAR ? this->leds[LEFT_L] = 1 : this->leds[LEFT_L] = 0; + */ + + + //printf("\n\rcurrent robot state: %d", state); + if( this->pixy->getDetects() ) return 1; + switch( state ){ + case neutral: + if( counter > MAX ){ + state = counterMax; + } + else if( this->sensors[FWD].read() < NEAR ){ + state = wallF; + counter = 0; + } + else if( this->sensors[LEFT].read() < NEAR ){ + state = wallL; + counter = 0; + } + else if( this->sensors[RIGHT].read() < NEAR ){ + state = wallR; + counter = 0; + } + else{ + this->drive(); + counter = 0; + } + break; + + case counterMax:{ + int i = 0; + if( i < 1000 ){ + rando == -1 ? rando = rand() % 2 : rando = rando; + this->turnAround(rando); + i++; + } + else{ + state = neutral; + rando = -1; + counter = 0; + i = 0; + } + break; + } + + case wallF: + if( this->sensors[FWD].read() < NEAR ){ + rando == -1 ? rando = rand() % 2 : rando = rando; + this->turnAround(rando); + counter++; + } + else{ + state = neutral; + rando = -1; + } + break; + + case wallL: + if( this->sensors[LEFT].read() < NEAR ){ + this->turnRight(); + counter++; + } + else{ + state = neutral; + } + break; + + case wallR: + if( this->sensors[RIGHT].read() < NEAR ){ + this->turnLeft(); + counter++; + } + else{ + state = neutral; + } + break; + + } + return 0; +} + +float Robot::see(int sensor){ + if( sensor == FWD_L ){ + return this->USsensor.read(); + } + else{ + return this->sensors[sensor].read(); + } +} + +int Robot::getErrorMotor(){ + return 0; //errorMotor; +} \ No newline at end of file