EEP fORK
Dependencies: BLE_API mbed nRF51822
Fork of MCS_LRF by
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?
User | Revision | Line number | New 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 |