File content as of revision 0:15a8480061e8:

#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;
}