Shinji Takano / OmniPosition

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red

OmniPosition.cpp@13:913b647071a8, 2018-10-23 (annotated)

Committer:
UCHITAKE
Date:
Tue Oct 23 19:29:35 2018 +0900
Revision:
13:913b647071a8
Parent:
12:edd4217ad7a5
Child:
14:16ee2a638ecb
tukaeru

Who changed what in which revision?

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