Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/IRSensor.cpp@98:07a8a858e7cd, 2017-05-03 (annotated)

Committer:
aeschsim
Date:
Wed May 03 06:59:34 2017 +0000
Revision:
98:07a8a858e7cd
Parent:
95:5afdfe17300f
Child:
99:78d87027c85b
Changed IR-Sensor Class

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 34:40d8d29b44b8 1 /*
cittecla 34:40d8d29b44b8 2 * IRSensor.cpp
cittecla 34:40d8d29b44b8 3 * Copyright (c) 2016, ZHAW
cittecla 34:40d8d29b44b8 4 * All rights reserved.
cittecla 34:40d8d29b44b8 5 */
cittecla 34:40d8d29b44b8 6
cittecla 34:40d8d29b44b8 7 #include <cmath>
cittecla 34:40d8d29b44b8 8 #include "IRSensor.h"
cittecla 34:40d8d29b44b8 9
cittecla 34:40d8d29b44b8 10 #define cycles 25
cittecla 34:40d8d29b44b8 11
cittecla 34:40d8d29b44b8 12 /**
cittecla 34:40d8d29b44b8 13 * Creates an IRSensor object.
cittecla 34:40d8d29b44b8 14 * @param distance an analog input object to read the voltage of the sensor.
cittecla 34:40d8d29b44b8 15 * @param bit0 a digital output to set the first bit of the multiplexer.
cittecla 34:40d8d29b44b8 16 * @param bit1 a digital output to set the second bit of the multiplexer.
cittecla 34:40d8d29b44b8 17 * @param bit2 a digital output to set the third bit of the multiplexer.
cittecla 34:40d8d29b44b8 18 * @param number the number of the sensor, either 0, 1, 2, 3, 4 or 5.
cittecla 34:40d8d29b44b8 19 */
cittecla 72:4e8a151d804e 20 IRSensor::IRSensor(AnalogIn* distance,AnalogIn* distance2, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
cittecla 34:40d8d29b44b8 21 {
cittecla 72:4e8a151d804e 22 init(distance,distance2, bit0, bit1, bit2, number);
cittecla 34:40d8d29b44b8 23 }
cittecla 34:40d8d29b44b8 24
cittecla 34:40d8d29b44b8 25
cittecla 34:40d8d29b44b8 26 IRSensor::IRSensor()
cittecla 34:40d8d29b44b8 27 {
cittecla 34:40d8d29b44b8 28 }
cittecla 34:40d8d29b44b8 29
cittecla 72:4e8a151d804e 30 void IRSensor::init(AnalogIn* distance,AnalogIn* distance2, DigitalOut* bit0, DigitalOut* bit1, DigitalOut* bit2, int number)
cittecla 34:40d8d29b44b8 31 {
cittecla 34:40d8d29b44b8 32
cittecla 34:40d8d29b44b8 33 this->distance = distance; // set local references to objects
cittecla 72:4e8a151d804e 34 this->distance2 = distance2;
cittecla 34:40d8d29b44b8 35 this->bit0 = bit0;
cittecla 34:40d8d29b44b8 36 this->bit1 = bit1;
cittecla 34:40d8d29b44b8 37 this->bit2 = bit2;
cittecla 34:40d8d29b44b8 38
cittecla 34:40d8d29b44b8 39 this->number = number;
cittecla 34:40d8d29b44b8 40 }
cittecla 34:40d8d29b44b8 41
cittecla 34:40d8d29b44b8 42 /**
cittecla 34:40d8d29b44b8 43 * Deletes the IRSensor object.
cittecla 34:40d8d29b44b8 44 */
cittecla 34:40d8d29b44b8 45 IRSensor::~IRSensor() {}
cittecla 34:40d8d29b44b8 46
cittecla 34:40d8d29b44b8 47 /**
cittecla 95:5afdfe17300f 48 read voltage of all sensor for Lowpassfilter
cittecla 95:5afdfe17300f 49 */
cittecla 95:5afdfe17300f 50 void IRSensor::voltage()
cittecla 34:40d8d29b44b8 51 {
cittecla 95:5afdfe17300f 52
cittecla 80:92b9d083322d 53 if(number == 3) {
cittecla 80:92b9d083322d 54 *bit0 = 0;
cittecla 80:92b9d083322d 55 *bit1 = 0;
cittecla 80:92b9d083322d 56 *bit2 = 1;
cittecla 80:92b9d083322d 57 } else {
cittecla 80:92b9d083322d 58 *bit0 = (number >> 0) & 1;
cittecla 80:92b9d083322d 59 *bit1 = (number >> 1) & 1;
cittecla 80:92b9d083322d 60 *bit2 = (number >> 2) & 1;
cittecla 72:4e8a151d804e 61 }
cittecla 80:92b9d083322d 62
cittecla 80:92b9d083322d 63 float voltage = 0;
cittecla 80:92b9d083322d 64 if(number == 4) {
cittecla 80:92b9d083322d 65 voltage = distance2->read();
cittecla 80:92b9d083322d 66 } else {
cittecla 72:4e8a151d804e 67 voltage = distance->read();
cittecla 34:40d8d29b44b8 68 }
cittecla 95:5afdfe17300f 69 mean = 0.8f*mean + 0.2f*voltage;
cittecla 95:5afdfe17300f 70 }
cittecla 95:5afdfe17300f 71
cittecla 95:5afdfe17300f 72
cittecla 95:5afdfe17300f 73 /**
cittecla 95:5afdfe17300f 74 * Gets the distance measured with the IR sensor in [m].
cittecla 95:5afdfe17300f 75 * @return the distance, given in [m].
cittecla 95:5afdfe17300f 76 */
cittecla 95:5afdfe17300f 77
aeschsim 98:07a8a858e7cd 78 float IRSensor::read()
cittecla 95:5afdfe17300f 79 {
cittecla 95:5afdfe17300f 80 float d = 0;
cittecla 80:92b9d083322d 81 switch (number) {
cittecla 80:92b9d083322d 82 case 0:
cittecla 95:5afdfe17300f 83 d = 0.18f/pow((1.5f*mean-0.1f),0.7f)-0.095f;
cittecla 80:92b9d083322d 84 break;
cittecla 80:92b9d083322d 85 case 1:
cittecla 95:5afdfe17300f 86 d = 0.18f/pow((1.5f*mean-0.1f),0.7f)-0.095f;
cittecla 80:92b9d083322d 87 break;
cittecla 80:92b9d083322d 88 case 2:
cittecla 95:5afdfe17300f 89 d = 0.095f/pow((0.94f*mean-0.05f),0.8f)-0.04f;
cittecla 80:92b9d083322d 90 break;
cittecla 80:92b9d083322d 91 case 3:
cittecla 95:5afdfe17300f 92 d = 0.192f/pow((1.8f*mean-0.13f),0.7f)-0.095f;
cittecla 80:92b9d083322d 93 break;
cittecla 80:92b9d083322d 94 case 4:
cittecla 95:5afdfe17300f 95 d = 0.14f/pow((1.2f*mean-0.09f),0.7f)-0.088f;
cittecla 80:92b9d083322d 96 break;
cittecla 80:92b9d083322d 97 case 5:
cittecla 95:5afdfe17300f 98 d = 0.18f/pow((1.55f*mean-0.12f),0.7f)-0.095f;
cittecla 80:92b9d083322d 99 break;
cittecla 80:92b9d083322d 100 default:
cittecla 80:92b9d083322d 101 break;
cittecla 80:92b9d083322d 102 }
cittecla 95:5afdfe17300f 103 return d;
cittecla 34:40d8d29b44b8 104 }
cittecla 80:92b9d083322d 105
cittecla 80:92b9d083322d 106
cittecla 80:92b9d083322d 107
cittecla 34:40d8d29b44b8 108 /**
cittecla 34:40d8d29b44b8 109 * The empty operator is a shorthand notation of the <code>read()</code> method.
cittecla 34:40d8d29b44b8 110 */
cittecla 34:40d8d29b44b8 111 IRSensor::operator float()
cittecla 34:40d8d29b44b8 112 {
aeschsim 98:07a8a858e7cd 113 return read();
cittecla 34:40d8d29b44b8 114 }