Kim Youngsik / Mbed 2 deprecated 0ROBOFRIEN_FCC_v1_12

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed BufferedSerial ConfigFile

ROBOFRIEN_SUB_FUNC/Function.h@1:9530746906b6, 2018-11-28 (annotated)

Committer:
skyyoungsik
Date:
Wed Nov 28 13:06:23 2018 +0000
Revision:
1:9530746906b6
test1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
skyyoungsik 1:9530746906b6 1 #define M_PI 3.141592
skyyoungsik 1:9530746906b6 2
skyyoungsik 1:9530746906b6 3 double deg2rad(double deg) {
skyyoungsik 1:9530746906b6 4 return deg * (M_PI/180.0);
skyyoungsik 1:9530746906b6 5 }
skyyoungsik 1:9530746906b6 6
skyyoungsik 1:9530746906b6 7
skyyoungsik 1:9530746906b6 8 double getDistance(double lat1,double lon1,double lat2,double lon2) {
skyyoungsik 1:9530746906b6 9 double R = 6371; // Radius of the earth in km
skyyoungsik 1:9530746906b6 10 double dLat = deg2rad(lat2-lat1); // deg2rad below
skyyoungsik 1:9530746906b6 11 double dLon = deg2rad(lon2-lon1);
skyyoungsik 1:9530746906b6 12 double a =
skyyoungsik 1:9530746906b6 13 sin(dLat/2) * sin(dLat/2) +
skyyoungsik 1:9530746906b6 14 cos(deg2rad(lat1)) * cos(deg2rad(lat2)) *
skyyoungsik 1:9530746906b6 15 sin(dLon/2) * sin(dLon/2)
skyyoungsik 1:9530746906b6 16 ;
skyyoungsik 1:9530746906b6 17 double c = 2 * atan2(sqrt(a), sqrt(1-a));
skyyoungsik 1:9530746906b6 18 double d = R * c; // Distance in km
skyyoungsik 1:9530746906b6 19 d = d * 1000; // Distance in meter
skyyoungsik 1:9530746906b6 20 return d;
skyyoungsik 1:9530746906b6 21 }
skyyoungsik 1:9530746906b6 22
skyyoungsik 1:9530746906b6 23 double MIN_MAX(double input, double min, double max){
skyyoungsik 1:9530746906b6 24 double output;
skyyoungsik 1:9530746906b6 25 if( input < min) output = min;
skyyoungsik 1:9530746906b6 26 else if(input > max) output = max;
skyyoungsik 1:9530746906b6 27 else output = input;
skyyoungsik 1:9530746906b6 28
skyyoungsik 1:9530746906b6 29 return output;
skyyoungsik 1:9530746906b6 30 }
skyyoungsik 1:9530746906b6 31
skyyoungsik 1:9530746906b6 32
skyyoungsik 1:9530746906b6 33 float bearing(float lat1,float lon1,float lat2,float lon2){
skyyoungsik 1:9530746906b6 34
skyyoungsik 1:9530746906b6 35 float teta1 = (lat1)*M_PI/180.0;
skyyoungsik 1:9530746906b6 36 float teta2 = (lat2)*M_PI/180.0;;
skyyoungsik 1:9530746906b6 37 // float delta1 = (lat2-lat1)*M_PI/180.0;;
skyyoungsik 1:9530746906b6 38 float delta2 = (lon2-lon1)*M_PI/180.0;;
skyyoungsik 1:9530746906b6 39
skyyoungsik 1:9530746906b6 40 //==================Heading Formula Calculation================//
skyyoungsik 1:9530746906b6 41
skyyoungsik 1:9530746906b6 42 float y = sin(delta2) * cos(teta2);
skyyoungsik 1:9530746906b6 43 float x = cos(teta1)*sin(teta2) - sin(teta1)*cos(teta2)*cos(delta2);
skyyoungsik 1:9530746906b6 44 float brng = atan2(y,x);
skyyoungsik 1:9530746906b6 45 brng = (brng)*180.0/M_PI;// radians to degrees
skyyoungsik 1:9530746906b6 46 brng = ( ((int)brng + 360) % 360 );
skyyoungsik 1:9530746906b6 47 // if(brng > 180) brng -= 360;
skyyoungsik 1:9530746906b6 48 if(brng > 360) brng -= 360;
skyyoungsik 1:9530746906b6 49 else if(brng < 0) brng += 360;
skyyoungsik 1:9530746906b6 50 return brng;
skyyoungsik 1:9530746906b6 51 }
skyyoungsik 1:9530746906b6 52
skyyoungsik 1:9530746906b6 53 float ABSOL(float input){
skyyoungsik 1:9530746906b6 54 float output;
skyyoungsik 1:9530746906b6 55 if( input > 0){
skyyoungsik 1:9530746906b6 56 output = input;
skyyoungsik 1:9530746906b6 57 }else{
skyyoungsik 1:9530746906b6 58 output = -input;
skyyoungsik 1:9530746906b6 59 }
skyyoungsik 1:9530746906b6 60 return output;
skyyoungsik 1:9530746906b6 61 }