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?
User | Revision | Line number | New 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 | } |