Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

EEP fORK

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

laser.cpp@12:cf8af0b4e0d2, 2016-10-17 (annotated)

Committer:
Farshad
Date:
Mon Oct 17 04:50:33 2016 +0000
Revision:
12:cf8af0b4e0d2
Parent:
11:0dafbbb3a686
Child:
15:bc4f8c597c26
Added low battery check and power save after i minute of inactivity. Also added function to reset the laser module.

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