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@18:a158713a0049, 2017-05-19 (annotated)

Committer:
EpicG10
Date:
Fri May 19 06:40:33 2017 +0000
Revision:
18:a158713a0049
Parent:
17:4e1be70bdedb 
a

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 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