NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@3:47676abdf529, 2018-07-31 (annotated)

Committer:
tanabe2000
Date:
Tue Jul 31 08:30:35 2018 +0000
Revision:
3:47676abdf529
Parent:
2:edd33d3ad0fd
Child:
4:fc4c88fffef8
ver2.1;

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