Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

laser.cpp@10:d37cd13dd529, 2015-12-22 (annotated)

Committer:
Farshad
Date:
Tue Dec 22 03:46:12 2015 +0000
Revision:
10:d37cd13dd529
Parent:
8:ed66e7ef8243
Child:
11:0dafbbb3a686
Added command to turn the red dot on and off. Also use the button to push and hold to turn the red dot off.

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 8:ed66e7ef8243 6 Laser::Laser(Serial& serial) : timerRunning(false), idx(0), serial(serial)
Farshad 7:8a23a257b66a 7 {
Farshad 7:8a23a257b66a 8 }
Farshad 7:8a23a257b66a 9
Farshad 8:ed66e7ef8243 10 void Laser::triggerDistanceMeasurement()
Farshad 8:ed66e7ef8243 11 {
Farshad 8:ed66e7ef8243 12 char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 8:ed66e7ef8243 13 //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading of single measurement - This could make the laser to lock up and may need reseting
Farshad 8:ed66e7ef8243 14
Farshad 8:ed66e7ef8243 15
Farshad 8:ed66e7ef8243 16 sendCommand(cmd);
Farshad 8:ed66e7ef8243 17
Farshad 8:ed66e7ef8243 18 processResponse();
Farshad 8:ed66e7ef8243 19 // float distance = -5;
Farshad 8:ed66e7ef8243 20 // vector<char*> v;
Farshad 8:ed66e7ef8243 21 //
Farshad 8:ed66e7ef8243 22 // split(buf, ' ', v);
Farshad 8:ed66e7ef8243 23 //
Farshad 8:ed66e7ef8243 24 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 25 // // there is an error
Farshad 8:ed66e7ef8243 26 // distanceCallback(-1.0);
Farshad 8:ed66e7ef8243 27 // } else {
Farshad 8:ed66e7ef8243 28 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 29 // distanceCallback(distance);
Farshad 8:ed66e7ef8243 30 // }
Farshad 8:ed66e7ef8243 31 //
Farshad 8:ed66e7ef8243 32 //
Farshad 8:ed66e7ef8243 33 // // processBuffer();
Farshad 8:ed66e7ef8243 34
Farshad 8:ed66e7ef8243 35 // float distance = getDistance();
Farshad 8:ed66e7ef8243 36 // distanceCallback(distance);
Farshad 8:ed66e7ef8243 37 }
Farshad 8:ed66e7ef8243 38
Farshad 8:ed66e7ef8243 39 void Laser::processBuffer()
Farshad 8:ed66e7ef8243 40 {
Farshad 8:ed66e7ef8243 41 //float distance;
Farshad 8:ed66e7ef8243 42 // vector<char*> v;
Farshad 8:ed66e7ef8243 43 // // debugCallback(buf);
Farshad 8:ed66e7ef8243 44 //
Farshad 8:ed66e7ef8243 45 // split(buf, ' ', v);
Farshad 8:ed66e7ef8243 46 //
Farshad 8:ed66e7ef8243 47 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 48 // // there is an error
Farshad 8:ed66e7ef8243 49 // distanceCallback(-1.0);
Farshad 8:ed66e7ef8243 50 // } else
Farshad 8:ed66e7ef8243 51 // {
Farshad 8:ed66e7ef8243 52 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 53 // distanceCallback(distance);
Farshad 8:ed66e7ef8243 54 // }
Farshad 8:ed66e7ef8243 55
Farshad 8:ed66e7ef8243 56
Farshad 8:ed66e7ef8243 57 float distance = -5;
Farshad 8:ed66e7ef8243 58 vector<char*> v;
Farshad 8:ed66e7ef8243 59
Farshad 8:ed66e7ef8243 60 split(buf, ' ', v);
Farshad 8:ed66e7ef8243 61
Farshad 8:ed66e7ef8243 62 if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 63 // there is an error
Farshad 8:ed66e7ef8243 64 distanceCallback(-1.0, 0.0);
Farshad 8:ed66e7ef8243 65 } else {
Farshad 8:ed66e7ef8243 66 float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 8:ed66e7ef8243 67 distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 68 distanceCallback(distance, elapsed);
Farshad 8:ed66e7ef8243 69 }
Farshad 8:ed66e7ef8243 70 }
Farshad 8:ed66e7ef8243 71
Farshad 8:ed66e7ef8243 72
Farshad 8:ed66e7ef8243 73
Farshad 7:8a23a257b66a 74 void Laser::enableMeasurement(bool enable)
Farshad 7:8a23a257b66a 75 {
Farshad 7:8a23a257b66a 76 if (enable)
Farshad 7:8a23a257b66a 77 sendCommand(";\n");
Farshad 7:8a23a257b66a 78 else
Farshad 10:d37cd13dd529 79 sendCommand("switchMeasOff\n");
Farshad 10:d37cd13dd529 80 }
Farshad 10:d37cd13dd529 81
Farshad 10:d37cd13dd529 82 void Laser::setRedDot(bool on)
Farshad 10:d37cd13dd529 83 {
Farshad 10:d37cd13dd529 84 if(on) {
Farshad 10:d37cd13dd529 85 char cmd[] = "0 -1 10 doMeasDistExt\n"; // doing a measurement turns the redDot on
Farshad 10:d37cd13dd529 86 sendCommand(cmd);
Farshad 10:d37cd13dd529 87 } else
Farshad 10:d37cd13dd529 88 sendCommand("switchMeasOff\n");
Farshad 7:8a23a257b66a 89 }
Farshad 7:8a23a257b66a 90
Farshad 7:8a23a257b66a 91 bool Laser::sendCommand(char cmd[])
Farshad 7:8a23a257b66a 92 {
Farshad 8:ed66e7ef8243 93 // start timer before the first of the command is sent to the laser
Farshad 8:ed66e7ef8243 94 timer.reset();
Farshad 8:ed66e7ef8243 95 timer.start();
Farshad 8:ed66e7ef8243 96 timerRunning = true;
Farshad 8:ed66e7ef8243 97
Farshad 7:8a23a257b66a 98 for(int i = 0; i < strlen(cmd); i++) {
Farshad 7:8a23a257b66a 99 serial.putc(cmd[i]);
Farshad 7:8a23a257b66a 100 }
Farshad 7:8a23a257b66a 101
Farshad 7:8a23a257b66a 102 return true;
Farshad 8:ed66e7ef8243 103
Farshad 8:ed66e7ef8243 104 // return processResponse();
Farshad 8:ed66e7ef8243 105 }
Farshad 8:ed66e7ef8243 106
Farshad 8:ed66e7ef8243 107 bool Laser::processResponse()
Farshad 8:ed66e7ef8243 108 {
Farshad 8:ed66e7ef8243 109
Farshad 8:ed66e7ef8243 110 // // Note: Need to actually read from the serial to clear the RX interrupt
Farshad 8:ed66e7ef8243 111 // char c = 0;
Farshad 8:ed66e7ef8243 112 // uint16_t count = 0;
Farshad 8:ed66e7ef8243 113 // idx = 0;
Farshad 8:ed66e7ef8243 114 // do {
Farshad 8:ed66e7ef8243 115 // if(serial.readable()) {
Farshad 8:ed66e7ef8243 116 // buf[idx++] = serial.getc();
Farshad 8:ed66e7ef8243 117 // }
Farshad 8:ed66e7ef8243 118 // else{wait_us(100);}
Farshad 8:ed66e7ef8243 119 // } while (c != '\n' && idx < bufSize && count++ < 5000); // timeout after about 500ms and ensure no overflow
Farshad 8:ed66e7ef8243 120 //
Farshad 8:ed66e7ef8243 121 // if (count >= 5000 || idx == 0 || idx >= bufSize) return false; // timeout or overflow
Farshad 8:ed66e7ef8243 122 //
Farshad 8:ed66e7ef8243 123 // // now that we have the reply parse it
Farshad 8:ed66e7ef8243 124 // buf[idx - 1] = 0; // null terminate the string
Farshad 8:ed66e7ef8243 125 //
Farshad 8:ed66e7ef8243 126 // // processBuffer();
Farshad 8:ed66e7ef8243 127 // return true;
Farshad 8:ed66e7ef8243 128
Farshad 8:ed66e7ef8243 129
Farshad 8:ed66e7ef8243 130
Farshad 8:ed66e7ef8243 131 int i = 0;
Farshad 8:ed66e7ef8243 132 char c = 0;
Farshad 8:ed66e7ef8243 133 uint16_t count = 0;
Farshad 8:ed66e7ef8243 134 do {
Farshad 8:ed66e7ef8243 135 if(serial.readable()) {
Farshad 8:ed66e7ef8243 136 // 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 137 if(timerRunning) {
Farshad 8:ed66e7ef8243 138 timer.stop();
Farshad 8:ed66e7ef8243 139 timerRunning = false;
Farshad 8:ed66e7ef8243 140 }
Farshad 8:ed66e7ef8243 141 c = serial.getc();
Farshad 8:ed66e7ef8243 142 buf[i++] = c;
Farshad 8:ed66e7ef8243 143 } else {
Farshad 8:ed66e7ef8243 144 wait_us(100);
Farshad 8:ed66e7ef8243 145 }
Farshad 8:ed66e7ef8243 146 } while (c != '\n' && i < bufSize && count++ < 5000); // timeout after about 500ms and ensure no overflow
Farshad 8:ed66e7ef8243 147
Farshad 8:ed66e7ef8243 148
Farshad 8:ed66e7ef8243 149
Farshad 8:ed66e7ef8243 150 if (count >= 5000 || i >= bufSize) { // timeout or overflow
Farshad 8:ed66e7ef8243 151 } else {
Farshad 8:ed66e7ef8243 152 buf[i -1] = 0;
Farshad 8:ed66e7ef8243 153 }
Farshad 8:ed66e7ef8243 154
Farshad 8:ed66e7ef8243 155 float distance = -5;
Farshad 8:ed66e7ef8243 156 vector<char*> v;
Farshad 8:ed66e7ef8243 157
Farshad 8:ed66e7ef8243 158 split(buf, ' ', v);
Farshad 8:ed66e7ef8243 159
Farshad 8:ed66e7ef8243 160 if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 161 // there is an error
Farshad 8:ed66e7ef8243 162 distanceCallback(-1.0, 0.0);
Farshad 8:ed66e7ef8243 163 } else {
Farshad 8:ed66e7ef8243 164 float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 8:ed66e7ef8243 165 distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 166 distanceCallback(distance, elapsed);
Farshad 8:ed66e7ef8243 167 }
Farshad 8:ed66e7ef8243 168
Farshad 8:ed66e7ef8243 169 //processBuffer();
Farshad 8:ed66e7ef8243 170 return true;
Farshad 7:8a23a257b66a 171 }
Farshad 7:8a23a257b66a 172
Farshad 7:8a23a257b66a 173
Farshad 7:8a23a257b66a 174 void Laser::split(char str[], char c, std::vector<char*>& v)
Farshad 7:8a23a257b66a 175 {
Farshad 8:ed66e7ef8243 176 char * pch;
Farshad 8:ed66e7ef8243 177 char limiter[] = {c};
Farshad 8:ed66e7ef8243 178 pch = strtok (str, limiter);
Farshad 8:ed66e7ef8243 179 while (pch != NULL) {
Farshad 8:ed66e7ef8243 180 v.push_back(pch);
Farshad 8:ed66e7ef8243 181 pch = strtok (NULL, limiter);
Farshad 8:ed66e7ef8243 182 }
Farshad 7:8a23a257b66a 183 }
Farshad 7:8a23a257b66a 184
Farshad 8:ed66e7ef8243 185 void Laser::processRxData(char c)
Farshad 8:ed66e7ef8243 186 {
Farshad 8:ed66e7ef8243 187 if(c != '\n') {
Farshad 8:ed66e7ef8243 188 buf[idx++] = c;
Farshad 8:ed66e7ef8243 189 if(idx == bufSize) idx = 0; // avoid overflow
Farshad 8:ed66e7ef8243 190 } else {
Farshad 8:ed66e7ef8243 191 buf[idx] = 0; // null terminate the string
Farshad 8:ed66e7ef8243 192 processBuffer();
Farshad 8:ed66e7ef8243 193 idx = 0;
Farshad 8:ed66e7ef8243 194 }
Farshad 8:ed66e7ef8243 195 }
Farshad 8:ed66e7ef8243 196
Farshad 8:ed66e7ef8243 197 void Laser::setDistaceCallback(void (*callback)(float, float))
Farshad 8:ed66e7ef8243 198 {
Farshad 8:ed66e7ef8243 199 distanceCallback = callback;
Farshad 8:ed66e7ef8243 200 }
Farshad 8:ed66e7ef8243 201
Farshad 8:ed66e7ef8243 202 void Laser::setDebugCallback(void (*callback)(char*))
Farshad 8:ed66e7ef8243 203 {
Farshad 8:ed66e7ef8243 204 debugCallback = callback;
Farshad 8:ed66e7ef8243 205 }
Farshad 8:ed66e7ef8243 206
Farshad 8:ed66e7ef8243 207
Farshad 7:8a23a257b66a 208 Laser::~Laser()
Farshad 7:8a23a257b66a 209 {
Farshad 7:8a23a257b66a 210 enableMeasurement(false);
Farshad 7:8a23a257b66a 211 }
Farshad 10:d37cd13dd529 212
Farshad 10:d37cd13dd529 213
Farshad 10:d37cd13dd529 214
Farshad 10:d37cd13dd529 215 //float Laser::getDistance()
Farshad 10:d37cd13dd529 216 //{
Farshad 10:d37cd13dd529 217 // float distance = -4.0;
Farshad 10:d37cd13dd529 218 // const int bufSize = 50;
Farshad 10:d37cd13dd529 219 // char data[bufSize];
Farshad 10:d37cd13dd529 220 // int i = 0;
Farshad 10:d37cd13dd529 221 // //char cmd[] = ";\n";
Farshad 10:d37cd13dd529 222 // char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements
Farshad 10:d37cd13dd529 223 // //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading of single measurement
Farshad 10:d37cd13dd529 224 //
Farshad 10:d37cd13dd529 225 // if (sendCommand(cmd) == true) {
Farshad 10:d37cd13dd529 226 // // Note: Need to actually read from the serial to clear the RX interrupt
Farshad 10:d37cd13dd529 227 // char c = 0;
Farshad 10:d37cd13dd529 228 // uint16_t count = 0;
Farshad 10:d37cd13dd529 229 // do {
Farshad 10:d37cd13dd529 230 // if(serial.readable()) {
Farshad 10:d37cd13dd529 231 // c = serial.getc();
Farshad 10:d37cd13dd529 232 // data[i++] = c;
Farshad 10:d37cd13dd529 233 // } else {
Farshad 10:d37cd13dd529 234 // wait_us(100);
Farshad 10:d37cd13dd529 235 // }
Farshad 10:d37cd13dd529 236 // } while (c != '\n' && i < bufSize && count++ < 5000); // timeout after about 500ms and ensure no overflow
Farshad 10:d37cd13dd529 237 //
Farshad 10:d37cd13dd529 238 // if (count >= 5000 || i >= bufSize) return -2.0; // timeout or overflow
Farshad 10:d37cd13dd529 239 //
Farshad 10:d37cd13dd529 240 // // now that we have the reply parse it
Farshad 10:d37cd13dd529 241 // data[i - 1] = 0; // null terminate the string
Farshad 10:d37cd13dd529 242 //
Farshad 10:d37cd13dd529 243 // //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 244 // vector<char*> v;
Farshad 10:d37cd13dd529 245 // split(data, ' ', v);
Farshad 10:d37cd13dd529 246 //
Farshad 10:d37cd13dd529 247 // if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 10:d37cd13dd529 248 // // there is an error
Farshad 10:d37cd13dd529 249 // distance = -3.0;
Farshad 10:d37cd13dd529 250 // } else {
Farshad 10:d37cd13dd529 251 // distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 10:d37cd13dd529 252 // }
Farshad 10:d37cd13dd529 253 // }
Farshad 10:d37cd13dd529 254 //
Farshad 10:d37cd13dd529 255 // return distance;
Farshad 10:d37cd13dd529 256 //}
Farshad 10:d37cd13dd529 257