Calibrated Sharp distance sensors
Dependents: steppertest R5 2016 Robotics Team 1
LongRangeSensor.cpp@1:aed20a8e1da0, 2016-04-06 (annotated)
- Committer:
- j_j205
- Date:
- Wed Apr 06 22:02:15 2016 +0000
- Revision:
- 1:aed20a8e1da0
- Parent:
- 0:c8c67fabc804
- Child:
- 2:ed1f0f09eb5b
changes to localization
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
j_j205 | 0:c8c67fabc804 | 1 | #include "LongRangeSensor.h" |
j_j205 | 0:c8c67fabc804 | 2 | #include "mbed.h" |
j_j205 | 0:c8c67fabc804 | 3 | #include <cmath> // pow |
j_j205 | 0:c8c67fabc804 | 4 | |
j_j205 | 0:c8c67fabc804 | 5 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 6 | // LongRangeSensor(Serial &pc1, PinName _sensor) |
j_j205 | 0:c8c67fabc804 | 7 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 8 | // Serial &pc1, PinName _sensor |
j_j205 | 0:c8c67fabc804 | 9 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 10 | // None |
j_j205 | 0:c8c67fabc804 | 11 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 12 | // Constructor. |
j_j205 | 0:c8c67fabc804 | 13 | LongRangeSensor::LongRangeSensor(Serial &pc1, PinName _sensor) : |
j_j205 | 0:c8c67fabc804 | 14 | pc(pc1), sensor(_sensor) |
j_j205 | 0:c8c67fabc804 | 15 | {} |
j_j205 | 0:c8c67fabc804 | 16 | |
j_j205 | 0:c8c67fabc804 | 17 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 18 | // distL() |
j_j205 | 0:c8c67fabc804 | 19 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 20 | // None |
j_j205 | 0:c8c67fabc804 | 21 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 22 | // Returns distance in cm |
j_j205 | 0:c8c67fabc804 | 23 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 24 | // Returns average of 3 readings in cm. |
j_j205 | 0:c8c67fabc804 | 25 | float LongRangeSensor::distL() |
j_j205 | 0:c8c67fabc804 | 26 | { |
j_j205 | 0:c8c67fabc804 | 27 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 28 | |
j_j205 | 0:c8c67fabc804 | 29 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 30 | { |
j_j205 | 0:c8c67fabc804 | 31 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 32 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 33 | } |
j_j205 | 0:c8c67fabc804 | 34 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 35 | |
j_j205 | 0:c8c67fabc804 | 36 | double distMeas = 3.5097*pow(sensorRead, -1.425); |
j_j205 | 0:c8c67fabc804 | 37 | |
j_j205 | 0:c8c67fabc804 | 38 | return distMeas; |
j_j205 | 0:c8c67fabc804 | 39 | } |
j_j205 | 0:c8c67fabc804 | 40 | |
j_j205 | 0:c8c67fabc804 | 41 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 42 | // distInchesL() |
j_j205 | 0:c8c67fabc804 | 43 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 44 | // None |
j_j205 | 0:c8c67fabc804 | 45 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 46 | // Returns distance in cm |
j_j205 | 0:c8c67fabc804 | 47 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 48 | // Returns average of 3 readings in inches. |
j_j205 | 0:c8c67fabc804 | 49 | float LongRangeSensor::distInchesL() |
j_j205 | 0:c8c67fabc804 | 50 | { |
j_j205 | 0:c8c67fabc804 | 51 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 52 | |
j_j205 | 0:c8c67fabc804 | 53 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 54 | { |
j_j205 | 0:c8c67fabc804 | 55 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 56 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 57 | } |
j_j205 | 0:c8c67fabc804 | 58 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 59 | |
j_j205 | 0:c8c67fabc804 | 60 | double distMeas = 3.5097*pow(sensorRead, -1.425); |
j_j205 | 0:c8c67fabc804 | 61 | |
j_j205 | 0:c8c67fabc804 | 62 | return distMeas*0.393701; |
j_j205 | 0:c8c67fabc804 | 63 | } |
j_j205 | 0:c8c67fabc804 | 64 | |
j_j205 | 0:c8c67fabc804 | 65 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 66 | // distR() |
j_j205 | 0:c8c67fabc804 | 67 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 68 | // None |
j_j205 | 0:c8c67fabc804 | 69 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 70 | // Returns distance in cm |
j_j205 | 0:c8c67fabc804 | 71 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 72 | // Returns average of 3 readings in cm. |
j_j205 | 0:c8c67fabc804 | 73 | float LongRangeSensor::distR() |
j_j205 | 0:c8c67fabc804 | 74 | { |
j_j205 | 0:c8c67fabc804 | 75 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 76 | |
j_j205 | 0:c8c67fabc804 | 77 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 78 | { |
j_j205 | 0:c8c67fabc804 | 79 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 80 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 81 | } |
j_j205 | 0:c8c67fabc804 | 82 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 83 | |
j_j205 | 0:c8c67fabc804 | 84 | double distMeas = 3.4617*pow(sensorRead, -1.506); |
j_j205 | 0:c8c67fabc804 | 85 | |
j_j205 | 0:c8c67fabc804 | 86 | return distMeas; |
j_j205 | 0:c8c67fabc804 | 87 | } |
j_j205 | 0:c8c67fabc804 | 88 | |
j_j205 | 0:c8c67fabc804 | 89 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 90 | // distInchesR() |
j_j205 | 0:c8c67fabc804 | 91 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 92 | // None |
j_j205 | 0:c8c67fabc804 | 93 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 94 | // Returns distance in cm |
j_j205 | 0:c8c67fabc804 | 95 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 96 | // Returns average of 3 readings in inches. |
j_j205 | 0:c8c67fabc804 | 97 | float LongRangeSensor::distInchesR() |
j_j205 | 0:c8c67fabc804 | 98 | { |
j_j205 | 0:c8c67fabc804 | 99 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 100 | |
j_j205 | 0:c8c67fabc804 | 101 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 102 | { |
j_j205 | 0:c8c67fabc804 | 103 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 104 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 105 | } |
j_j205 | 0:c8c67fabc804 | 106 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 107 | |
j_j205 | 0:c8c67fabc804 | 108 | double distMeas = 3.4617*pow(sensorRead, -1.506); |
j_j205 | 0:c8c67fabc804 | 109 | |
j_j205 | 0:c8c67fabc804 | 110 | return distMeas*0.393701; |
j_j205 | 0:c8c67fabc804 | 111 | } |
j_j205 | 0:c8c67fabc804 | 112 | |
j_j205 | 0:c8c67fabc804 | 113 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 114 | // distAdjL() |
j_j205 | 0:c8c67fabc804 | 115 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 116 | // None |
j_j205 | 0:c8c67fabc804 | 117 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 118 | // Returns adjusted distance in cm |
j_j205 | 0:c8c67fabc804 | 119 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 120 | // Gets dist from sensor L and adjusts for error. |
j_j205 | 0:c8c67fabc804 | 121 | float LongRangeSensor::distAdjL() |
j_j205 | 0:c8c67fabc804 | 122 | { |
j_j205 | 0:c8c67fabc804 | 123 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 124 | |
j_j205 | 0:c8c67fabc804 | 125 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 126 | { |
j_j205 | 0:c8c67fabc804 | 127 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 128 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 129 | } |
j_j205 | 0:c8c67fabc804 | 130 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 131 | |
j_j205 | 0:c8c67fabc804 | 132 | double distMeas = 3.5097*pow(sensorRead, -1.425); |
j_j205 | 0:c8c67fabc804 | 133 | |
j_j205 | 0:c8c67fabc804 | 134 | double readError = (0.0052*pow(distMeas, 2.0)) - (0.3218*distMeas) |
j_j205 | 0:c8c67fabc804 | 135 | + 2.6653; |
j_j205 | 0:c8c67fabc804 | 136 | |
j_j205 | 0:c8c67fabc804 | 137 | return float(distMeas + readError); |
j_j205 | 0:c8c67fabc804 | 138 | } |
j_j205 | 0:c8c67fabc804 | 139 | |
j_j205 | 0:c8c67fabc804 | 140 | // FUNCTION: |
j_j205 | 0:c8c67fabc804 | 141 | // distAdjR() |
j_j205 | 0:c8c67fabc804 | 142 | // IN-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 143 | // None |
j_j205 | 0:c8c67fabc804 | 144 | // OUT-PARAMETERS: |
j_j205 | 0:c8c67fabc804 | 145 | // Returns adjusted distance in cm. |
j_j205 | 0:c8c67fabc804 | 146 | // DESCRIPTION: |
j_j205 | 0:c8c67fabc804 | 147 | // Gets dist for sensor R and adjusts for error. |
j_j205 | 0:c8c67fabc804 | 148 | float LongRangeSensor::distAdjR() |
j_j205 | 0:c8c67fabc804 | 149 | { |
j_j205 | 0:c8c67fabc804 | 150 | double sensorRead = 0.0; |
j_j205 | 0:c8c67fabc804 | 151 | |
j_j205 | 0:c8c67fabc804 | 152 | for(int i = 0; i < 3; i++) |
j_j205 | 0:c8c67fabc804 | 153 | { |
j_j205 | 0:c8c67fabc804 | 154 | sensorRead += sensor.read(); |
j_j205 | 0:c8c67fabc804 | 155 | wait(20e-3); |
j_j205 | 0:c8c67fabc804 | 156 | } |
j_j205 | 0:c8c67fabc804 | 157 | sensorRead = sensorRead/3.0; |
j_j205 | 0:c8c67fabc804 | 158 | |
j_j205 | 0:c8c67fabc804 | 159 | double distMeas = 3.4617*pow(sensorRead, -1.506); |
j_j205 | 0:c8c67fabc804 | 160 | |
j_j205 | 0:c8c67fabc804 | 161 | double readError = (0.0062*pow(distMeas, 2.0)) - (0.385*distMeas) |
j_j205 | 0:c8c67fabc804 | 162 | + 3.1692; |
j_j205 | 0:c8c67fabc804 | 163 | |
j_j205 | 0:c8c67fabc804 | 164 | return float(distMeas + readError); |
j_j205 | 1:aed20a8e1da0 | 165 | } |
j_j205 | 1:aed20a8e1da0 | 166 | |
j_j205 | 1:aed20a8e1da0 | 167 | // FUNCTION: |
j_j205 | 1:aed20a8e1da0 | 168 | // distInchesLOne() |
j_j205 | 1:aed20a8e1da0 | 169 | // IN-PARAMETERS: |
j_j205 | 1:aed20a8e1da0 | 170 | // None |
j_j205 | 1:aed20a8e1da0 | 171 | // OUT-PARAMETERS: |
j_j205 | 1:aed20a8e1da0 | 172 | // Returns distance in inches |
j_j205 | 1:aed20a8e1da0 | 173 | // DESCRIPTION: |
j_j205 | 1:aed20a8e1da0 | 174 | // Returns 1 reading in inches. |
j_j205 | 1:aed20a8e1da0 | 175 | float LongRangeSensor::distInchesLOne() |
j_j205 | 1:aed20a8e1da0 | 176 | { |
j_j205 | 1:aed20a8e1da0 | 177 | double sensorRead = sensor.read(); |
j_j205 | 1:aed20a8e1da0 | 178 | |
j_j205 | 1:aed20a8e1da0 | 179 | return 0.393701*(3.5097*pow(sensorRead, -1.425)); |
j_j205 | 1:aed20a8e1da0 | 180 | } |
j_j205 | 1:aed20a8e1da0 | 181 | |
j_j205 | 1:aed20a8e1da0 | 182 | // FUNCTION: |
j_j205 | 1:aed20a8e1da0 | 183 | // distInchesROne() |
j_j205 | 1:aed20a8e1da0 | 184 | // IN-PARAMETERS: |
j_j205 | 1:aed20a8e1da0 | 185 | // None |
j_j205 | 1:aed20a8e1da0 | 186 | // OUT-PARAMETERS: |
j_j205 | 1:aed20a8e1da0 | 187 | // Returns distance in inches |
j_j205 | 1:aed20a8e1da0 | 188 | // DESCRIPTION: |
j_j205 | 1:aed20a8e1da0 | 189 | // Returns average of 3 readings in inches. |
j_j205 | 1:aed20a8e1da0 | 190 | float LongRangeSensor::distInchesROne() |
j_j205 | 1:aed20a8e1da0 | 191 | { |
j_j205 | 1:aed20a8e1da0 | 192 | double sensorRead = sensor.read(); |
j_j205 | 1:aed20a8e1da0 | 193 | |
j_j205 | 1:aed20a8e1da0 | 194 | return 0.393701*(3.4617*pow(sensorRead, -1.506)); |
j_j205 | 1:aed20a8e1da0 | 195 | } |