Bernard Mentink / Mbed 2 deprecated MCS_LRF_EEP

EEP fORK

Dependencies:   BLE_API mbed nRF51822

Fork of MCS_LRF by Farshad N

laser.cpp@21:c19355cc5e79, 2018-03-06 (annotated)

Committer:
bmentink
Date:
Tue Mar 06 21:34:31 2018 +0000
Revision:
21:c19355cc5e79
Parent:
18:08184949ab30 
Added following:; 1. ACA modules that control VBUS and ID pins; 2. Turned off laser after powerup

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 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 18:08184949ab30 26 wait_ms(30); // 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 18:08184949ab30 44 setRedDot(true);
Farshad 15:bc4f8c597c26 45 }
Farshad 7:8a23a257b66a 46 }
Farshad 7:8a23a257b66a 47
Farshad 7:8a23a257b66a 48 bool Laser::sendCommand(char cmd[])
Farshad 7:8a23a257b66a 49 {
Farshad 8:ed66e7ef8243 50 // start timer before the first of the command is sent to the laser
Farshad 15:bc4f8c597c26 51 timer.reset();
Farshad 8:ed66e7ef8243 52 timer.start();
Farshad 8:ed66e7ef8243 53 timerRunning = true;
Farshad 8:ed66e7ef8243 54
Farshad 7:8a23a257b66a 55 for(int i = 0; i < strlen(cmd); i++) {
Farshad 7:8a23a257b66a 56 serial.putc(cmd[i]);
Farshad 7:8a23a257b66a 57 }
Farshad 7:8a23a257b66a 58
Farshad 7:8a23a257b66a 59 return true;
Farshad 8:ed66e7ef8243 60 }
Farshad 8:ed66e7ef8243 61
Farshad 8:ed66e7ef8243 62 bool Laser::processResponse()
Farshad 8:ed66e7ef8243 63 {
Farshad 15:bc4f8c597c26 64 #define TIMEOUT 25000 // in units of 100us
Farshad 8:ed66e7ef8243 65 int i = 0;
Farshad 8:ed66e7ef8243 66 char c = 0;
Farshad 8:ed66e7ef8243 67 uint16_t count = 0;
Farshad 15:bc4f8c597c26 68 busy = true;
Farshad 15:bc4f8c597c26 69
Farshad 8:ed66e7ef8243 70 do {
Farshad 8:ed66e7ef8243 71 if(serial.readable()) {
Farshad 8:ed66e7ef8243 72 // 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 73 if(timerRunning) {
Farshad 8:ed66e7ef8243 74 timer.stop();
Farshad 8:ed66e7ef8243 75 timerRunning = false;
Farshad 8:ed66e7ef8243 76 }
Farshad 8:ed66e7ef8243 77 c = serial.getc();
Farshad 8:ed66e7ef8243 78 buf[i++] = c;
Farshad 8:ed66e7ef8243 79 } else {
Farshad 8:ed66e7ef8243 80 wait_us(100);
Farshad 8:ed66e7ef8243 81 }
Farshad 15:bc4f8c597c26 82 } while (c != '\n' && i < bufSize && count++ < TIMEOUT); // timeout after about 500ms and ensure no overflow
Farshad 8:ed66e7ef8243 83
Farshad 8:ed66e7ef8243 84
Farshad 8:ed66e7ef8243 85
Farshad 15:bc4f8c597c26 86 if (count >= TIMEOUT || i >= bufSize) { // timeout or overflow
Farshad 15:bc4f8c597c26 87 // need to reset the LRF module before talking to it again, otherwise it may lock up
Farshad 15:bc4f8c597c26 88 nReset = 0;
Farshad 15:bc4f8c597c26 89 wait_ms(100);
Farshad 15:bc4f8c597c26 90 nReset = 1;
Farshad 15:bc4f8c597c26 91 wait_ms(1000);
Farshad 15:bc4f8c597c26 92 enableMeasurement(true);
Farshad 15:bc4f8c597c26 93 busy = false;
Farshad 8:ed66e7ef8243 94 } else {
Farshad 8:ed66e7ef8243 95 buf[i -1] = 0;
Farshad 8:ed66e7ef8243 96 }
Farshad 8:ed66e7ef8243 97
Farshad 8:ed66e7ef8243 98 float distance = -5;
Farshad 8:ed66e7ef8243 99 vector<char*> v;
Farshad 8:ed66e7ef8243 100
Farshad 8:ed66e7ef8243 101 split(buf, ' ', v);
Farshad 8:ed66e7ef8243 102
Farshad 8:ed66e7ef8243 103 if (v.size() != 6 || atoi(v[1]) != 0 || strcmp(v[5], "Reply") != 0) {
Farshad 8:ed66e7ef8243 104 // there is an error
Farshad 8:ed66e7ef8243 105 distanceCallback(-1.0, 0.0);
Farshad 8:ed66e7ef8243 106 } else {
Farshad 8:ed66e7ef8243 107 float elapsed = (float)(timer.read_us()/1000.0); // elapsed in ms
Farshad 8:ed66e7ef8243 108 distance = atoi(v[2]) / 1000000.0; // distance in m
Farshad 8:ed66e7ef8243 109 distanceCallback(distance, elapsed);
Farshad 8:ed66e7ef8243 110 }
Farshad 15:bc4f8c597c26 111
Farshad 15:bc4f8c597c26 112 busy = false;
Farshad 8:ed66e7ef8243 113
Farshad 8:ed66e7ef8243 114 return true;
Farshad 7:8a23a257b66a 115 }
Farshad 7:8a23a257b66a 116
Farshad 11:0dafbbb3a686 117 void Laser::enableMeasurement(bool enable)
Farshad 11:0dafbbb3a686 118 {
Farshad 12:cf8af0b4e0d2 119 if (enable) {
Farshad 11:0dafbbb3a686 120 sendCommand(";\n");
Farshad 12:cf8af0b4e0d2 121 discardResponse();
Farshad 12:cf8af0b4e0d2 122
Farshad 12:cf8af0b4e0d2 123 } else {
Farshad 11:0dafbbb3a686 124 sendCommand("switchMeasOff\n");
Farshad 18:08184949ab30 125 discardResponse();
Farshad 18:08184949ab30 126 sendCommand("2 0 startPwrSave;");
Farshad 18:08184949ab30 127 discardResponse();
Farshad 12:cf8af0b4e0d2 128 }
Farshad 11:0dafbbb3a686 129 }
Farshad 11:0dafbbb3a686 130
Farshad 11:0dafbbb3a686 131 void Laser::setRedDot(bool on)
Farshad 11:0dafbbb3a686 132 {
Farshad 11:0dafbbb3a686 133 if(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 12:cf8af0b4e0d2 138 } else {
Farshad 11:0dafbbb3a686 139 sendCommand("switchMeasOff\n");
Farshad 12:cf8af0b4e0d2 140 discardResponse();
Farshad 12:cf8af0b4e0d2 141 }
Farshad 11:0dafbbb3a686 142 }
Farshad 7:8a23a257b66a 143
Farshad 7:8a23a257b66a 144 void Laser::split(char str[], char c, std::vector<char*>& v)
Farshad 7:8a23a257b66a 145 {
Farshad 8:ed66e7ef8243 146 char * pch;
Farshad 8:ed66e7ef8243 147 char limiter[] = {c};
Farshad 8:ed66e7ef8243 148 pch = strtok (str, limiter);
Farshad 8:ed66e7ef8243 149 while (pch != NULL) {
Farshad 8:ed66e7ef8243 150 v.push_back(pch);
Farshad 8:ed66e7ef8243 151 pch = strtok (NULL, limiter);
Farshad 8:ed66e7ef8243 152 }
Farshad 7:8a23a257b66a 153 }
Farshad 7:8a23a257b66a 154
Farshad 8:ed66e7ef8243 155 void Laser::setDistaceCallback(void (*callback)(float, float))
Farshad 8:ed66e7ef8243 156 {
Farshad 8:ed66e7ef8243 157 distanceCallback = callback;
Farshad 8:ed66e7ef8243 158 }
Farshad 8:ed66e7ef8243 159
Farshad 8:ed66e7ef8243 160 void Laser::setDebugCallback(void (*callback)(char*))
Farshad 8:ed66e7ef8243 161 {
Farshad 8:ed66e7ef8243 162 debugCallback = callback;
Farshad 8:ed66e7ef8243 163 }
Farshad 8:ed66e7ef8243 164
Farshad 15:bc4f8c597c26 165 void Laser::turnLaserPowerOn()
Farshad 15:bc4f8c597c26 166 {
Farshad 15:bc4f8c597c26 167 if(powerOffState == true) {
Farshad 15:bc4f8c597c26 168 powerOffState = false;
Farshad 15:bc4f8c597c26 169
Farshad 15:bc4f8c597c26 170 connectPower();
Farshad 15:bc4f8c597c26 171 enableMeasurement(true);
Farshad 18:08184949ab30 172 setRedDot(true);
Farshad 15:bc4f8c597c26 173 }
Farshad 15:bc4f8c597c26 174 }
Farshad 15:bc4f8c597c26 175
Farshad 15:bc4f8c597c26 176 void Laser::turnLaserPowerOff()
Farshad 15:bc4f8c597c26 177 {
Farshad 15:bc4f8c597c26 178 if(powerOffState == false) {
Farshad 15:bc4f8c597c26 179 powerOffState = true;
Farshad 18:08184949ab30 180 setRedDot(false); // this disables measurements
Farshad 15:bc4f8c597c26 181
Farshad 15:bc4f8c597c26 182 removePower();
Farshad 15:bc4f8c597c26 183 }
Farshad 15:bc4f8c597c26 184 }
Farshad 15:bc4f8c597c26 185
Farshad 15:bc4f8c597c26 186 // when connceting power to the laser module ensure that tx and rx pins are
Farshad 15:bc4f8c597c26 187 // reassigned to the serial port
Farshad 15:bc4f8c597c26 188 void Laser::connectPower()
Farshad 15:bc4f8c597c26 189 {
Farshad 15:bc4f8c597c26 190 disableLRF = 0;
Farshad 15:bc4f8c597c26 191 nReset = 1;
Farshad 15:bc4f8c597c26 192 *ptx = 27; // p27 for tx
Farshad 15:bc4f8c597c26 193 *prx = 26; // p26 for rx
Farshad 15:bc4f8c597c26 194 wait_ms(1000);
Farshad 15:bc4f8c597c26 195 }
Farshad 15:bc4f8c597c26 196
Farshad 15:bc4f8c597c26 197 // when removing power from the laser module ensure that rx and tx pins are not
Farshad 15:bc4f8c597c26 198 // driving voltage into the laser module to avoid hardware damage
Farshad 15:bc4f8c597c26 199 void Laser::removePower()
Farshad 15:bc4f8c597c26 200 {
Farshad 15:bc4f8c597c26 201 *ptx = 0xffffffff; // no pin for tx
Farshad 15:bc4f8c597c26 202 *prx = 0xffffffff; // no pin for rx
Farshad 15:bc4f8c597c26 203 DigitalOut rx(p26);
Farshad 15:bc4f8c597c26 204 DigitalOut tx(p27);
Farshad 15:bc4f8c597c26 205 rx = 0;
Farshad 15:bc4f8c597c26 206 tx = 0;
Farshad 15:bc4f8c597c26 207 nReset = 0;
Farshad 15:bc4f8c597c26 208 disableLRF = 1;
Farshad 15:bc4f8c597c26 209 }
Farshad 15:bc4f8c597c26 210
Farshad 7:8a23a257b66a 211 Laser::~Laser()
Farshad 7:8a23a257b66a 212 {
Farshad 7:8a23a257b66a 213 enableMeasurement(false);
Farshad 7:8a23a257b66a 214 }