NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@1:0229fc98a26f, 2018-06-30 (annotated)

Committer:
takeuchi
Date:
Sat Jun 30 14:23:07 2018 +0900
Revision:
1:0229fc98a26f
Parent:
0:58910ef3f2b0
Child:
2:edd33d3ad0fd
roughly

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:58910ef3f2b0 1 #include "OmniPosition.h"
UCHITAKE 0:58910ef3f2b0 2
takeuchi 1:0229fc98a26f 3 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
takeuchi 1:0229fc98a26f 4 RawSerial(serialTX, serialRX, DEFAULT_BAUD),
takeuchi 1:0229fc98a26f 5 readCounter(SERIAL_BUFFER_SIZE, 0),
takeuchi 1:0229fc98a26f 6 takeCounter(SERIAL_BUFFER_SIZE, 0)
takeuchi 1:0229fc98a26f 7 {
takeuchi 1:0229fc98a26f 8 buffer = new char[SERIAL_BUFFER_SIZE];
takeuchi 1:0229fc98a26f 9 attach(callback(this, &OmniPosition::readData));
takeuchi 1:0229fc98a26f 10 ticker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
takeuchi 1:0229fc98a26f 11 }
takeuchi 1:0229fc98a26f 12
takeuchi 1:0229fc98a26f 13 void OmniPosition::readData()
takeuchi 1:0229fc98a26f 14 {
takeuchi 1:0229fc98a26f 15 buffer[(int)readCounter] = getc();
takeuchi 1:0229fc98a26f 16 ++readCounter;
takeuchi 1:0229fc98a26f 17 }
takeuchi 1:0229fc98a26f 18
takeuchi 1:0229fc98a26f 19 void OmniPosition::assemble()
UCHITAKE 0:58910ef3f2b0 20 {
takeuchi 1:0229fc98a26f 21 //Find header
takeuchi 1:0229fc98a26f 22 headerCheck = false;
takeuchi 1:0229fc98a26f 23 headerPoint = 0xff;
takeuchi 1:0229fc98a26f 24
takeuchi 1:0229fc98a26f 25 for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
takeuchi 1:0229fc98a26f 26 if(buffer[i] == HEADER_FIRST_BYTE) {
takeuchi 1:0229fc98a26f 27 takeCounter = i;
takeuchi 1:0229fc98a26f 28 ++takeCounter;
takeuchi 1:0229fc98a26f 29 if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
takeuchi 1:0229fc98a26f 30 headerCheck = true;
takeuchi 1:0229fc98a26f 31 headerPoint = i;
takeuchi 1:0229fc98a26f 32 }
takeuchi 1:0229fc98a26f 33 }
takeuchi 1:0229fc98a26f 34 }
takeuchi 1:0229fc98a26f 35 if(headerPoint == 0xff) {
takeuchi 1:0229fc98a26f 36 return;
takeuchi 1:0229fc98a26f 37 }
takeuchi 1:0229fc98a26f 38
takeuchi 1:0229fc98a26f 39 //assemble
takeuchi 1:0229fc98a26f 40 takeCounter = headerPoint; //firstheader
takeuchi 1:0229fc98a26f 41 ++takeCounter; //secondheader
takeuchi 1:0229fc98a26f 42 }