File content as of revision 18:a158713a0049:

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

}

//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.42f;
    *right= 0.42f;

}

void Robot::turnRight()
{
    *powerSignal = 1;
    *left=  0.6f;
    *right= 0.6f;
}

void Robot::turnRightS()
{
    
    *powerSignal = 1;
    *left=  0.58f;
    *right= 0.58f;

}

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, 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;
    
    //static int stay =       -1;     //Stay is used to remain in a certain state
    /*
    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);
    switch( state ){
        case neutral:
            if( counter > MAX ){
                state = max;
            }
            else if( this->see(FWD) < NEAR ){
                state = wallF;
            }
            else if( this->see(LEFT) < NEAR ){
                state = wallL;
            }
            else if( this->see(RIGHT) < NEAR ){
                state = wallR;
            }
            else if( this->see(FWD_L) < NEAR_LEGO + 0.03f ){
                state = legoF;
            }
            else if( this->see(LEFT_L) < NEAR_LEGO ){
                state = legoL;
            }
            else if( this->see(RIGHT_L) < NEAR_LEGO ){
                state = legoR;
            }
            else{
                this->drive();
                counter = 0;
            }
            break;
            
        case max: {
            int  time = 0;
            if( time < 15 && this->see(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->see(FWD) < NEAR ){
                rando == -1 ? rando = rand() % 2 : rando = rando;
                this->turnAround(rando);
            }
            else{
                state = neutral;
                rando = -1;
            }
            break;
        
        case wallL:
            counter++;
            if( this->see(LEFT) < NEAR ){
                this->turnRight();
            }
            else{
                state = neutral;
            }
            break;
            
        case wallR:
            counter++;
            if( this->see(RIGHT) < NEAR ){
                this->turnLeft();
            }
            else{
                state = neutral;
            }
            break;
        
        case legoF:
            //counter++;
            if( this->see(FWD) < NEAR ){
                state = wallF;
                //stay = -1;
            }
            else if( this->see(FWD_L) < 0.14f ){
                this->driveBackSlowly();
            }
            else{
                //state = neutral;
                //stay = -1;
                counter = 0;
                this->stop();
                state=neutral;
                return 1;
            }
            break;
        
        case legoL:
            counter++;
            if(counter>MAX) state=neutral;
            static int i=0;
           if(i<4 && see(LEFT_L)< 0.15){
                this->driveB();
                i++;
                }
            else{
            this->stop();
                
            if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){
                this->turnLeftS();
                this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0;
            }
            else if(see(FWD) < NEAR_LEGO + 0.05f ){
                state= wallF;
                }
            else{
                state = legoF;
                i=0;
                //this->drive();
                //stay = 1;
            }
            }
            break;
        
        case legoR:
            if(counter>MAX) state=neutral;
            counter++;
            static int a=0;
           if(a<4 && this->see(RIGHT_L)< 0.15){
                this->driveB();
                a++;
                }
            else{
            this->stop();
            
            if( this->see(FWD_L) > NEAR_LEGO + 0.15f ){
                this->turnRightS();
                this->USsensor.read()<0.15 ? this->leds[5]=1:leds[5]=0;
            }
            else if(see(FWD) < NEAR_LEGO + 0.05f ){
                state= wallF;
                }
            else{
                //this->drive();
                a=0;
                state = legoF;
                //stay = 1;
            }
            }
            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;
}