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@72:4e8a151d804e, 2017-04-21 (annotated)

Committer:
cittecla
Date:
Fri Apr 21 12:12:57 2017 +0000
Revision:
72:4e8a151d804e
Parent:
34:40d8d29b44b8
Child:
80:92b9d083322d
updated sensors to new inputs

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 72:4e8a151d804e 54 if(number == 2){
cittecla 72:4e8a151d804e 55 *bit0 = 0;
cittecla 72:4e8a151d804e 56 *bit1 = 0;
cittecla 72:4e8a151d804e 57 *bit2 = 1;
cittecla 72:4e8a151d804e 58 }else {
cittecla 34:40d8d29b44b8 59 *bit0 = (number >> 0) & 1;
cittecla 34:40d8d29b44b8 60 *bit1 = (number >> 1) & 1;
cittecla 34:40d8d29b44b8 61 *bit2 = (number >> 2) & 1;
cittecla 72:4e8a151d804e 62 }
cittecla 34:40d8d29b44b8 63 int i;
cittecla 34:40d8d29b44b8 64 float avarage = 0;
cittecla 34:40d8d29b44b8 65 float d = 0;
cittecla 34:40d8d29b44b8 66 for(i = 0; i<cycles; i++) {
cittecla 72:4e8a151d804e 67 float voltage = 0;
cittecla 72:4e8a151d804e 68 if(number == 4){
cittecla 72:4e8a151d804e 69 voltage = distance2->read();
cittecla 72:4e8a151d804e 70 } else {
cittecla 72:4e8a151d804e 71 voltage = distance->read();
cittecla 72:4e8a151d804e 72 }
cittecla 34:40d8d29b44b8 73 if(voltage < 0.1f) voltage = 0.1;
cittecla 34:40d8d29b44b8 74 switch (number) {
cittecla 34:40d8d29b44b8 75 case 0:
cittecla 34:40d8d29b44b8 76 d = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
cittecla 34:40d8d29b44b8 77 break;
cittecla 34:40d8d29b44b8 78 case 1:
cittecla 34:40d8d29b44b8 79 d = 0.18f/pow((1.5f*voltage-0.1f),0.7f)-0.095f;
cittecla 34:40d8d29b44b8 80 break;
cittecla 34:40d8d29b44b8 81 case 2:
cittecla 34:40d8d29b44b8 82 d = 0.095f/pow((0.94f*voltage-0.05f),0.8f)-0.04f;
cittecla 34:40d8d29b44b8 83 break;
cittecla 34:40d8d29b44b8 84 case 3:
cittecla 34:40d8d29b44b8 85 d = 0.192f/pow((1.8f*voltage-0.13f),0.7f)-0.095f;
cittecla 34:40d8d29b44b8 86 break;
cittecla 34:40d8d29b44b8 87 case 4:
cittecla 34:40d8d29b44b8 88 d = 0.14f/pow((1.2f*voltage-0.09f),0.7f)-0.088f;
cittecla 34:40d8d29b44b8 89 break;
cittecla 34:40d8d29b44b8 90 case 5:
cittecla 34:40d8d29b44b8 91 d = 0.18f/pow((1.55f*voltage-0.12f),0.7f)-0.095f;
cittecla 34:40d8d29b44b8 92 break;
cittecla 34:40d8d29b44b8 93 default:
cittecla 34:40d8d29b44b8 94 break;
cittecla 34:40d8d29b44b8 95 }
cittecla 34:40d8d29b44b8 96 avarage += d;
cittecla 34:40d8d29b44b8 97 }
cittecla 34:40d8d29b44b8 98 avarage = avarage/ cycles;
cittecla 34:40d8d29b44b8 99 return avarage;
cittecla 34:40d8d29b44b8 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 }