vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Diff: Sources/DistanceSensors.cpp
- Revision:
- 0:d267b248eff4
- Child:
- 1:388c915756f5
diff -r 000000000000 -r d267b248eff4 Sources/DistanceSensors.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Sources/DistanceSensors.cpp Sat Mar 11 10:14:00 2017 +0000 @@ -0,0 +1,46 @@ +#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(); +} +