NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@2:edd33d3ad0fd, 2018-07-01 (annotated)

Committer:
takeuchi
Date:
Sun Jul 01 16:39:13 2018 +0900
Revision:
2:edd33d3ad0fd
Parent:
1:0229fc98a26f
Child:
3:47676abdf529
working

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 2:edd33d3ad0fd 6 takeCounter(SERIAL_BUFFER_SIZE, 0),
takeuchi 2:edd33d3ad0fd 7 X(0),
takeuchi 2:edd33d3ad0fd 8 Y(0)
takeuchi 1:0229fc98a26f 9 {
takeuchi 1:0229fc98a26f 10 buffer = new char[SERIAL_BUFFER_SIZE];
takeuchi 2:edd33d3ad0fd 11 data = new char[2];
takeuchi 1:0229fc98a26f 12 attach(callback(this, &OmniPosition::readData));
takeuchi 2:edd33d3ad0fd 13 assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
takeuchi 2:edd33d3ad0fd 14 sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
takeuchi 1:0229fc98a26f 15 }
takeuchi 1:0229fc98a26f 16
takeuchi 1:0229fc98a26f 17 void OmniPosition::readData()
takeuchi 1:0229fc98a26f 18 {
takeuchi 1:0229fc98a26f 19 buffer[(int)readCounter] = getc();
takeuchi 1:0229fc98a26f 20 ++readCounter;
takeuchi 1:0229fc98a26f 21 }
takeuchi 1:0229fc98a26f 22
takeuchi 1:0229fc98a26f 23 void OmniPosition::assemble()
UCHITAKE 0:58910ef3f2b0 24 {
takeuchi 1:0229fc98a26f 25 //Find header
takeuchi 1:0229fc98a26f 26 headerCheck = false;
takeuchi 1:0229fc98a26f 27 headerPoint = 0xff;
takeuchi 1:0229fc98a26f 28
takeuchi 1:0229fc98a26f 29 for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
takeuchi 1:0229fc98a26f 30 if(buffer[i] == HEADER_FIRST_BYTE) {
takeuchi 1:0229fc98a26f 31 takeCounter = i;
takeuchi 1:0229fc98a26f 32 ++takeCounter;
takeuchi 1:0229fc98a26f 33 if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
takeuchi 1:0229fc98a26f 34 headerCheck = true;
takeuchi 1:0229fc98a26f 35 headerPoint = i;
takeuchi 1:0229fc98a26f 36 }
takeuchi 1:0229fc98a26f 37 }
takeuchi 1:0229fc98a26f 38 }
takeuchi 1:0229fc98a26f 39 if(headerPoint == 0xff) {
takeuchi 1:0229fc98a26f 40 return;
takeuchi 1:0229fc98a26f 41 }
takeuchi 1:0229fc98a26f 42
takeuchi 1:0229fc98a26f 43 //assemble
takeuchi 1:0229fc98a26f 44 takeCounter = headerPoint; //firstheader
takeuchi 1:0229fc98a26f 45 ++takeCounter; //secondheader
takeuchi 2:edd33d3ad0fd 46
takeuchi 2:edd33d3ad0fd 47 ++takeCounter;
takeuchi 2:edd33d3ad0fd 48 data[0] = buffer[(int)takeCounter];
takeuchi 2:edd33d3ad0fd 49 ++takeCounter;
takeuchi 2:edd33d3ad0fd 50 data[1] = buffer[(int)takeCounter];
takeuchi 2:edd33d3ad0fd 51 X = ((data[0]<<8)|data[1]) - 32768;
takeuchi 2:edd33d3ad0fd 52
takeuchi 2:edd33d3ad0fd 53 ++takeCounter;
takeuchi 2:edd33d3ad0fd 54 data[0] = buffer[(int)takeCounter];
takeuchi 2:edd33d3ad0fd 55 ++takeCounter;
takeuchi 2:edd33d3ad0fd 56 data[1] = buffer[(int)takeCounter];
takeuchi 2:edd33d3ad0fd 57 Y = ((data[0]<<8)|data[1]) - 32768;
takeuchi 1:0229fc98a26f 58 }
takeuchi 2:edd33d3ad0fd 59
takeuchi 2:edd33d3ad0fd 60 void OmniPosition::send()
takeuchi 2:edd33d3ad0fd 61 {
takeuchi 2:edd33d3ad0fd 62 if(resetSend) {
takeuchi 2:edd33d3ad0fd 63 putc('R');
takeuchi 2:edd33d3ad0fd 64 resetSend = false;
takeuchi 2:edd33d3ad0fd 65 } else {
takeuchi 2:edd33d3ad0fd 66 //putc(0);
takeuchi 2:edd33d3ad0fd 67 }
takeuchi 2:edd33d3ad0fd 68 }
takeuchi 2:edd33d3ad0fd 69
takeuchi 2:edd33d3ad0fd 70 int OmniPosition::getX()
takeuchi 2:edd33d3ad0fd 71 {
takeuchi 2:edd33d3ad0fd 72 return X;
takeuchi 2:edd33d3ad0fd 73 }
takeuchi 2:edd33d3ad0fd 74
takeuchi 2:edd33d3ad0fd 75 int OmniPosition::getY()
takeuchi 2:edd33d3ad0fd 76 {
takeuchi 2:edd33d3ad0fd 77 return Y;
takeuchi 2:edd33d3ad0fd 78 }
takeuchi 2:edd33d3ad0fd 79
takeuchi 2:edd33d3ad0fd 80 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 81 {
takeuchi 2:edd33d3ad0fd 82 resetSend = true;
takeuchi 2:edd33d3ad0fd 83 }