Texas State IEEE / LongRangeSensor

Calibrated Sharp distance sensors

Dependents:   steppertest R5 2016 Robotics Team 1

LongRangeSensor.cpp@2:ed1f0f09eb5b, 2016-04-08 (annotated)

Committer:
j_j205
Date:
Fri Apr 08 04:35:52 2016 +0000
Revision:
2:ed1f0f09eb5b
Parent:
1:aed20a8e1da0 
latest

Who changed what in which revision?

UserRevisionLine numberNew 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 2:ed1f0f09eb5b 60 double distMeas = 3.1814*pow(sensorRead, -1.8824); //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 2:ed1f0f09eb5b 179 return 0.393701*(3.1814*pow(sensorRead, -1.8824)); // 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 }