vf
Dependencies: Servo ServoArm mbed
Fork of PES_Official by
Sources/DistanceSensors.cpp@17:4e1be70bdedb, 2017-05-18 (annotated)
- Committer:
- beacon
- Date:
- Thu May 18 14:27:09 2017 +0000
- Revision:
- 17:4e1be70bdedb
- Parent:
- 4:67d7177c213f
Si compila.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
beacon | 0:d267b248eff4 | 1 | #include <cmath> |
beacon | 0:d267b248eff4 | 2 | #include "Robot.h" |
beacon | 4:67d7177c213f | 3 | #include "Declarations.h" |
beacon | 0:d267b248eff4 | 4 | |
beacon | 0:d267b248eff4 | 5 | |
beacon | 0:d267b248eff4 | 6 | |
beacon | 1:388c915756f5 | 7 | DistanceSensors::DistanceSensors() |
beacon | 0:d267b248eff4 | 8 | { |
beacon | 0:d267b248eff4 | 9 | //Nothing |
beacon | 0:d267b248eff4 | 10 | } |
beacon | 0:d267b248eff4 | 11 | |
beacon | 4:67d7177c213f | 12 | DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) |
beacon | 0:d267b248eff4 | 13 | { |
beacon | 4:67d7177c213f | 14 | init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number); |
beacon | 0:d267b248eff4 | 15 | } |
beacon | 0:d267b248eff4 | 16 | |
beacon | 0:d267b248eff4 | 17 | //initialise |
beacon | 4:67d7177c213f | 18 | void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number) |
beacon | 0:d267b248eff4 | 19 | { |
beacon | 0:d267b248eff4 | 20 | this->sensorVoltage = sensorVoltage; |
beacon | 4:67d7177c213f | 21 | this->frontS = frontS; |
beacon | 4:67d7177c213f | 22 | this->leftS = leftS; |
beacon | 4:67d7177c213f | 23 | this->rightS = rightS; |
beacon | 4:67d7177c213f | 24 | |
beacon | 0:d267b248eff4 | 25 | this->bit0 = bit0; |
beacon | 0:d267b248eff4 | 26 | this->bit1 = bit1; |
beacon | 1:388c915756f5 | 27 | this->bit2 = bit2; |
beacon | 1:388c915756f5 | 28 | this->number = number; |
beacon | 0:d267b248eff4 | 29 | } |
beacon | 0:d267b248eff4 | 30 | |
beacon | 0:d267b248eff4 | 31 | |
beacon | 1:388c915756f5 | 32 | DistanceSensors::~DistanceSensors() |
beacon | 0:d267b248eff4 | 33 | { |
beacon | 0:d267b248eff4 | 34 | } |
beacon | 0:d267b248eff4 | 35 | |
beacon | 0:d267b248eff4 | 36 | |
beacon | 1:388c915756f5 | 37 | float DistanceSensors::read()//Return the distance of an object |
beacon | 4:67d7177c213f | 38 | { |
beacon | 0:d267b248eff4 | 39 | |
beacon | 4:67d7177c213f | 40 | float Usensor; |
beacon | 17:4e1be70bdedb | 41 | |
beacon | 4:67d7177c213f | 42 | if (number < 6){ |
beacon | 4:67d7177c213f | 43 | *bit0 = number & 1; // Set the first bit of the Sensors MUX |
beacon | 4:67d7177c213f | 44 | *bit1 = number & 2; // Set the second bit of the Sensors MUX |
beacon | 4:67d7177c213f | 45 | *bit2 = number & 4; // Set the third bit of the Sensors MUX |
beacon | 4:67d7177c213f | 46 | |
beacon | 4:67d7177c213f | 47 | Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor |
beacon | 4:67d7177c213f | 48 | } |
beacon | 4:67d7177c213f | 49 | else{ |
beacon | 4:67d7177c213f | 50 | switch(number){ |
beacon | 4:67d7177c213f | 51 | case 6: |
beacon | 4:67d7177c213f | 52 | Usensor=frontS->read(); |
beacon | 4:67d7177c213f | 53 | break; |
beacon | 4:67d7177c213f | 54 | case 7: |
beacon | 4:67d7177c213f | 55 | Usensor=leftS->read(); |
beacon | 4:67d7177c213f | 56 | break; |
beacon | 4:67d7177c213f | 57 | case 8: |
beacon | 4:67d7177c213f | 58 | Usensor=rightS->read(); |
beacon | 4:67d7177c213f | 59 | break; |
beacon | 4:67d7177c213f | 60 | } |
beacon | 4:67d7177c213f | 61 | } |
beacon | 4:67d7177c213f | 62 | |
beacon | 4:67d7177c213f | 63 | //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor |
beacon | 17:4e1be70bdedb | 64 | Usensor *= 3.3f; |
beacon | 17:4e1be70bdedb | 65 | float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628; |
beacon | 17:4e1be70bdedb | 66 | Distance /= 100; |
beacon | 17:4e1be70bdedb | 67 | |
beacon | 17:4e1be70bdedb | 68 | static float distance_filtered = 0.0f; |
beacon | 17:4e1be70bdedb | 69 | distance_filtered = 0.55f * distance_filtered + 0.45f * Distance; |
beacon | 17:4e1be70bdedb | 70 | |
beacon | 0:d267b248eff4 | 71 | return Distance; |
beacon | 17:4e1be70bdedb | 72 | return distance_filtered; |
beacon | 0:d267b248eff4 | 73 | } |
beacon | 0:d267b248eff4 | 74 | |
beacon | 1:388c915756f5 | 75 | DistanceSensors::operator float() |
beacon | 0:d267b248eff4 | 76 | { |
beacon | 0:d267b248eff4 | 77 | return read(); |
beacon | 0:d267b248eff4 | 78 | } |
beacon | 0:d267b248eff4 | 79 |