TFmini(Micro LiDAR Module) library tested on mbedos5.x
Dependents: 2018NHK_gakugaku_robo 2018NHK_gaku_ver2 2019NHK_A_sensor
TFmini.cpp@3:04330e82af20, 2018-08-22 (annotated)
- Committer:
- UCHITAKE
- Date:
- Wed Aug 22 10:59:25 2018 +0000
- Revision:
- 3:04330e82af20
- Parent:
- 1:12f2bfb3577a
- Child:
- 4:7622beb45675
fix func
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
UCHITAKE | 0:fe2031b62770 | 1 | #include "TFmini.h" |
UCHITAKE | 0:fe2031b62770 | 2 | |
UCHITAKE | 0:fe2031b62770 | 3 | TFmini::TFmini(PinName serialTX, PinName serialRX) : |
takeuchi |
1:12f2bfb3577a | 4 | RawSerial(serialTX, serialRX, TF_DEFAULT_BAUD), |
takeuchi |
1:12f2bfb3577a | 5 | readCounter(TF_SERIAL_BUFFER_SIZE), |
takeuchi |
1:12f2bfb3577a | 6 | takeCounter(TF_SERIAL_BUFFER_SIZE), |
UCHITAKE | 0:fe2031b62770 | 7 | distance(0) |
UCHITAKE | 0:fe2031b62770 | 8 | { |
takeuchi |
1:12f2bfb3577a | 9 | buffer = new uint8_t[TF_SERIAL_BUFFER_SIZE]; |
UCHITAKE | 0:fe2031b62770 | 10 | data = new uint8_t[2]; |
UCHITAKE | 0:fe2031b62770 | 11 | attach(callback(this, &TFmini::readData)); |
takeuchi |
1:12f2bfb3577a | 12 | assembleTicker.attach(callback(this, &TFmini::assemble), TF_RECEIVE_FREQ); |
UCHITAKE | 0:fe2031b62770 | 13 | } |
UCHITAKE | 0:fe2031b62770 | 14 | |
UCHITAKE | 0:fe2031b62770 | 15 | void TFmini::readData() |
UCHITAKE | 0:fe2031b62770 | 16 | { |
UCHITAKE | 0:fe2031b62770 | 17 | buffer[(int)readCounter] = getc(); |
UCHITAKE | 3:04330e82af20 | 18 | readCounter = incrementCounter(readCounter); |
UCHITAKE | 0:fe2031b62770 | 19 | } |
UCHITAKE | 0:fe2031b62770 | 20 | |
UCHITAKE | 0:fe2031b62770 | 21 | void TFmini::assemble() |
UCHITAKE | 0:fe2031b62770 | 22 | { |
UCHITAKE | 0:fe2031b62770 | 23 | //Find header |
UCHITAKE | 0:fe2031b62770 | 24 | headerCheck = false; |
UCHITAKE | 0:fe2031b62770 | 25 | headerPoint = 0xff; |
UCHITAKE | 0:fe2031b62770 | 26 | |
takeuchi |
1:12f2bfb3577a | 27 | for(int i = 0; i < TF_SERIAL_BUFFER_SIZE; i++) { |
takeuchi |
1:12f2bfb3577a | 28 | if(buffer[i] == TF_HEADER_FIRST_BYTE) { |
UCHITAKE | 0:fe2031b62770 | 29 | takeCounter = i; |
UCHITAKE | 3:04330e82af20 | 30 | takeCounter = incrementCounter(takeCounter); |
takeuchi |
1:12f2bfb3577a | 31 | if(buffer[(int)takeCounter] == TF_HEADER_SECOND_BYTE) { |
UCHITAKE | 0:fe2031b62770 | 32 | headerCheck = true; |
UCHITAKE | 0:fe2031b62770 | 33 | headerPoint = i; |
UCHITAKE | 0:fe2031b62770 | 34 | } |
UCHITAKE | 0:fe2031b62770 | 35 | } |
UCHITAKE | 0:fe2031b62770 | 36 | } |
UCHITAKE | 0:fe2031b62770 | 37 | if(headerPoint == 0xff) { |
UCHITAKE | 0:fe2031b62770 | 38 | return; |
UCHITAKE | 0:fe2031b62770 | 39 | } |
UCHITAKE | 0:fe2031b62770 | 40 | |
UCHITAKE | 0:fe2031b62770 | 41 | //assemble |
UCHITAKE | 0:fe2031b62770 | 42 | takeCounter = headerPoint; //firstheader |
UCHITAKE | 3:04330e82af20 | 43 | takeCounter = incrementCounter(takeCounter); //secondheader |
UCHITAKE | 0:fe2031b62770 | 44 | |
UCHITAKE | 3:04330e82af20 | 45 | takeCounter = incrementCounter(takeCounter); |
UCHITAKE | 0:fe2031b62770 | 46 | data[0] = buffer[(int)takeCounter]; |
UCHITAKE | 3:04330e82af20 | 47 | takeCounter = incrementCounter(takeCounter); |
UCHITAKE | 0:fe2031b62770 | 48 | data[1] = buffer[(int)takeCounter]; |
UCHITAKE | 0:fe2031b62770 | 49 | distance = ((data[1]<<8)|data[0]); |
UCHITAKE | 0:fe2031b62770 | 50 | |
UCHITAKE | 3:04330e82af20 | 51 | takeCounter = incrementCounter(takeCounter); |
UCHITAKE | 0:fe2031b62770 | 52 | data[0] = buffer[(int)takeCounter]; |
UCHITAKE | 3:04330e82af20 | 53 | takeCounter = incrementCounter(takeCounter); |
UCHITAKE | 0:fe2031b62770 | 54 | data[1] = buffer[(int)takeCounter]; |
UCHITAKE | 0:fe2031b62770 | 55 | strength = ((data[1]<<8)|data[0]); |
UCHITAKE | 0:fe2031b62770 | 56 | } |
UCHITAKE | 0:fe2031b62770 | 57 | |
UCHITAKE | 3:04330e82af20 | 58 | int TFmini::incrementCounter(int counter) |
takeuchi |
1:12f2bfb3577a | 59 | { |
takeuchi |
1:12f2bfb3577a | 60 | ++counter; |
UCHITAKE | 3:04330e82af20 | 61 | if(counter >= TF_SERIAL_BUFFER_SIZE) { |
takeuchi |
1:12f2bfb3577a | 62 | counter -= TF_SERIAL_BUFFER_SIZE; |
takeuchi |
1:12f2bfb3577a | 63 | } |
UCHITAKE | 3:04330e82af20 | 64 | return counter; |
takeuchi |
1:12f2bfb3577a | 65 | } |
takeuchi |
1:12f2bfb3577a | 66 | |
UCHITAKE | 0:fe2031b62770 | 67 | int TFmini::getDistance() |
UCHITAKE | 0:fe2031b62770 | 68 | { |
UCHITAKE | 0:fe2031b62770 | 69 | return distance; |
takeuchi |
1:12f2bfb3577a | 70 | } |
UCHITAKE | 3:04330e82af20 | 71 | |
UCHITAKE | 3:04330e82af20 | 72 | int TFmini::getStrength() |
UCHITAKE | 3:04330e82af20 | 73 | { |
UCHITAKE | 3:04330e82af20 | 74 | return strength; |
UCHITAKE | 3:04330e82af20 | 75 | } |