Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

EEP fORK

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

laser.cpp@15:bc4f8c597c26, 2017-02-27 (annotated)

Committer:
Farshad
Date:
Mon Feb 27 01:35:10 2017 +0000
Revision:
15:bc4f8c597c26
Parent:
12:cf8af0b4e0d2
Child:
16:dc9956bac1a3
Child:
18:08184949ab30
Firmware version 1.3. Added option for number of samples needed to do a measurement.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Farshad 7:8a23a257b66a 1
Farshad 7:8a23a257b66a 2 #include "laser.h"
Farshad 7:8a23a257b66a 3
Farshad 8:ed66e7ef8243 4 #define LASER_BAUD_RATE 115200
Farshad 7:8a23a257b66a 5
Farshad 15:bc4f8c597c26 6 extern DigitalOut disableLRF;
Farshad 15:bc4f8c597c26 7 extern DigitalOut nReset;
Farshad 15:bc4f8c597c26 8
Farshad 15:bc4f8c597c26 9 uint32_t base = 0x40002000;
Farshad 15:bc4f8c597c26 10 uint32_t rxOffset = 0x514;
Farshad 15:bc4f8c597c26 11 uint32_t txOffset = 0x50c;
Farshad 15:bc4f8c597c26 12
Farshad 15:bc4f8c597c26 13 uint32_t rx = base + rxOffset;
Farshad 15:bc4f8c597c26 14 uint32_t tx = base + txOffset;
Farshad 15:bc4f8c597c26 15
Farshad 15:bc4f8c597c26 16 uint32_t* prx = (uint32_t*)rx;
Farshad 15:bc4f8c597c26 17 uint32_t* ptx = (uint32_t*)tx;
Farshad 15:bc4f8c597c26 18
Farshad 15:bc4f8c597c26 19 Laser::Laser(Serial& serial, int n) : timerRunning(false), idx(0), serial(serial), powerOffState (true), busy(false), nSamples(n)
Farshad 7:8a23a257b66a 20 {
Farshad 7:8a23a257b66a 21 }
Farshad 7:8a23a257b66a 22
Farshad 12:cf8af0b4e0d2 23 void Laser::discardResponse()
Farshad 12:cf8af0b4e0d2 24 {
Farshad 15:bc4f8c597c26 25 // char c = 0;
Farshad 12:cf8af0b4e0d2 26 wait_ms(20); // wait for the response
Farshad 12:cf8af0b4e0d2 27 while(serial.readable()) {
Farshad 15:bc4f8c597c26 28 serial.getc();
Farshad 12:cf8af0b4e0d2 29 }
Farshad 12:cf8af0b4e0d2 30 }
Farshad 12:cf8af0b4e0d2 31
Farshad 8:ed66e7ef8243 32 void Laser::triggerDistanceMeasurement()
Farshad 8:ed66e7ef8243 33 {
Farshad 15:bc4f8c597c26 34 if(!busy){
Farshad 15:bc4f8c597c26 35 const int bufSize = 30;
Farshad 15:bc4f8c597c26 36 char cmd[bufSize];
Farshad 15:bc4f8c597c26 37 snprintf(&cmd[0], bufSize, "0 -1 %d doMeasDistExt\n", nSamples);
Farshad 15:bc4f8c597c26 38 //char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 15:bc4f8c597c26 39 //char cmd[] = "0 -1 100 doMeasDistExt\n"; // single reading averaged over 100 measurements
Farshad 15:bc4f8c597c26 40 //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading auto choice of number of averages - This could make the laser to lock up and may need reseting
Farshad 15:bc4f8c597c26 41
Farshad 15:bc4f8c597c26 42 sendCommand(cmd);
Farshad 15:bc4f8c597c26 43 processResponse();
Farshad 15:bc4f8c597c26 44 }
Farshad 7:8a23a257b66a 45 }
Farshad 7:8a23a257b66a 46
Farshad 7:8a23a257b66a 47 bool Laser::sendCommand(char cmd[])
Farshad 7:8a23a257b66a 48 {
Farshad 8:ed66e7ef8243 49 // start timer before the first of the command is sent to the laser
Farshad 15:bc4f8c597c26 50 timer.reset();
Farshad 8:ed66e7ef8243 51 timer.start();
Farshad 8:ed66e7ef8243 52 timerRunning = true;
Farshad 8:ed66e7ef8243 53
Farshad 7:8a23a257b66a 54 for(int i = 0; i < strlen(cmd); i++) {
Farshad 7:8a23a257b66a 55 serial.putc(cmd[i]);
Farshad 7:8a23a257b66a 56 }
Farshad 7:8a23a257b66a 57
Farshad 7:8a23a257b66a 58 return true;
Farshad 8:ed66e7ef8243 59 }
Farshad 8:ed66e7ef8243 60
Farshad 8:ed66e7ef8243 61 bool Laser::processResponse()
Farshad 8:ed66e7ef8243 62 {
Farshad 15:bc4f8c597c26 63 #define TIMEOUT 25000 // in units of 100us
Farshad 8:ed66e7ef8243 64 int i = 0;
Farshad 8:ed66e7ef8243 65 char c = 0;
Farshad 8:ed66e7ef8243 66 uint16_t count = 0;
Farshad 15:bc4f8c597c26 67 busy = true;
Farshad 15:bc4f8c597c26 68
Farshad 8:ed66e7ef8243 69 do {
Farshad 8:ed66e7ef8243 70 if(serial.readable()) {
Farshad 8:ed66e7ef8243 71 // stop timer as soon as we have received the first byte of the response. We need to subtract the time of this receiption
Farshad 8:ed66e7ef8243 72 if(timerRunning) {
Farshad 8:ed66e7ef8243 73 timer.stop();
Farshad 8:ed66e7ef8243 74 timerRunning = false;
Farshad 8:ed66e7ef8243 75 }
Farshad 8:ed66e7ef8243 76 c = serial.getc();
Farshad 8:ed66e7ef8243 77 buf[i++] = c;
Farshad 8:ed66e7ef8243 78 } else {
Farshad 8:ed66e7ef8243 79 wait_us(100);
Farshad 8:ed66e7ef8243 80 }
Farshad 15:bc4f8c597c26 81 } while (c != '\n' && i < bufSize && count++ < TIMEOUT); // timeout after about 500ms and ensure no overflow
Farshad 8:ed66e7ef8243 82
Farshad 8:ed66e7ef8243 83
Farshad 8:ed66e7ef8243 84
Farshad 15:bc4f8c597c26 85 if (count >= TIMEOUT || i >= bufSize) { // timeout or overflow
Farshad 15:bc4f8c597c26 86 // need to reset the LRF module before talking to it again, otherwise it may lock up
Farshad 15:bc4f8c597c26 87 nReset = 0;
Farshad 15:bc4f8c597c26 88 wait_ms(100);
Farshad 15:bc4f8c597c26 89 nReset = 1;
Farshad 15:bc4f8c597c26 90 wait_ms(1000);
Farshad 15:bc4f8c597c26 91 enableMeasurement(true);
Farshad 15:bc4f8c597c26 92 busy = false;
Farshad 15:bc4f8c597c26 93 //return true;
Farshad 15:bc4f8c597c26 94
Farshad 8:ed66e7ef8243 95 } else {
Farshad 8:ed66e7ef8243 96 buf[i -1] = 0;
Farshad 8:ed66e7ef8243 97 }
Farshad 8:ed66e7ef8243 98
Farshad 8:ed66e7ef8243 99 float distance = -5;
Farshad 8:ed66e7ef8243 100 vector<char*> v;
Farshad 8:ed66e7ef8243 101
Farshad 8:ed66e7ef8243 102 split(buf, ' ', v);
Farshad 8:ed66e7ef8243 103
Farshad 8:ed66e7ef8243 104 if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 105 // there is an error
Farshad 8:ed66e7ef8243 106 distanceCallback(-1.0, 0.0);
Farshad 8:ed66e7ef8243 107 } else {
Farshad 8:ed66e7ef8243 108 float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 8:ed66e7ef8243 109 distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 110 distanceCallback(distance, elapsed);
Farshad 8:ed66e7ef8243 111 }
Farshad 15:bc4f8c597c26 112
Farshad 15:bc4f8c597c26 113 busy = false;
Farshad 8:ed66e7ef8243 114
Farshad 8:ed66e7ef8243 115 return true;
Farshad 7:8a23a257b66a 116 }
Farshad 7:8a23a257b66a 117
Farshad 11:0dafbbb3a686 118 void Laser::enableMeasurement(bool enable)
Farshad 11:0dafbbb3a686 119 {
Farshad 12:cf8af0b4e0d2 120 if (enable) {
Farshad 11:0dafbbb3a686 121 sendCommand(";\n");
Farshad 12:cf8af0b4e0d2 122 discardResponse();
Farshad 12:cf8af0b4e0d2 123
Farshad 12:cf8af0b4e0d2 124 } else {
Farshad 11:0dafbbb3a686 125 sendCommand("switchMeasOff\n");
Farshad 12:cf8af0b4e0d2 126 discardResponse();
Farshad 12:cf8af0b4e0d2 127 }
Farshad 11:0dafbbb3a686 128 }
Farshad 11:0dafbbb3a686 129
Farshad 11:0dafbbb3a686 130 void Laser::setRedDot(bool on)
Farshad 11:0dafbbb3a686 131 {
Farshad 11:0dafbbb3a686 132 if(on) {
Farshad 15:bc4f8c597c26 133 //char cmd[] = "0 -1 5 doMeasDistExt\n"; // doing a measurement turns the redDot on
Farshad 15:bc4f8c597c26 134 char cmd[] = "0 3 0 -1 3 doLaserCmd;\n";
Farshad 11:0dafbbb3a686 135 sendCommand(cmd);
Farshad 12:cf8af0b4e0d2 136 wait_ms(120); // take a while for a response for to measurement command
Farshad 12:cf8af0b4e0d2 137 discardResponse();
Farshad 15:bc4f8c597c26 138 // triggerDistanceMeasurement();
Farshad 12:cf8af0b4e0d2 139 } else {
Farshad 11:0dafbbb3a686 140 sendCommand("switchMeasOff\n");
Farshad 12:cf8af0b4e0d2 141 discardResponse();
Farshad 12:cf8af0b4e0d2 142 }
Farshad 11:0dafbbb3a686 143 }
Farshad 7:8a23a257b66a 144
Farshad 7:8a23a257b66a 145 void Laser::split(char str[], char c, std::vector<char*>& v)
Farshad 7:8a23a257b66a 146 {
Farshad 8:ed66e7ef8243 147 char * pch;
Farshad 8:ed66e7ef8243 148 char limiter[] = {c};
Farshad 8:ed66e7ef8243 149 pch = strtok (str, limiter);
Farshad 8:ed66e7ef8243 150 while (pch != NULL) {
Farshad 8:ed66e7ef8243 151 v.push_back(pch);
Farshad 8:ed66e7ef8243 152 pch = strtok (NULL, limiter);
Farshad 8:ed66e7ef8243 153 }
Farshad 7:8a23a257b66a 154 }
Farshad 7:8a23a257b66a 155
Farshad 8:ed66e7ef8243 156 void Laser::setDistaceCallback(void (*callback)(float, float))
Farshad 8:ed66e7ef8243 157 {
Farshad 8:ed66e7ef8243 158 distanceCallback = callback;
Farshad 8:ed66e7ef8243 159 }
Farshad 8:ed66e7ef8243 160
Farshad 8:ed66e7ef8243 161 void Laser::setDebugCallback(void (*callback)(char*))
Farshad 8:ed66e7ef8243 162 {
Farshad 8:ed66e7ef8243 163 debugCallback = callback;
Farshad 8:ed66e7ef8243 164 }
Farshad 8:ed66e7ef8243 165
Farshad 15:bc4f8c597c26 166 void Laser::turnLaserPowerOn()
Farshad 15:bc4f8c597c26 167 {
Farshad 15:bc4f8c597c26 168 if(powerOffState == true) {
Farshad 15:bc4f8c597c26 169 powerOffState = false;
Farshad 15:bc4f8c597c26 170
Farshad 15:bc4f8c597c26 171 connectPower();
Farshad 15:bc4f8c597c26 172
Farshad 15:bc4f8c597c26 173 // trun power back on and reinstate serial and laser
Farshad 15:bc4f8c597c26 174 //DigitalIn rx(p26, PullUp);
Farshad 15:bc4f8c597c26 175 // serialPtr = new Serial(p27, p26);
Farshad 15:bc4f8c597c26 176 // serialPtr->baud(READER_BAUD_RATE);
Farshad 15:bc4f8c597c26 177 // disableFlowControl();
Farshad 15:bc4f8c597c26 178 // laserPtr = new Laser(*serialPtr);
Farshad 15:bc4f8c597c26 179
Farshad 15:bc4f8c597c26 180 enableMeasurement(true);
Farshad 15:bc4f8c597c26 181 setRedDot(1);
Farshad 15:bc4f8c597c26 182 }
Farshad 15:bc4f8c597c26 183 }
Farshad 15:bc4f8c597c26 184
Farshad 15:bc4f8c597c26 185 void Laser::turnLaserPowerOff()
Farshad 15:bc4f8c597c26 186 {
Farshad 15:bc4f8c597c26 187 if(powerOffState == false) {
Farshad 15:bc4f8c597c26 188 powerOffState = true;
Farshad 15:bc4f8c597c26 189 setRedDot(0); // this disables measurements
Farshad 15:bc4f8c597c26 190
Farshad 15:bc4f8c597c26 191 removePower();
Farshad 15:bc4f8c597c26 192
Farshad 15:bc4f8c597c26 193 // also take power off the Laser, but first ensure io pins are not powered
Farshad 15:bc4f8c597c26 194 // if(serialPtr != NULL) delete serialPtr;
Farshad 15:bc4f8c597c26 195 // serialPtr = new Serial(NC, p26);
Farshad 15:bc4f8c597c26 196 // DigitalOut rx(p26);
Farshad 15:bc4f8c597c26 197 // DigitalOut tx(p27);
Farshad 15:bc4f8c597c26 198 // rx = 0;
Farshad 15:bc4f8c597c26 199 // tx = 0;
Farshad 15:bc4f8c597c26 200 // if(laserPtr != NULL) delete laserPtr;
Farshad 15:bc4f8c597c26 201 // laserPtr = new Laser(*serialPtr);
Farshad 15:bc4f8c597c26 202
Farshad 15:bc4f8c597c26 203 // nReset = 0;
Farshad 15:bc4f8c597c26 204 // disableLRF = 1;
Farshad 15:bc4f8c597c26 205 }
Farshad 15:bc4f8c597c26 206 }
Farshad 15:bc4f8c597c26 207
Farshad 15:bc4f8c597c26 208 // when connceting power to the laser module ensure that tx and rx pins are
Farshad 15:bc4f8c597c26 209 // reassigned to the serial port
Farshad 15:bc4f8c597c26 210 void Laser::connectPower()
Farshad 15:bc4f8c597c26 211 {
Farshad 15:bc4f8c597c26 212 disableLRF = 0;
Farshad 15:bc4f8c597c26 213 nReset = 1;
Farshad 15:bc4f8c597c26 214 *ptx = 27; // p27 for tx
Farshad 15:bc4f8c597c26 215 *prx = 26; // p26 for rx
Farshad 15:bc4f8c597c26 216 wait_ms(1000);
Farshad 15:bc4f8c597c26 217 }
Farshad 15:bc4f8c597c26 218
Farshad 15:bc4f8c597c26 219 // when removing power from the laser module ensure that rx and tx pins are not
Farshad 15:bc4f8c597c26 220 // driving voltage into the laser module to avoid hardware damage
Farshad 15:bc4f8c597c26 221 void Laser::removePower()
Farshad 15:bc4f8c597c26 222 {
Farshad 15:bc4f8c597c26 223 *ptx = 0xffffffff; // no pin for tx
Farshad 15:bc4f8c597c26 224 *prx = 0xffffffff; // no pin for rx
Farshad 15:bc4f8c597c26 225 DigitalOut rx(p26);
Farshad 15:bc4f8c597c26 226 DigitalOut tx(p27);
Farshad 15:bc4f8c597c26 227 rx = 0;
Farshad 15:bc4f8c597c26 228 tx = 0;
Farshad 15:bc4f8c597c26 229 nReset = 0;
Farshad 15:bc4f8c597c26 230 disableLRF = 1;
Farshad 15:bc4f8c597c26 231 }
Farshad 15:bc4f8c597c26 232
Farshad 7:8a23a257b66a 233 Laser::~Laser()
Farshad 7:8a23a257b66a 234 {
Farshad 7:8a23a257b66a 235 enableMeasurement(false);
Farshad 7:8a23a257b66a 236 }
Farshad 10:d37cd13dd529 237
Farshad 10:d37cd13dd529 238
Farshad 10:d37cd13dd529 239
Farshad 11:0dafbbb3a686 240
Farshad 11:0dafbbb3a686 241 //void Laser::processRxData(char c)
Farshad 11:0dafbbb3a686 242 //{
Farshad 11:0dafbbb3a686 243 // if(c != '\n') {
Farshad 11:0dafbbb3a686 244 // buf[idx++] = c;
Farshad 11:0dafbbb3a686 245 // if(idx == bufSize) idx = 0; // avoid overflow
Farshad 11:0dafbbb3a686 246 // } else {
Farshad 11:0dafbbb3a686 247 // buf[idx] = 0; // null terminate the string
Farshad 11:0dafbbb3a686 248 // processBuffer();
Farshad 11:0dafbbb3a686 249 // idx = 0;
Farshad 11:0dafbbb3a686 250 // }
Farshad 11:0dafbbb3a686 251 //}
Farshad 11:0dafbbb3a686 252
Farshad 10:d37cd13dd529 253 //float Laser::getDistance()
Farshad 10:d37cd13dd529 254 //{
Farshad 10:d37cd13dd529 255 // float distance = -4.0;
Farshad 10:d37cd13dd529 256 // const int bufSize = 50;
Farshad 10:d37cd13dd529 257 // char data[bufSize];
Farshad 10:d37cd13dd529 258 // int i = 0;
Farshad 10:d37cd13dd529 259 // //char cmd[] = ";\n";
Farshad 10:d37cd13dd529 260 // char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 10:d37cd13dd529 261 // //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading of single measurement
Farshad 10:d37cd13dd529 262 //
Farshad 10:d37cd13dd529 263 // if (sendCommand(cmd) == true) {
Farshad 10:d37cd13dd529 264 // // Note: Need to actually read from the serial to clear the RX interrupt
Farshad 10:d37cd13dd529 265 // char c = 0;
Farshad 10:d37cd13dd529 266 // uint16_t count = 0;
Farshad 10:d37cd13dd529 267 // do {
Farshad 10:d37cd13dd529 268 // if(serial.readable()) {
Farshad 10:d37cd13dd529 269 // c = serial.getc();
Farshad 10:d37cd13dd529 270 // data[i++] = c;
Farshad 10:d37cd13dd529 271 // } else {
Farshad 10:d37cd13dd529 272 // wait_us(100);
Farshad 10:d37cd13dd529 273 // }
Farshad 10:d37cd13dd529 274 // } while (c != '\n' && i < bufSize && count++ < 5000); // timeout after about 500ms and ensure no overflow
Farshad 10:d37cd13dd529 275 //
Farshad 10:d37cd13dd529 276 // if (count >= 5000 || i >= bufSize) return -2.0; // timeout or overflow
Farshad 10:d37cd13dd529 277 //
Farshad 10:d37cd13dd529 278 // // now that we have the reply parse it
Farshad 10:d37cd13dd529 279 // data[i - 1] = 0; // null terminate the string
Farshad 10:d37cd13dd529 280 //
Farshad 10:d37cd13dd529 281 // //reply is in this form '2 0 1844364 324 Reply'. 2nd number is error code, 3rd number is distance in um, 4th number is measurement signal
Farshad 10:d37cd13dd529 282 // vector<char*> v;
Farshad 10:d37cd13dd529 283 // split(data, ' ', v);
Farshad 10:d37cd13dd529 284 //
Farshad 10:d37cd13dd529 285 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 10:d37cd13dd529 286 // // there is an error
Farshad 10:d37cd13dd529 287 // distance = -3.0;
Farshad 10:d37cd13dd529 288 // } else {
Farshad 10:d37cd13dd529 289 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 10:d37cd13dd529 290 // }
Farshad 10:d37cd13dd529 291 // }
Farshad 10:d37cd13dd529 292 //
Farshad 10:d37cd13dd529 293 // return distance;
Farshad 10:d37cd13dd529 294 //}
Farshad 10:d37cd13dd529 295
Farshad 11:0dafbbb3a686 296 //void Laser::processBuffer()
Farshad 11:0dafbbb3a686 297 //{
Farshad 11:0dafbbb3a686 298 // //float distance;
Farshad 11:0dafbbb3a686 299 //// vector<char*> v;
Farshad 11:0dafbbb3a686 300 //// // debugCallback(buf);
Farshad 11:0dafbbb3a686 301 ////
Farshad 11:0dafbbb3a686 302 //// split(buf, ' ', v);
Farshad 11:0dafbbb3a686 303 ////
Farshad 11:0dafbbb3a686 304 //// if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 11:0dafbbb3a686 305 //// // there is an error
Farshad 11:0dafbbb3a686 306 //// distanceCallback(-1.0);
Farshad 11:0dafbbb3a686 307 //// } else
Farshad 11:0dafbbb3a686 308 //// {
Farshad 11:0dafbbb3a686 309 //// distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 11:0dafbbb3a686 310 //// distanceCallback(distance);
Farshad 11:0dafbbb3a686 311 //// }
Farshad 11:0dafbbb3a686 312 //
Farshad 11:0dafbbb3a686 313 //
Farshad 11:0dafbbb3a686 314 // float distance = -5;
Farshad 11:0dafbbb3a686 315 // vector<char*> v;
Farshad 11:0dafbbb3a686 316 //
Farshad 11:0dafbbb3a686 317 // split(buf, ' ', v);
Farshad 11:0dafbbb3a686 318 //
Farshad 11:0dafbbb3a686 319 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 11:0dafbbb3a686 320 // // there is an error
Farshad 11:0dafbbb3a686 321 // distanceCallback(-1.0, 0.0);
Farshad 11:0dafbbb3a686 322 // } else {
Farshad 11:0dafbbb3a686 323 // float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 11:0dafbbb3a686 324 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 11:0dafbbb3a686 325 // distanceCallback(distance, elapsed);
Farshad 11:0dafbbb3a686 326 // }
Farshad 11:0dafbbb3a686 327 //}
Farshad 11:0dafbbb3a686 328