quadcopter cufe
/
FollowWall2
Go to goal robot with follow wall algo
SHARPIR.cpp@0:efef62b55c86, 2014-04-08 (annotated)
- Committer:
- khaledelmadawi
- Date:
- Tue Apr 08 13:21:17 2014 +0000
- Revision:
- 0:efef62b55c86
Follow wall
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
khaledelmadawi | 0:efef62b55c86 | 1 | /* mbed SHARPIR distance sensor |
khaledelmadawi | 0:efef62b55c86 | 2 | * Copyright (c) 2010 Tomas Johansen |
khaledelmadawi | 0:efef62b55c86 | 3 | * |
khaledelmadawi | 0:efef62b55c86 | 4 | * Permission is hereby granted, free of charge, to any person obtaining a copy |
khaledelmadawi | 0:efef62b55c86 | 5 | * of this software and associated documentation files (the "Software"), to deal |
khaledelmadawi | 0:efef62b55c86 | 6 | * in the Software without restriction, including without limitation the rights |
khaledelmadawi | 0:efef62b55c86 | 7 | * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
khaledelmadawi | 0:efef62b55c86 | 8 | * copies of the Software, and to permit persons to whom the Software is |
khaledelmadawi | 0:efef62b55c86 | 9 | * furnished to do so, subject to the following conditions: |
khaledelmadawi | 0:efef62b55c86 | 10 | * |
khaledelmadawi | 0:efef62b55c86 | 11 | * The above copyright notice and this permission notice shall be included in |
khaledelmadawi | 0:efef62b55c86 | 12 | * all copies or substantial portions of the Software. |
khaledelmadawi | 0:efef62b55c86 | 13 | * |
khaledelmadawi | 0:efef62b55c86 | 14 | * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
khaledelmadawi | 0:efef62b55c86 | 15 | * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
khaledelmadawi | 0:efef62b55c86 | 16 | * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
khaledelmadawi | 0:efef62b55c86 | 17 | * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
khaledelmadawi | 0:efef62b55c86 | 18 | * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
khaledelmadawi | 0:efef62b55c86 | 19 | * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
khaledelmadawi | 0:efef62b55c86 | 20 | * THE SOFTWARE. |
khaledelmadawi | 0:efef62b55c86 | 21 | */ |
khaledelmadawi | 0:efef62b55c86 | 22 | |
khaledelmadawi | 0:efef62b55c86 | 23 | |
khaledelmadawi | 0:efef62b55c86 | 24 | #include "mbed.h" |
khaledelmadawi | 0:efef62b55c86 | 25 | #include "SHARPIR.h" |
khaledelmadawi | 0:efef62b55c86 | 26 | |
khaledelmadawi | 0:efef62b55c86 | 27 | |
khaledelmadawi | 0:efef62b55c86 | 28 | /* This function is currently only working for the Sharp GP2Y0A02YK0F sensor |
khaledelmadawi | 0:efef62b55c86 | 29 | * which measures from around 20cm to 150cm. To adapt this to other Sharp IR |
khaledelmadawi | 0:efef62b55c86 | 30 | * sensors, you have to calculate the variables reg and exp. |
khaledelmadawi | 0:efef62b55c86 | 31 | * |
khaledelmadawi | 0:efef62b55c86 | 32 | * To quickly calculate reg and exp, use microsoft excel to plot the graph |
khaledelmadawi | 0:efef62b55c86 | 33 | * provided in the datasheet. You should create a scatterplot (except that |
khaledelmadawi | 0:efef62b55c86 | 34 | * the y-axis is [cm], and x-axis is [V]). |
khaledelmadawi | 0:efef62b55c86 | 35 | * |
khaledelmadawi | 0:efef62b55c86 | 36 | * When you get the values, right click the line in the graph and select |
khaledelmadawi | 0:efef62b55c86 | 37 | * "Add Trendline". Select "power". Also, in the trendline options, check |
khaledelmadawi | 0:efef62b55c86 | 38 | * that you want to see the function. It will then be printed in your |
khaledelmadawi | 0:efef62b55c86 | 39 | * scatterplot. |
khaledelmadawi | 0:efef62b55c86 | 40 | * |
khaledelmadawi | 0:efef62b55c86 | 41 | * This is the function: Reg*x^exp. |
khaledelmadawi | 0:efef62b55c86 | 42 | * |
khaledelmadawi | 0:efef62b55c86 | 43 | * |
khaledelmadawi | 0:efef62b55c86 | 44 | * Example: |
khaledelmadawi | 0:efef62b55c86 | 45 | * |
khaledelmadawi | 0:efef62b55c86 | 46 | * SHARPIR sensor(p20); |
khaledelmadawi | 0:efef62b55c86 | 47 | * sensor.calibrate(57.373, 1.3166, 0.45, 2.5); |
khaledelmadawi | 0:efef62b55c86 | 48 | * while(1){ |
khaledelmadawi | 0:efef62b55c86 | 49 | * serial.printf("cm: %f ", sensor.cm()); |
khaledelmadawi | 0:efef62b55c86 | 50 | * wait_ms(50); |
khaledelmadawi | 0:efef62b55c86 | 51 | * } |
khaledelmadawi | 0:efef62b55c86 | 52 | * |
khaledelmadawi | 0:efef62b55c86 | 53 | * You can also use this method to manually plot values you've measured with |
khaledelmadawi | 0:efef62b55c86 | 54 | * the "volt" function, which in the end should give a result similar to the |
khaledelmadawi | 0:efef62b55c86 | 55 | * values provided below. |
khaledelmadawi | 0:efef62b55c86 | 56 | * |
khaledelmadawi | 0:efef62b55c86 | 57 | * Feel free to contact me with improvements for the source code, and |
khaledelmadawi | 0:efef62b55c86 | 58 | * especially for values that would work for other sensors. |
khaledelmadawi | 0:efef62b55c86 | 59 | */ |
khaledelmadawi | 0:efef62b55c86 | 60 | |
khaledelmadawi | 0:efef62b55c86 | 61 | |
khaledelmadawi | 0:efef62b55c86 | 62 | SHARPIR::SHARPIR(PinName AnalogPort) |
khaledelmadawi | 0:efef62b55c86 | 63 | : _analogin(AnalogPort) { |
khaledelmadawi | 0:efef62b55c86 | 64 | higherrange=2.5; |
khaledelmadawi | 0:efef62b55c86 | 65 | lowerrange=0.45; |
khaledelmadawi | 0:efef62b55c86 | 66 | reg=57.373; //60.495 |
khaledelmadawi | 0:efef62b55c86 | 67 | exp=-1.3166; //-1.1904 |
khaledelmadawi | 0:efef62b55c86 | 68 | } |
khaledelmadawi | 0:efef62b55c86 | 69 | |
khaledelmadawi | 0:efef62b55c86 | 70 | void calibrate(double reg, float exp, double lowerrange, double higherrange) { //sets new reg and exp value |
khaledelmadawi | 0:efef62b55c86 | 71 | } |
khaledelmadawi | 0:efef62b55c86 | 72 | |
khaledelmadawi | 0:efef62b55c86 | 73 | float SHARPIR::volt() { |
khaledelmadawi | 0:efef62b55c86 | 74 | return(_analogin.read()*3.3); //analogin function returns a percentage which describes how much of 3.3v it's reading, therefor multiply it by 3,3 to get the correct analogin voltage. |
khaledelmadawi | 0:efef62b55c86 | 75 | } |
khaledelmadawi | 0:efef62b55c86 | 76 | |
khaledelmadawi | 0:efef62b55c86 | 77 | float SHARPIR::cm() { |
khaledelmadawi | 0:efef62b55c86 | 78 | float v; |
khaledelmadawi | 0:efef62b55c86 | 79 | v=volt(); |
khaledelmadawi | 0:efef62b55c86 | 80 | if (v>higherrange) //sensor is out of higher range |
khaledelmadawi | 0:efef62b55c86 | 81 | return(reg*pow(v, exp));//0 |
khaledelmadawi | 0:efef62b55c86 | 82 | else if (v<lowerrange) |
khaledelmadawi | 0:efef62b55c86 | 83 | return(-1.0); //sensor is out of lower range |
khaledelmadawi | 0:efef62b55c86 | 84 | else |
khaledelmadawi | 0:efef62b55c86 | 85 | return(reg*pow(v, exp)); |
khaledelmadawi | 0:efef62b55c86 | 86 | } |
khaledelmadawi | 0:efef62b55c86 | 87 | |
khaledelmadawi | 0:efef62b55c86 | 88 | float SHARPIR::inch() { |
khaledelmadawi | 0:efef62b55c86 | 89 | float v; |
khaledelmadawi | 0:efef62b55c86 | 90 | v=volt(); |
khaledelmadawi | 0:efef62b55c86 | 91 | if (v>higherrange) //sensor is out of higher range |
khaledelmadawi | 0:efef62b55c86 | 92 | return(0); |
khaledelmadawi | 0:efef62b55c86 | 93 | else if (v<lowerrange) |
khaledelmadawi | 0:efef62b55c86 | 94 | return(-1.0); //sensor is out of range |
khaledelmadawi | 0:efef62b55c86 | 95 | else |
khaledelmadawi | 0:efef62b55c86 | 96 | return(0.393700787*reg*pow(v, exp)); |
khaledelmadawi | 0:efef62b55c86 | 97 | } |