NagaokaRoboticsClub_mbedTeam / TFmini

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?

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