zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_Official

Do NOT modify!

Dependencies:   mbed Servo ServoArm

Fork of PES_Yanick by zhaw_st16b_pes2_10

Sources/DistanceSensors.cpp@12:c0bcb95885dd, 2017-05-10 (annotated)

Committer:
beacon
Date:
Wed May 10 13:30:13 2017 +0000
Revision:
12:c0bcb95885dd
Parent:
4:67d7177c213f 
k

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 4:67d7177c213f 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 0:d267b248eff4 64 float Distance=-0.38f*sqrt(Usensor)+0.38f;
beacon 0:d267b248eff4 65 return Distance;
beacon 0:d267b248eff4 66 }
beacon 0:d267b248eff4 67
beacon 1:388c915756f5 68 DistanceSensors::operator float()
beacon 0:d267b248eff4 69 {
beacon 0:d267b248eff4 70 return read();
beacon 0:d267b248eff4 71 }
beacon 0:d267b248eff4 72