File content as of revision 5:acb938f45b9c:

#include <cmath>
#include "Robot.h"
#include "Declarations.h"



DistanceSensors::DistanceSensors() {
    //Nothing
}
    
DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number){   
    init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
}

//initialise
void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number){
    this->sensorVoltage = sensorVoltage;
    this->frontS = frontS;
    this->leftS = leftS;
    this->rightS = rightS;
    
    this->bit0 = bit0;
    this->bit1 = bit1;
    this->bit2 = bit2;
    this->number = number;
}

DistanceSensors::~DistanceSensors(){ 

}

float DistanceSensors::read(){ //Return the distance of an object
    float Usensor;
    if (number < 6){
        *bit0 = number & 1; // Set the first bit of the Sensors MUX
        *bit1 = number & 2; // Set the second bit of the Sensors MUX
        *bit2 = number & 4; // Set the third bit of the Sensors MUX
        
        Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
    }
    else{
        switch(number){
            case 6:
                Usensor=frontS->read();
                break;
            case 7:
                Usensor=leftS->read();
                break;
            case 8:
                Usensor=rightS->read();
                break;
        }
    }
    Usensor *= 3.3f;
    float Distance= 5.906f*Usensor*Usensor - 30.831f*Usensor + 47.628f;
    Distance /= 100.0f;
    return Distance;  
}

DistanceSensors::operator float(){ 
    return read();
}