2021 NHK B
OmniPosition.cpp@10:a21aa2bd05c5, 2018-10-06 (annotated)
- Committer:
- UCHITAKE
- Date:
- Sat Oct 06 05:00:58 2018 +0000
- Revision:
- 10:a21aa2bd05c5
- Parent:
- 9:222f7fcbd05a
- Child:
- 11:7028941a60c1
tinyfix
Who changed what in which revision?
User | Revision | Line number | New 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 | 10:a21aa2bd05c5 | 4 | : RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD), debugled(PB_8) { |
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 |