NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@11:7028941a60c1, 2018-10-08 (annotated)

Committer:
UCHITAKE
Date:
Mon Oct 08 21:05:15 2018 +0900
Revision:
11:7028941a60c1
Parent:
10:a21aa2bd05c5 
change led

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:58910ef3f2b0 1 #include "OmniPosition.h"
UCHITAKE 0:58910ef3f2b0 2
UCHITAKE 10:a21aa2bd05c5 3 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX)
UCHITAKE 11:7028941a60c1 4 : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PA_11) {
UCHITAKE 10:a21aa2bd05c5 5 attach(callback(this, &OmniPosition::receiveByte));
UCHITAKE 10:a21aa2bd05c5 6 X = 0;
UCHITAKE 10:a21aa2bd05c5 7 Y = 0;
UCHITAKE 10:a21aa2bd05c5 8 bufferSize = 9;
UCHITAKE 10:a21aa2bd05c5 9 bufferPoint = 0;
UCHITAKE 10:a21aa2bd05c5 10 receivedBytes = 0;
takeuchi 6:f8dbbe93bc7b 11 }
takeuchi 1:0229fc98a26f 12
UCHITAKE 10:a21aa2bd05c5 13 void OmniPosition::receiveByte() {
UCHITAKE 10:a21aa2bd05c5 14 buffer[bufferPoint % bufferSize] = getc();
UCHITAKE 10:a21aa2bd05c5 15 if (bufferPoint != 0xff) {
UCHITAKE 10:a21aa2bd05c5 16 ++bufferPoint;
UCHITAKE 10:a21aa2bd05c5 17 } else {
UCHITAKE 10:a21aa2bd05c5 18 bufferPoint = (255 % bufferSize) + 1;
UCHITAKE 10:a21aa2bd05c5 19 }
UCHITAKE 10:a21aa2bd05c5 20
UCHITAKE 10:a21aa2bd05c5 21 ++receivedBytes;
UCHITAKE 10:a21aa2bd05c5 22
UCHITAKE 10:a21aa2bd05c5 23 if (receivedBytes >= bufferSize) {
UCHITAKE 10:a21aa2bd05c5 24 checkData();
UCHITAKE 10:a21aa2bd05c5 25 }
takeuchi 5:f8c3aeb4e65f 26 }
takeuchi 5:f8c3aeb4e65f 27
UCHITAKE 10:a21aa2bd05c5 28 void OmniPosition::checkData() {
UCHITAKE 10:a21aa2bd05c5 29 for (int i = 0; i < bufferSize; i++) {
UCHITAKE 10:a21aa2bd05c5 30 if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE &&
UCHITAKE 10:a21aa2bd05c5 31 buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) {
UCHITAKE 10:a21aa2bd05c5 32 uint8_t checksum = 0;
UCHITAKE 10:a21aa2bd05c5 33 for (int j = 0; j < bufferSize - 3; j++) {
UCHITAKE 10:a21aa2bd05c5 34 checksum += buffer[(i + 2 + j) % bufferSize];
UCHITAKE 10:a21aa2bd05c5 35 }
UCHITAKE 10:a21aa2bd05c5 36 if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) {
UCHITAKE 10:a21aa2bd05c5 37 debugled = !debugled;
UCHITAKE 10:a21aa2bd05c5 38 X = ((buffer[(i + 2 + 0) % bufferSize] << 8) |
UCHITAKE 10:a21aa2bd05c5 39 buffer[(i + 2 + 1) % bufferSize]) -
UCHITAKE 10:a21aa2bd05c5 40 32768;
UCHITAKE 10:a21aa2bd05c5 41 Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) |
UCHITAKE 10:a21aa2bd05c5 42 buffer[(i + 2 + 3) % bufferSize]) -
UCHITAKE 10:a21aa2bd05c5 43 32768;
UCHITAKE 10:a21aa2bd05c5 44 theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) |
UCHITAKE 10:a21aa2bd05c5 45 ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00);
UCHITAKE 10:a21aa2bd05c5 46 receivedBytes = 0;
UCHITAKE 10:a21aa2bd05c5 47 return;
UCHITAKE 10:a21aa2bd05c5 48 }
UCHITAKE 10:a21aa2bd05c5 49 }
UCHITAKE 10:a21aa2bd05c5 50 }
takeuchi 4:fc4c88fffef8 51 }
takeuchi 4:fc4c88fffef8 52
UCHITAKE 10:a21aa2bd05c5 53 int16_t OmniPosition::getX() { return X; }
UCHITAKE 10:a21aa2bd05c5 54
UCHITAKE 10:a21aa2bd05c5 55 int16_t OmniPosition::getY() { return Y; }
UCHITAKE 10:a21aa2bd05c5 56
UCHITAKE 10:a21aa2bd05c5 57 float OmniPosition::getTheta() {
UCHITAKE 10:a21aa2bd05c5 58 return (float)(theta / 100.0) * (M_PI / 180.0);
takeuchi 4:fc4c88fffef8 59 }
takeuchi 2:edd33d3ad0fd 60
UCHITAKE 10:a21aa2bd05c5 61 void OmniPosition::reset() { putc('R'); }
takeuchi 4:fc4c88fffef8 62