File content as of revision 12:c0bcb95885dd:

#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 )
{
    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;

}

//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::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->drive();
    }
    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, max, wallF, wallL, wallR, legoF, legoL, legoR };
    
    static int state = neutral;
    
    static int counter = 0;     //counter is used blabla
    
    static int rando = -1;
    
    //static int lastAct = 0;
    
    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;
        
    }
    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
//}

//Add management for Overpower!! Pin PB_15