EEP fORK
Dependencies: BLE_API mbed nRF51822
Fork of MCS_LRF by
laser.cpp@7:8a23a257b66a, 2015-12-09 (annotated)
- Committer:
- Farshad
- Date:
- Wed Dec 09 03:28:42 2015 +0000
- Revision:
- 7:8a23a257b66a
- Child:
- 8:ed66e7ef8243
First cut. Testing works with single measurement averaging 10 readings.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Farshad | 7:8a23a257b66a | 1 | |
Farshad | 7:8a23a257b66a | 2 | |
Farshad | 7:8a23a257b66a | 3 | |
Farshad | 7:8a23a257b66a | 4 | #include "laser.h" |
Farshad | 7:8a23a257b66a | 5 | |
Farshad | 7:8a23a257b66a | 6 | |
Farshad | 7:8a23a257b66a | 7 | Laser::Laser(Serial& serial) : serial(serial) |
Farshad | 7:8a23a257b66a | 8 | { |
Farshad | 7:8a23a257b66a | 9 | } |
Farshad | 7:8a23a257b66a | 10 | |
Farshad | 7:8a23a257b66a | 11 | |
Farshad | 7:8a23a257b66a | 12 | float Laser::getDistance() |
Farshad | 7:8a23a257b66a | 13 | { |
Farshad | 7:8a23a257b66a | 14 | float distance = -2.0; |
Farshad | 7:8a23a257b66a | 15 | const int bufSize = 50; |
Farshad | 7:8a23a257b66a | 16 | char data[bufSize]; |
Farshad | 7:8a23a257b66a | 17 | int i = 0; |
Farshad | 7:8a23a257b66a | 18 | //char cmd[] = ";\n"; |
Farshad | 7:8a23a257b66a | 19 | char cmd[] = "0 -1 10 doMeasDistExt\n"; // single reading averaged over 10 measurements |
Farshad | 7:8a23a257b66a | 20 | //char cmd[] = "0 -1 -1 doMeasDistExt\n"; // single reading of single measurement |
Farshad | 7:8a23a257b66a | 21 | if (sendCommand(cmd) == true) { |
Farshad | 7:8a23a257b66a | 22 | // Note: Need to actually read from the serial to clear the RX interrupt |
Farshad | 7:8a23a257b66a | 23 | char c = 0; |
Farshad | 7:8a23a257b66a | 24 | uint16_t count = 0; |
Farshad | 7:8a23a257b66a | 25 | do { |
Farshad | 7:8a23a257b66a | 26 | if(serial.readable()) { |
Farshad | 7:8a23a257b66a | 27 | c = serial.getc(); |
Farshad | 7:8a23a257b66a | 28 | data[i++] = c; |
Farshad | 7:8a23a257b66a | 29 | } |
Farshad | 7:8a23a257b66a | 30 | else{wait_us(100);} |
Farshad | 7:8a23a257b66a | 31 | } while (c != '\n' && count++ < 1000); |
Farshad | 7:8a23a257b66a | 32 | //while (c != '\n'); |
Farshad | 7:8a23a257b66a | 33 | if (count >= 1000) return -1; |
Farshad | 7:8a23a257b66a | 34 | |
Farshad | 7:8a23a257b66a | 35 | // now that we have the reply parse it |
Farshad | 7:8a23a257b66a | 36 | data[i - 1] = 0; // null terminate the string |
Farshad | 7:8a23a257b66a | 37 | |
Farshad | 7:8a23a257b66a | 38 | //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 | 7:8a23a257b66a | 39 | vector<char*> v; |
Farshad | 7:8a23a257b66a | 40 | split(data, ' ', v); |
Farshad | 7:8a23a257b66a | 41 | |
Farshad | 7:8a23a257b66a | 42 | if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) { |
Farshad | 7:8a23a257b66a | 43 | // there is an error |
Farshad | 7:8a23a257b66a | 44 | distance = -1.0; |
Farshad | 7:8a23a257b66a | 45 | } else { |
Farshad | 7:8a23a257b66a | 46 | distance = atoi(v[2]) / 1000000.0; // distance in m |
Farshad | 7:8a23a257b66a | 47 | } |
Farshad | 7:8a23a257b66a | 48 | } |
Farshad | 7:8a23a257b66a | 49 | |
Farshad | 7:8a23a257b66a | 50 | return distance; |
Farshad | 7:8a23a257b66a | 51 | } |
Farshad | 7:8a23a257b66a | 52 | |
Farshad | 7:8a23a257b66a | 53 | void Laser::enableMeasurement(bool enable) |
Farshad | 7:8a23a257b66a | 54 | { |
Farshad | 7:8a23a257b66a | 55 | if (enable) |
Farshad | 7:8a23a257b66a | 56 | sendCommand(";\n"); |
Farshad | 7:8a23a257b66a | 57 | else |
Farshad | 7:8a23a257b66a | 58 | sendCommand("switchMeasOff"); |
Farshad | 7:8a23a257b66a | 59 | } |
Farshad | 7:8a23a257b66a | 60 | |
Farshad | 7:8a23a257b66a | 61 | bool Laser::sendCommand(char cmd[]) |
Farshad | 7:8a23a257b66a | 62 | { |
Farshad | 7:8a23a257b66a | 63 | for(int i = 0; i < strlen(cmd); i++) { |
Farshad | 7:8a23a257b66a | 64 | serial.putc(cmd[i]); |
Farshad | 7:8a23a257b66a | 65 | } |
Farshad | 7:8a23a257b66a | 66 | |
Farshad | 7:8a23a257b66a | 67 | return true; |
Farshad | 7:8a23a257b66a | 68 | } |
Farshad | 7:8a23a257b66a | 69 | |
Farshad | 7:8a23a257b66a | 70 | |
Farshad | 7:8a23a257b66a | 71 | void Laser::split(char str[], char c, std::vector<char*>& v) |
Farshad | 7:8a23a257b66a | 72 | { |
Farshad | 7:8a23a257b66a | 73 | char * pch; |
Farshad | 7:8a23a257b66a | 74 | char limiter[] = {c}; |
Farshad | 7:8a23a257b66a | 75 | pch = strtok (str, limiter); |
Farshad | 7:8a23a257b66a | 76 | while (pch != NULL) |
Farshad | 7:8a23a257b66a | 77 | { |
Farshad | 7:8a23a257b66a | 78 | v.push_back(pch); |
Farshad | 7:8a23a257b66a | 79 | pch = strtok (NULL, limiter); |
Farshad | 7:8a23a257b66a | 80 | } |
Farshad | 7:8a23a257b66a | 81 | } |
Farshad | 7:8a23a257b66a | 82 | |
Farshad | 7:8a23a257b66a | 83 | Laser::~Laser() |
Farshad | 7:8a23a257b66a | 84 | { |
Farshad | 7:8a23a257b66a | 85 | enableMeasurement(false); |
Farshad | 7:8a23a257b66a | 86 | } |