zhaw_st16b_pes2_10
/
PES-Yanick2
ahah
Fork of PES_Official by
Diff: Sources/DistanceSensors.cpp
- Revision:
- 4:67d7177c213f
- Parent:
- 1:388c915756f5
--- a/Sources/DistanceSensors.cpp Wed Mar 22 13:33:07 2017 +0000 +++ b/Sources/DistanceSensors.cpp Wed Apr 19 12:23:52 2017 +0000 @@ -1,5 +1,6 @@ #include <cmath> #include "Robot.h" +#include "Declarations.h" @@ -8,15 +9,19 @@ //Nothing } -DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) +DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) { - init(sensorVoltage, bit0, bit1, bit2, number); + init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number); } //initialise -void DistanceSensors::init(AnalogIn* sensorVoltage, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) +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; @@ -30,12 +35,32 @@ float 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 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=sensorVoltage->read(); //Read the Voltage from the selected distance sensor float Distance=-0.38f*sqrt(Usensor)+0.38f; return Distance; }