library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers OmniPosition.cpp Source File

OmniPosition.cpp

00001 #include "OmniPosition.h"
00002 
00003 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX)
00004     : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PA_11) {
00005   attach(callback(this, &OmniPosition::receiveByte));
00006   X = 0;
00007   Y = 0;
00008   bufferSize = 9;
00009   bufferPoint = 0;
00010   receivedBytes = 0;
00011 }
00012 
00013 void OmniPosition::receiveByte() {
00014   buffer[bufferPoint % bufferSize] = getc();
00015   if (bufferPoint != 0xff) {
00016     ++bufferPoint;
00017   } else {
00018     bufferPoint = (255 % bufferSize) + 1;
00019   }
00020 
00021   ++receivedBytes;
00022 
00023   if (receivedBytes >= bufferSize) {
00024     checkData();
00025   }
00026 }
00027 
00028 void OmniPosition::checkData() {
00029   for (int i = 0; i < bufferSize; i++) {
00030     if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE &&
00031         buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) {
00032       uint8_t checksum = 0;
00033       for (int j = 0; j < bufferSize - 3; j++) {
00034         checksum += buffer[(i + 2 + j) % bufferSize];
00035       }
00036       if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) {
00037         debugled = !debugled;
00038         X = ((buffer[(i + 2 + 0) % bufferSize] << 8) |
00039              buffer[(i + 2 + 1) % bufferSize]) -
00040             32768;
00041         Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) |
00042              buffer[(i + 2 + 3) % bufferSize]) -
00043             32768;
00044         theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) |
00045                 ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00);
00046         receivedBytes = 0;
00047         return;
00048       }
00049     }
00050   }
00051 }
00052 
00053 int16_t OmniPosition::getX() { return X; }
00054 
00055 int16_t OmniPosition::getY() { return Y; }
00056 
00057 float OmniPosition::getTheta() {
00058   return (float)(theta / 100.0) * (M_PI / 180.0);
00059 }
00060 
00061 void OmniPosition::reset() { putc('R'); }
00062