zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official-US

a

Dependencies:   Servo ServoArm mbed

Fork of PES_Official-TestF by zhaw_st16b_pes2_10

Sources/DistanceSensors.cpp@0:d267b248eff4, 2017-03-11 (annotated)

Committer:
beacon
Date:
Sat Mar 11 10:14:00 2017 +0000
Revision:
0:d267b248eff4
Child:
1:388c915756f5
l

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:d267b248eff4 1 #include <cmath>
beacon 0:d267b248eff4 2 #include "Robot.h"
beacon 0:d267b248eff4 3
beacon 0:d267b248eff4 4
beacon 0:d267b248eff4 5
beacon 0:d267b248eff4 6 Robot::DistanceSensors::DistanceSensors()
beacon 0:d267b248eff4 7 {
beacon 0:d267b248eff4 8 //Nothing
beacon 0:d267b248eff4 9 }
beacon 0:d267b248eff4 10
beacon 0:d267b248eff4 11 Robot::DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
beacon 0:d267b248eff4 12 {
beacon 0:d267b248eff4 13 init(sensorVoltage, bit0, bit1, bit2, number);
beacon 0:d267b248eff4 14 }
beacon 0:d267b248eff4 15
beacon 0:d267b248eff4 16 //initialise
beacon 0:d267b248eff4 17 void Robot::DistanceSensors::init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
beacon 0:d267b248eff4 18 {
beacon 0:d267b248eff4 19 this->sensorVoltage = sensorVoltage;
beacon 0:d267b248eff4 20 this->bit0 = bit0;
beacon 0:d267b248eff4 21 this->bit1 = bit1;
beacon 0:d267b248eff4 22 this->bit2 = bit2;
beacon 0:d267b248eff4 23 }
beacon 0:d267b248eff4 24
beacon 0:d267b248eff4 25
beacon 0:d267b248eff4 26 Robot::DistanceSensors::~DistanceSensors()
beacon 0:d267b248eff4 27 {
beacon 0:d267b248eff4 28 }
beacon 0:d267b248eff4 29
beacon 0:d267b248eff4 30
beacon 0:d267b248eff4 31 float Robot::DistanceSensors::read()//Return the distance of an object
beacon 0:d267b248eff4 32 {
beacon 0:d267b248eff4 33 *bit0 = number & 1; // Set the first bit of the Sensors MUX
beacon 0:d267b248eff4 34 *bit1 = number & 2; // Set the second bit of the Sensors MUX
beacon 0:d267b248eff4 35 *bit2 = number & 4; // Set the third bit of the Sensors MUX
beacon 0:d267b248eff4 36
beacon 0:d267b248eff4 37 float Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
beacon 0:d267b248eff4 38 float Distance=-0.38f*sqrt(Usensor)+0.38f;
beacon 0:d267b248eff4 39 return Distance;
beacon 0:d267b248eff4 40 }
beacon 0:d267b248eff4 41
beacon 0:d267b248eff4 42 Robot::DistanceSensors::operator float()
beacon 0:d267b248eff4 43 {
beacon 0:d267b248eff4 44 return read();
beacon 0:d267b248eff4 45 }
beacon 0:d267b248eff4 46