File content as of revision 3:fa61be4ecaa6:

#include "Robot.h"

/* Work in progress -------------------------------------------- */

Robot::Robot(PwmOut* left, PwmOut* right, DigitalOut* enableSignal, DigitalOut* leds, AnalogIn* FarbVoltage )
{
    this->powerSignal = enableSignal;
    this->left =        left;
    this->right =       right;

    this->left->period(0.00005f);
    this->right->period(0.00005f);
    
    this->leds =        leds;
    
    this->FarbVoltage = FarbVoltage;

}

/* work in progress ------------------------------------------- */
void Robot::drive()
{
    //pwm determine what direction it goes.
    *powerSignal = 1;
    *left=  0.6f;
    *right= 0.4f;
}

void Robot::turnLeft()
{
    
    *powerSignal = 1;
    *left=  0.4f;
    *right= 0.4f;

}

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

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::search(int* counter, Timer* t){
    int rando;
    
    if (*counter >= 5) {
        //t->reset();
        
        rando = rand() % 2;
        while (this->sensors[FWD] < NEAR){
            if ( t->read() > TIMEOUT ){
                break;
            }
            
            this->leds[FWD] = 1;
            this->turnAround(rando);
        }
        this->leds[FWD] = 0;   
    }
    
    else if (this->sensors[RIGHT] < NEAR){
        t-> reset();
        
        *counter += 1;
        while (this->sensors[RIGHT] < NEAR){
            if ( t->read() > TIMEOUT ){
                break;
            }
            
            this->turnLeft();
            this->leds[RIGHT] = 1;
        }
        this->leds[RIGHT] = 0;
    }
    
    else if (this->sensors[LEFT] < NEAR){
        t-> reset();
        
        *counter += 1;
        while (this->sensors[LEFT] < NEAR){
            if ( t->read() > TIMEOUT ){
                break;
            }
            
            this->turnRight();
            this->leds[LEFT] = 1;
        }
        this->leds[LEFT] = 0;
    }
    
    else if (this->sensors[FWD] < NEAR) {
        t-> reset();
        
        rando = rand() % 2;
        while (this->sensors[FWD] < NEAR){
            if ( t->read() > TIMEOUT ){
                break;
            }
            
            this->leds[FWD] = 1;
            this->turnAround(rando);
        }
     this->leds[FWD] = 0;   
    }
    
    else {
        *counter = 0;
        this->drive();
    }
}

//void Robot::init(){
//  Robot.DistanceSensors.init(&sensorVoltage, &bit0, &bit1, &bit2, ii);
//}

//Remember to set
//DigitalOut powerMotor(PB_2) = 1;
//DigitalIn errorMotor(PB_14);
//if (errorMotor){
//reset
//}

//Add management for Overpower!! Pin PB_15