quadcopter cufe / Mbed 2 deprecated 2006_Theta_Control_yaw

Quadcopter Attitude Control(Yaw-Pitch-Roll)

Dependencies:   mbed

SHARPIR.cpp@0:e63996fd7d3e, 2015-07-03 (annotated)

Committer:
khaledelmadawi
Date:
Fri Jul 03 11:16:02 2015 +0000
Revision:
0:e63996fd7d3e
Quadcopter Attitude Control(Yaw-Pitch-Roll)

Who changed what in which revision?

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