ultrasonic.cpp

00001 #include "ultrasonic.h"
00002 #include "math.h"
00003 int arraysize =20;
00004 int rangevalue[20];
00005 int modE;
00006 int UL_rR = 0;
00007 int UL_R = 0;
00008 int UL_F = 0;
00009 int UL_L = 0;
00010 int UL_rL = 0;
00011 int UL_B = 0;
00012 void isort(int *a, int n)
00013 {
00014 // *a is an array pointer function
00015     for (int i = 1; i < n; ++i) {
00016         int j = a[i];
00017         int k;
00018         for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
00019             a[k + 1] = a[k];
00020         }
00021         a[k + 1] = j;
00022     }
00023 }
00024 
00025 //Mode function, returning the mode or median.
00026 int mode(int *x,int n)
00027 {
00028     int number = x[0];
00029     int mode = number;
00030     int mode2=number;
00031     int count = 1;
00032     int countMode = 1;
00033 
00034     for (int i=1; i<n; i++) {
00035         if (x[i] == number) {
00036             // count occurrences of the current number
00037             count++;
00038         } else {
00039             // now this is a different number
00040             if (count > countMode) {
00041                 countMode = count; // mode is the biggest ocurrences
00042                 mode = number;
00043                 mode2=number;
00044             } else if(count == countMode && count!=1) {
00045                 mode2=number;
00046             } else if(countMode==1) {
00047                 mode=x[((n-1)/2)];
00048                 mode2=x[((n+1)/2)];
00049             }
00050             count = 1; // reset count for the new number
00051             number = x[i];
00052         }
00053     }
00054     return ((mode+mode2)/2);
00055 }
00056 int sensorvalue(AnalogIn pin)
00057 {
00058     //int sum=0;
00059     for(int i=0; i<arraysize; i++) {
00060         rangevalue[i]= (pin.read()*5*102.54*2.54*10);
00061         //sum=sum+rangevalue[i];
00062     }
00063     //return sum/100;
00064     isort(rangevalue,arraysize);
00065     //modE = mode(rangevalue,arraysize);
00066     modE=rangevalue[arraysize/2];
00067     return modE;
00068 }
00069 void ultrasonic_run()
00070 {
00071        SRX=1;
00072        wait_us(30);
00073        SRX=0;
00074        Thread::wait(150);
00075        stdio_mutex.lock();
00076        UL_L=sensorvalue(uL);
00077        UL_rL=sensorvalue(urL);
00078        UL_B=sensorvalue(uB);
00079        UL_rR=sensorvalue(urR);
00080        UL_R=sensorvalue(uR);
00081        UL_F=sensorvalue(uF);
00082        stdio_mutex.unlock();
00083 }