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@117:66d64dbd1b36, 2017-05-10 (annotated)

Committer:
cittecla
Date:
Wed May 10 10:02:53 2017 +0000
Revision:
117:66d64dbd1b36
Parent:
113:c7afe49752b9 
reversed to old IR sensor measurement, search for brick is now working better, still not perfect.

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