File content as of revision 5:acb938f45b9c:

#include "Robot.h"
#include "Declarations.h"


Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* powerSignal, DigitalOut* leds, AnalogIn* FarbVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, ServoArm* Arm, Servo* Greifer, 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->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();
    }
}

void Robot::search(int* time){
    enum states {neutral = 0, counterMax, wallF, wallL, wallR};
    static int state = neutral;
    static int rando = -1;
    int oldx = this->pixy->getX();
    static int counter = 0;
  
    //if( this->pixy->getDetects() ) return 1;
    switch( state ){
        case neutral:
            if( counter > MAX ){
                state = counterMax;
            }
            else if( this->sensors[FWD].read() < NEAR ){
                state = wallF;
                time = 0;
            }
            else if( this->sensors[LEFT].read() < NEAR ){
                state = wallL;
                time = 0;
            }
            else if( this->sensors[RIGHT].read() < NEAR ){
                state = wallR;
                time = 0;
            }
            else{
                this->drive();
                time = 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;
        
    }
}


int Robot::getErrorMotor(){
    return 0; //errorMotor;
}