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@80:92b9d083322d, 2017-04-26 (annotated)

Committer:
cittecla
Date:
Wed Apr 26 07:30:03 2017 +0000
Revision:
80:92b9d083322d
Parent:
72:4e8a151d804e
Child:
95:5afdfe17300f
Fixed IRSensor reading error.

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 /**
cittecla 34:40d8d29b44b8 44 * Deletes the IRSensor object.
cittecla 34:40d8d29b44b8 45 */
cittecla 34:40d8d29b44b8 46 IRSensor::~IRSensor() {}
cittecla 34:40d8d29b44b8 47
cittecla 34:40d8d29b44b8 48 /**
cittecla 34:40d8d29b44b8 49 * Gets the distance measured with the IR sensor in [m].
cittecla 34:40d8d29b44b8 50 * @return the distance, given in [m].
cittecla 34:40d8d29b44b8 51 */
cittecla 34:40d8d29b44b8 52 float IRSensor::read()
cittecla 34:40d8d29b44b8 53 {
cittecla 80:92b9d083322d 54 static float mean[6] = {0};
cittecla 80:92b9d083322d 55 if(number == 3) {
cittecla 80:92b9d083322d 56 *bit0 = 0;
cittecla 80:92b9d083322d 57 *bit1 = 0;
cittecla 80:92b9d083322d 58 *bit2 = 1;
cittecla 80:92b9d083322d 59 } else {
cittecla 80:92b9d083322d 60 *bit0 = (number >> 0) & 1;
cittecla 80:92b9d083322d 61 *bit1 = (number >> 1) & 1;
cittecla 80:92b9d083322d 62 *bit2 = (number >> 2) & 1;
cittecla 72:4e8a151d804e 63 }
cittecla 80:92b9d083322d 64
cittecla 34:40d8d29b44b8 65 float d = 0;
cittecla 80:92b9d083322d 66 float voltage = 0;
cittecla 80:92b9d083322d 67 if(number == 4) {
cittecla 80:92b9d083322d 68 voltage = distance2->read();
cittecla 80:92b9d083322d 69 } else {
cittecla 72:4e8a151d804e 70 voltage = distance->read();
cittecla 34:40d8d29b44b8 71 }
cittecla 80:92b9d083322d 72 if(voltage < 0.1f) voltage = 0.1; // set voltage if not big enough
cittecla 80:92b9d083322d 73 switch (number) {
cittecla 80:92b9d083322d 74 case 0:
cittecla 80:92b9d083322d 75 d = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
cittecla 80:92b9d083322d 76 break;
cittecla 80:92b9d083322d 77 case 1:
cittecla 80:92b9d083322d 78 d = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
cittecla 80:92b9d083322d 79 break;
cittecla 80:92b9d083322d 80 case 2:
cittecla 80:92b9d083322d 81 d = 0.095f/pow((0.94f*voltage-0.05f),0.8f)-0.04f;
cittecla 80:92b9d083322d 82 break;
cittecla 80:92b9d083322d 83 case 3:
cittecla 80:92b9d083322d 84 d = 0.192f/pow((1.8f*voltage-0.13f),0.7f)-0.095f;
cittecla 80:92b9d083322d 85 break;
cittecla 80:92b9d083322d 86 case 4:
cittecla 80:92b9d083322d 87 d = 0.14f/pow((1.2f*voltage-0.09f),0.7f)-0.088f;
cittecla 80:92b9d083322d 88 break;
cittecla 80:92b9d083322d 89 case 5:
cittecla 80:92b9d083322d 90 d = 0.18f/pow((1.55f*voltage-0.12f),0.7f)-0.095f;
cittecla 80:92b9d083322d 91 break;
cittecla 80:92b9d083322d 92 default:
cittecla 80:92b9d083322d 93 break;
cittecla 80:92b9d083322d 94 }
cittecla 80:92b9d083322d 95 mean[number] = 0.8f*mean[number] + 0.2f*d;
cittecla 80:92b9d083322d 96 return mean[number];
cittecla 34:40d8d29b44b8 97 }
cittecla 80:92b9d083322d 98
cittecla 80:92b9d083322d 99
cittecla 80:92b9d083322d 100
cittecla 34:40d8d29b44b8 101 /**
cittecla 34:40d8d29b44b8 102 * The empty operator is a shorthand notation of the <code>read()</code> method.
cittecla 34:40d8d29b44b8 103 */
cittecla 34:40d8d29b44b8 104 IRSensor::operator float()
cittecla 34:40d8d29b44b8 105 {
cittecla 34:40d8d29b44b8 106 return read();
cittecla 34:40d8d29b44b8 107 }