2019
Dependents: wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red
OmniPosition.cpp
- Committer:
- UCHITAKE
- Date:
- 2018-10-06
- Revision:
- 10:a21aa2bd05c5
- Parent:
- 9:222f7fcbd05a
- Child:
- 11:7028941a60c1
File content as of revision 10:a21aa2bd05c5:
#include "OmniPosition.h" OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PB_8) { attach(callback(this, &OmniPosition::receiveByte)); X = 0; Y = 0; bufferSize = 9; bufferPoint = 0; receivedBytes = 0; } void OmniPosition::receiveByte() { buffer[bufferPoint % bufferSize] = getc(); if (bufferPoint != 0xff) { ++bufferPoint; } else { bufferPoint = (255 % bufferSize) + 1; } ++receivedBytes; if (receivedBytes >= bufferSize) { checkData(); } } void OmniPosition::checkData() { for (int i = 0; i < bufferSize; i++) { if (buffer[i % bufferSize] == OP_HEADER_FIRST_BYTE && buffer[(i + 1) % bufferSize] == OP_HEADER_SECOND_BYTE) { uint8_t checksum = 0; for (int j = 0; j < bufferSize - 3; j++) { checksum += buffer[(i + 2 + j) % bufferSize]; } if ((uint8_t)checksum == buffer[(i + bufferSize - 1) % bufferSize]) { debugled = !debugled; X = ((buffer[(i + 2 + 0) % bufferSize] << 8) | buffer[(i + 2 + 1) % bufferSize]) - 32768; Y = ((buffer[(i + 2 + 2) % bufferSize] << 8) | buffer[(i + 2 + 3) % bufferSize]) - 32768; theta = (buffer[(i + 2 + 4) % bufferSize] & 0xFF) | ((buffer[(i + 2 + 5) % bufferSize] << 8) & 0xFF00); receivedBytes = 0; return; } } } } int16_t OmniPosition::getX() { return X; } int16_t OmniPosition::getY() { return Y; } float OmniPosition::getTheta() { return (float)(theta / 100.0) * (M_PI / 180.0); } void OmniPosition::reset() { putc('R'); }