Shinji Takano / OmniPosition

2019

Dependents:   wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red

OmniPosition.cpp@14:16ee2a638ecb, 2019-06-20 (annotated)

Committer:
highfieldsnj
Date:
Thu Jun 20 08:04:24 2019 +0000
Revision:
14:16ee2a638ecb
Parent:
13:913b647071a8
Child:
15:09c818629bd3
change header

Who changed what in which revision?

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