Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

EEP fORK

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

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?

UserRevisionLine numberNew 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 }