zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official

k

Dependencies:   Servo ServoArm mbed

Sources/DistanceSensors.cpp@0:15a8480061e8, 2017-05-22 (annotated)

Committer:
beacon
Date:
Mon May 22 11:24:46 2017 +0000
Revision:
0:15a8480061e8
o

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 #include <cmath>
beacon 0:15a8480061e8 2 #include "Robot.h"
beacon 0:15a8480061e8 3 #include "Declarations.h"
beacon 0:15a8480061e8 4
beacon 0:15a8480061e8 5
beacon 0:15a8480061e8 6
beacon 0:15a8480061e8 7 DistanceSensors::DistanceSensors()
beacon 0:15a8480061e8 8 {
beacon 0:15a8480061e8 9 //Nothing
beacon 0:15a8480061e8 10 }
beacon 0:15a8480061e8 11
beacon 0:15a8480061e8 12 DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
beacon 0:15a8480061e8 13 {
beacon 0:15a8480061e8 14 init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
beacon 0:15a8480061e8 15 }
beacon 0:15a8480061e8 16
beacon 0:15a8480061e8 17 //initialise
beacon 0:15a8480061e8 18 void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
beacon 0:15a8480061e8 19 {
beacon 0:15a8480061e8 20 this->sensorVoltage = sensorVoltage;
beacon 0:15a8480061e8 21 this->frontS = frontS;
beacon 0:15a8480061e8 22 this->leftS = leftS;
beacon 0:15a8480061e8 23 this->rightS = rightS;
beacon 0:15a8480061e8 24
beacon 0:15a8480061e8 25 this->bit0 = bit0;
beacon 0:15a8480061e8 26 this->bit1 = bit1;
beacon 0:15a8480061e8 27 this->bit2 = bit2;
beacon 0:15a8480061e8 28 this->number = number;
beacon 0:15a8480061e8 29 }
beacon 0:15a8480061e8 30
beacon 0:15a8480061e8 31
beacon 0:15a8480061e8 32 DistanceSensors::~DistanceSensors()
beacon 0:15a8480061e8 33 {
beacon 0:15a8480061e8 34 }
beacon 0:15a8480061e8 35
beacon 0:15a8480061e8 36
beacon 0:15a8480061e8 37 float DistanceSensors::read()//Return the distance of an object
beacon 0:15a8480061e8 38 {
beacon 0:15a8480061e8 39
beacon 0:15a8480061e8 40 float Usensor;
beacon 0:15a8480061e8 41
beacon 0:15a8480061e8 42 if (number < 6){
beacon 0:15a8480061e8 43 *bit0 = number & 1; // Set the first bit of the Sensors MUX
beacon 0:15a8480061e8 44 *bit1 = number & 2; // Set the second bit of the Sensors MUX
beacon 0:15a8480061e8 45 *bit2 = number & 4; // Set the third bit of the Sensors MUX
beacon 0:15a8480061e8 46
beacon 0:15a8480061e8 47 Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
beacon 0:15a8480061e8 48 }
beacon 0:15a8480061e8 49 else{
beacon 0:15a8480061e8 50 switch(number){
beacon 0:15a8480061e8 51 case 6:
beacon 0:15a8480061e8 52 Usensor=frontS->read();
beacon 0:15a8480061e8 53 break;
beacon 0:15a8480061e8 54 case 7:
beacon 0:15a8480061e8 55 Usensor=leftS->read();
beacon 0:15a8480061e8 56 break;
beacon 0:15a8480061e8 57 case 8:
beacon 0:15a8480061e8 58 Usensor=rightS->read();
beacon 0:15a8480061e8 59 break;
beacon 0:15a8480061e8 60 }
beacon 0:15a8480061e8 61 }
beacon 0:15a8480061e8 62
beacon 0:15a8480061e8 63 //Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
beacon 0:15a8480061e8 64 Usensor *= 3.3f;
beacon 0:15a8480061e8 65 float Distance= 5.906*Usensor*Usensor - 30.831*Usensor + 47.628;
beacon 0:15a8480061e8 66 Distance /= 100;
beacon 0:15a8480061e8 67
beacon 0:15a8480061e8 68 static float distance_filtered = 0.0f;
beacon 0:15a8480061e8 69 distance_filtered = 0.55f * distance_filtered + 0.45f * Distance;
beacon 0:15a8480061e8 70
beacon 0:15a8480061e8 71 return Distance;
beacon 0:15a8480061e8 72 return distance_filtered;
beacon 0:15a8480061e8 73 }
beacon 0:15a8480061e8 74
beacon 0:15a8480061e8 75 DistanceSensors::operator float()
beacon 0:15a8480061e8 76 {
beacon 0:15a8480061e8 77 return read();
beacon 0:15a8480061e8 78 }
beacon 0:15a8480061e8 79