File content as of revision 10:f76476943a6c:

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


void Robot::search(int* counter, int* timer, int* found){
    
    static int rando = -1;          //Rando will be used, to randomize turnAround()
                                    //-1 := unused => set; 0 := turnRight(); 1 := turnLeft();
    
    static int lastAct = -1;        //Is used, to check if the same action in Robot::search() was made multiple times.
                                    //-1 := unused; x != -1 := action x was called last.
                                    
    static int legoFound = -1;      //Is used to determine, on what side the lego was found.
                                    //-1 := unused; 0:= front; 1:= right, 2:= left.
                                    
    *timer += 1;                    //timer holds the time in 0.1s
    
    
    if (*counter >= MAX) {          //Robot is stuck turning left & right
        counterMax(counter, timer, &lastAct, &rando);
    }
    
    //Wall actions:
    else if (this->sensors[RIGHT] < NEAR && legoFound == -1){   //Robot has spotted an obstacle on the right.
        wallRight(counter, timer, &lastAct);
    }
    
    else if (this->sensors[LEFT] < NEAR && legoFound == -1) {   //Robot has spotted an obstacle on the left.
        wallLeft(counter, timer, &lastAct);
    }
    
    else if (this->sensors[FWD] < NEAR && legoFound == -1)  {   //Robot has spotted an obstacle infront
        wallFront(counter, timer, &lastAct);
    }
    
    //Lego actions:
    else if (this->sensors[FWD_L] < NEAR_LEGO + 0.03f && legoFound == -1 || legoFound == 0){ //There's a Lego in front.
        legoFront(counter, timer, &lastAct, &legoFound, found);
    }

    else if (this->sensors[RIGHT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 1){      //There's a Lego on the right.
        legoRight(counter, timer, &lastAct, &legoFound);
    }
    
    else if (this->sensors[LEFT_L] < NEAR_LEGO && legoFound == -1 || legoFound == 2){       //There's a Lego on the left.
        legoLeft(counter, timer, &lastAct, &legoFound);
    }
    
    //Lego is ready to be grabbed.
    else if(legoFound == 3){
        *found = 1;             //When found is set to 1, main will call arms functions
        *counter = 0;
        *timer = 0;
        legoFound = -1;
    }
    
    
    //Nothing found
    else {
        nothingFound(counter, timer, &lastAct);
    }
}

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

//reset
//}

//Add management for Overpower!! Pin PB_15