EdgeBotix / Mbed 2 deprecated eBot_Firmware_V1

Initial Commit

Dependencies:   mbed HC05 QEI MODSERIAL SWSPI mbed-rtos

Drivers/ultrasonic/ultrasonic.cpp@3:d1197b5ea68a, 2014-10-12 (annotated)

Committer:
Throwbot
Date:
Sun Oct 12 23:27:43 2014 +0000
Revision:
3:d1197b5ea68a
Parent:
2:0f80c8a1c236
Child:
4:81b0de07841f
Ebot demo working

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Throwbot 1:1da89c13dfa1 1 #include "ultrasonic.h"
Throwbot 1:1da89c13dfa1 2 #include "math.h"
Throwbot 2:0f80c8a1c236 3 int arraysize =20;
Throwbot 2:0f80c8a1c236 4 int rangevalue[20];
Throwbot 1:1da89c13dfa1 5 int modE;
Throwbot 2:0f80c8a1c236 6 int UL_rR = 0;
Throwbot 2:0f80c8a1c236 7 int UL_R = 0;
Throwbot 2:0f80c8a1c236 8 int UL_F = 0;
Throwbot 2:0f80c8a1c236 9 int UL_L = 0;
Throwbot 2:0f80c8a1c236 10 int UL_rL = 0;
Throwbot 2:0f80c8a1c236 11 int UL_B = 0;
Throwbot 1:1da89c13dfa1 12 void isort(int *a, int n)
Throwbot 1:1da89c13dfa1 13 {
Throwbot 1:1da89c13dfa1 14 // *a is an array pointer function
Throwbot 1:1da89c13dfa1 15 for (int i = 1; i < n; ++i) {
Throwbot 1:1da89c13dfa1 16 int j = a[i];
Throwbot 1:1da89c13dfa1 17 int k;
Throwbot 1:1da89c13dfa1 18 for (k = i - 1; (k >= 0) && (j < a[k]); k--) {
Throwbot 1:1da89c13dfa1 19 a[k + 1] = a[k];
Throwbot 1:1da89c13dfa1 20 }
Throwbot 1:1da89c13dfa1 21 a[k + 1] = j;
Throwbot 1:1da89c13dfa1 22 }
Throwbot 1:1da89c13dfa1 23 }
Throwbot 1:1da89c13dfa1 24
Throwbot 1:1da89c13dfa1 25 //Mode function, returning the mode or median.
Throwbot 1:1da89c13dfa1 26 int mode(int *x,int n)
Throwbot 1:1da89c13dfa1 27 {
Throwbot 1:1da89c13dfa1 28 int number = x[0];
Throwbot 1:1da89c13dfa1 29 int mode = number;
Throwbot 1:1da89c13dfa1 30 int mode2=number;
Throwbot 1:1da89c13dfa1 31 int count = 1;
Throwbot 1:1da89c13dfa1 32 int countMode = 1;
Throwbot 1:1da89c13dfa1 33
Throwbot 1:1da89c13dfa1 34 for (int i=1; i<n; i++) {
Throwbot 1:1da89c13dfa1 35 if (x[i] == number) {
Throwbot 1:1da89c13dfa1 36 // count occurrences of the current number
Throwbot 1:1da89c13dfa1 37 count++;
Throwbot 1:1da89c13dfa1 38 } else {
Throwbot 1:1da89c13dfa1 39 // now this is a different number
Throwbot 1:1da89c13dfa1 40 if (count > countMode) {
Throwbot 1:1da89c13dfa1 41 countMode = count; // mode is the biggest ocurrences
Throwbot 1:1da89c13dfa1 42 mode = number;
Throwbot 1:1da89c13dfa1 43 mode2=number;
Throwbot 1:1da89c13dfa1 44 } else if(count == countMode && count!=1) {
Throwbot 1:1da89c13dfa1 45 mode2=number;
Throwbot 1:1da89c13dfa1 46 } else if(countMode==1) {
Throwbot 1:1da89c13dfa1 47 mode=x[((n-1)/2)];
Throwbot 1:1da89c13dfa1 48 mode2=x[((n+1)/2)];
Throwbot 1:1da89c13dfa1 49 }
Throwbot 1:1da89c13dfa1 50 count = 1; // reset count for the new number
Throwbot 1:1da89c13dfa1 51 number = x[i];
Throwbot 1:1da89c13dfa1 52 }
Throwbot 1:1da89c13dfa1 53 }
Throwbot 1:1da89c13dfa1 54 return ((mode+mode2)/2);
Throwbot 1:1da89c13dfa1 55 }
Throwbot 1:1da89c13dfa1 56 int sensorvalue(AnalogIn pin)
Throwbot 1:1da89c13dfa1 57 {
Throwbot 1:1da89c13dfa1 58 //int sum=0;
Throwbot 1:1da89c13dfa1 59 for(int i=0; i<arraysize; i++) {
Throwbot 1:1da89c13dfa1 60 rangevalue[i]= (pin.read()*5*102.54*2.54*10);
Throwbot 1:1da89c13dfa1 61 //sum=sum+rangevalue[i];
Throwbot 1:1da89c13dfa1 62 }
Throwbot 1:1da89c13dfa1 63 //return sum/100;
Throwbot 1:1da89c13dfa1 64 isort(rangevalue,arraysize);
Throwbot 1:1da89c13dfa1 65 //modE = mode(rangevalue,arraysize);
Throwbot 1:1da89c13dfa1 66 modE=rangevalue[arraysize/2];
Throwbot 1:1da89c13dfa1 67 return modE;
Throwbot 2:0f80c8a1c236 68 }
Throwbot 2:0f80c8a1c236 69 void ultrasonic_run()
Throwbot 2:0f80c8a1c236 70 {
Throwbot 3:d1197b5ea68a 71 SRX=1;
Throwbot 3:d1197b5ea68a 72 wait_us(30);
Throwbot 3:d1197b5ea68a 73 SRX=0;
Throwbot 3:d1197b5ea68a 74 Thread::wait(150);
Throwbot 3:d1197b5ea68a 75 stdio_mutex.lock();
Throwbot 3:d1197b5ea68a 76 UL_L=sensorvalue(uL);
Throwbot 3:d1197b5ea68a 77 UL_rL=sensorvalue(urL);
Throwbot 3:d1197b5ea68a 78 UL_B=sensorvalue(uB);
Throwbot 3:d1197b5ea68a 79 UL_rR=sensorvalue(urR);
Throwbot 3:d1197b5ea68a 80 UL_R=sensorvalue(uR);
Throwbot 3:d1197b5ea68a 81 UL_F=sensorvalue(uF);
Throwbot 3:d1197b5ea68a 82 stdio_mutex.unlock();
Throwbot 3:d1197b5ea68a 83 /* SRX=1;
Throwbot 2:0f80c8a1c236 84 wait_ms(5);
Throwbot 2:0f80c8a1c236 85 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 86 UL_rR=sensorvalue(urR);
Throwbot 2:0f80c8a1c236 87 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 88 SRX=0;
Throwbot 2:0f80c8a1c236 89 wait_ms(5);
Throwbot 2:0f80c8a1c236 90
Throwbot 2:0f80c8a1c236 91 SRX=1;
Throwbot 2:0f80c8a1c236 92 wait_ms(5);
Throwbot 2:0f80c8a1c236 93 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 94 UL_R=sensorvalue(uR);
Throwbot 2:0f80c8a1c236 95 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 96 SRX = 0;
Throwbot 2:0f80c8a1c236 97 wait_ms(5);
Throwbot 2:0f80c8a1c236 98
Throwbot 2:0f80c8a1c236 99 SRX=1;
Throwbot 2:0f80c8a1c236 100 wait_ms(5);
Throwbot 2:0f80c8a1c236 101 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 102 UL_F=sensorvalue(uF);
Throwbot 2:0f80c8a1c236 103 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 104 SRX=0;
Throwbot 2:0f80c8a1c236 105 wait_ms(5);
Throwbot 2:0f80c8a1c236 106
Throwbot 2:0f80c8a1c236 107 SRX=1;
Throwbot 2:0f80c8a1c236 108 wait_ms(5);
Throwbot 2:0f80c8a1c236 109 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 110 UL_L=sensorvalue(uL);
Throwbot 2:0f80c8a1c236 111 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 112 SRX=0;
Throwbot 2:0f80c8a1c236 113 wait_ms(5);
Throwbot 2:0f80c8a1c236 114
Throwbot 2:0f80c8a1c236 115 SRX=1;
Throwbot 2:0f80c8a1c236 116 wait_ms(5);
Throwbot 2:0f80c8a1c236 117 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 118 UL_rL=sensorvalue(urL);
Throwbot 2:0f80c8a1c236 119 stdio_mutex.unlock();
Throwbot 2:0f80c8a1c236 120 SRX=0;
Throwbot 2:0f80c8a1c236 121 wait_ms(5);
Throwbot 2:0f80c8a1c236 122
Throwbot 2:0f80c8a1c236 123 SRX=1;
Throwbot 2:0f80c8a1c236 124 wait_ms(5);
Throwbot 2:0f80c8a1c236 125 stdio_mutex.lock();
Throwbot 2:0f80c8a1c236 126 UL_B=sensorvalue(uB);
Throwbot 2:0f80c8a1c236 127 stdio_mutex.unlock();
Throwbot 3:d1197b5ea68a 128 SRX=0; */
Throwbot 2:0f80c8a1c236 129 }