zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official_Richtig

x

Dependencies:   Servo ServoArm mbed

Fork of PES_PIXY_Officiall by zhaw_st16b_pes2_10

Sources/DistanceSensors.cpp@3:63da1d1fae15, 2017-05-26 (annotated)

Committer:
EpicG10
Date:
Fri May 26 08:01:48 2017 +0000
Revision:
3:63da1d1fae15
Parent:
0:15a8480061e8 
c

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
EpicG10 3:63da1d1fae15 7 DistanceSensors::DistanceSensors() {
beacon 0:15a8480061e8 8 //Nothing
beacon 0:15a8480061e8 9 }
beacon 0:15a8480061e8 10
EpicG10 3:63da1d1fae15 11 DistanceSensors::DistanceSensors(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number){
beacon 0:15a8480061e8 12 init(sensorVoltage, frontS, leftS, rightS, bit0, bit1, bit2, number);
beacon 0:15a8480061e8 13 }
beacon 0:15a8480061e8 14
beacon 0:15a8480061e8 15 //initialise
EpicG10 3:63da1d1fae15 16 void DistanceSensors::init(AnalogIn* sensorVoltage, AnalogIn* frontS, AnalogIn* leftS, AnalogIn* rightS, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number){
beacon 0:15a8480061e8 17 this->sensorVoltage = sensorVoltage;
beacon 0:15a8480061e8 18 this->frontS = frontS;
beacon 0:15a8480061e8 19 this->leftS = leftS;
beacon 0:15a8480061e8 20 this->rightS = rightS;
beacon 0:15a8480061e8 21
beacon 0:15a8480061e8 22 this->bit0 = bit0;
beacon 0:15a8480061e8 23 this->bit1 = bit1;
beacon 0:15a8480061e8 24 this->bit2 = bit2;
beacon 0:15a8480061e8 25 this->number = number;
beacon 0:15a8480061e8 26 }
beacon 0:15a8480061e8 27
EpicG10 3:63da1d1fae15 28 DistanceSensors::~DistanceSensors(){
beacon 0:15a8480061e8 29
beacon 0:15a8480061e8 30 }
beacon 0:15a8480061e8 31
EpicG10 3:63da1d1fae15 32 float DistanceSensors::read(){ //Return the distance of an object
beacon 0:15a8480061e8 33 float Usensor;
beacon 0:15a8480061e8 34 if (number < 6){
beacon 0:15a8480061e8 35 *bit0 = number & 1; // Set the first bit of the Sensors MUX
beacon 0:15a8480061e8 36 *bit1 = number & 2; // Set the second bit of the Sensors MUX
beacon 0:15a8480061e8 37 *bit2 = number & 4; // Set the third bit of the Sensors MUX
beacon 0:15a8480061e8 38
beacon 0:15a8480061e8 39 Usensor=sensorVoltage->read(); //Read the Voltage from the selected distance sensor
beacon 0:15a8480061e8 40 }
beacon 0:15a8480061e8 41 else{
beacon 0:15a8480061e8 42 switch(number){
beacon 0:15a8480061e8 43 case 6:
beacon 0:15a8480061e8 44 Usensor=frontS->read();
beacon 0:15a8480061e8 45 break;
beacon 0:15a8480061e8 46 case 7:
beacon 0:15a8480061e8 47 Usensor=leftS->read();
beacon 0:15a8480061e8 48 break;
beacon 0:15a8480061e8 49 case 8:
beacon 0:15a8480061e8 50 Usensor=rightS->read();
beacon 0:15a8480061e8 51 break;
beacon 0:15a8480061e8 52 }
beacon 0:15a8480061e8 53 }
beacon 0:15a8480061e8 54 Usensor *= 3.3f;
EpicG10 3:63da1d1fae15 55 float Distance= 5.906f*Usensor*Usensor - 30.831f*Usensor + 47.628f;
EpicG10 3:63da1d1fae15 56 Distance /= 100.0f;
EpicG10 3:63da1d1fae15 57 return Distance;
beacon 0:15a8480061e8 58 }
beacon 0:15a8480061e8 59
EpicG10 3:63da1d1fae15 60 DistanceSensors::operator float(){
beacon 0:15a8480061e8 61 return read();
beacon 0:15a8480061e8 62 }
beacon 0:15a8480061e8 63