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@9:ac362674c480, 2017-05-02 (annotated)

Committer:
beacon
Date:
Tue May 02 08:39:35 2017 +0000
Revision:
9:ac362674c480
kl

Who changed what in which revision?

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