library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
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
Generated on Mon Jul 25 2022 06:50:48 by
1.7.2