File content as of revision 0:d267b248eff4:

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



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

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


Robot::DistanceSensors::~DistanceSensors()
{ 
}


float Robot::DistanceSensors::read()//Return the distance of an object
{  
    *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
    
    float Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor   
    float Distance=-0.38f*sqrt(Usensor)+0.38f;
    return Distance;
}

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