NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@4:fc4c88fffef8, 2018-08-21 (annotated)

Committer:
takeuchi
Date:
Tue Aug 21 13:41:25 2018 +0900
Revision:
4:fc4c88fffef8
Parent:
3:47676abdf529
Child:
5:f8c3aeb4e65f
fix func

Who changed what in which revision?

UserRevisionLine numberNew contents of line
UCHITAKE 0:58910ef3f2b0 1 #include "OmniPosition.h"
UCHITAKE 0:58910ef3f2b0 2
takeuchi 1:0229fc98a26f 3 OmniPosition::OmniPosition(PinName serialTX, PinName serialRX) :
takeuchi 1:0229fc98a26f 4 RawSerial(serialTX, serialRX, DEFAULT_BAUD),
takeuchi 1:0229fc98a26f 5 readCounter(SERIAL_BUFFER_SIZE, 0),
takeuchi 2:edd33d3ad0fd 6 takeCounter(SERIAL_BUFFER_SIZE, 0),
takeuchi 4:fc4c88fffef8 7 X(0),
takeuchi 4:fc4c88fffef8 8 Y(0),
takeuchi 4:fc4c88fffef8 9 theta(0.0)
takeuchi 1:0229fc98a26f 10 {
takeuchi 4:fc4c88fffef8 11 buffer = new char[SERIAL_BUFFER_SIZE];
takeuchi 4:fc4c88fffef8 12 data = new char[2];
takeuchi 1:0229fc98a26f 13 attach(callback(this, &OmniPosition::readData));
takeuchi 2:edd33d3ad0fd 14 assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ);
takeuchi 2:edd33d3ad0fd 15 sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ);
takeuchi 1:0229fc98a26f 16 }
takeuchi 1:0229fc98a26f 17
takeuchi 1:0229fc98a26f 18 void OmniPosition::readData()
takeuchi 1:0229fc98a26f 19 {
takeuchi 1:0229fc98a26f 20 buffer[(int)readCounter] = getc();
takeuchi 1:0229fc98a26f 21 ++readCounter;
takeuchi 1:0229fc98a26f 22 }
takeuchi 1:0229fc98a26f 23
takeuchi 1:0229fc98a26f 24 void OmniPosition::assemble()
UCHITAKE 0:58910ef3f2b0 25 {
takeuchi 1:0229fc98a26f 26 //Find header
takeuchi 1:0229fc98a26f 27 headerCheck = false;
takeuchi 1:0229fc98a26f 28 headerPoint = 0xff;
takeuchi 1:0229fc98a26f 29
takeuchi 1:0229fc98a26f 30 for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) {
takeuchi 1:0229fc98a26f 31 if(buffer[i] == HEADER_FIRST_BYTE) {
takeuchi 1:0229fc98a26f 32 takeCounter = i;
takeuchi 1:0229fc98a26f 33 ++takeCounter;
takeuchi 1:0229fc98a26f 34 if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) {
takeuchi 1:0229fc98a26f 35 headerCheck = true;
takeuchi 1:0229fc98a26f 36 headerPoint = i;
takeuchi 1:0229fc98a26f 37 }
takeuchi 1:0229fc98a26f 38 }
takeuchi 1:0229fc98a26f 39 }
takeuchi 1:0229fc98a26f 40 if(headerPoint == 0xff) {
takeuchi 1:0229fc98a26f 41 return;
takeuchi 1:0229fc98a26f 42 }
takeuchi 1:0229fc98a26f 43
takeuchi 1:0229fc98a26f 44 //assemble
takeuchi 4:fc4c88fffef8 45 checksum = 0;
takeuchi 1:0229fc98a26f 46 takeCounter = headerPoint; //firstheader
takeuchi 1:0229fc98a26f 47 ++takeCounter; //secondheader
takeuchi 2:edd33d3ad0fd 48
takeuchi 2:edd33d3ad0fd 49 ++takeCounter;
takeuchi 4:fc4c88fffef8 50 data[0] = buffer[(int)takeCounter];
takeuchi 4:fc4c88fffef8 51 ++takeCounter;
takeuchi 4:fc4c88fffef8 52 data[1] = buffer[(int)takeCounter];
takeuchi 4:fc4c88fffef8 53 X = ((data[0]<<8)|data[1]) - 32768;
takeuchi 4:fc4c88fffef8 54 checksum = (data[0] ^ data[1]);
takeuchi 4:fc4c88fffef8 55
takeuchi 4:fc4c88fffef8 56 ++takeCounter;
takeuchi 4:fc4c88fffef8 57 data[0] = buffer[(int)takeCounter];
takeuchi 4:fc4c88fffef8 58 ++takeCounter;
takeuchi 4:fc4c88fffef8 59 data[1] = buffer[(int)takeCounter];
takeuchi 4:fc4c88fffef8 60 Y = ((data[0]<<8)|data[1]) - 32768;
takeuchi 4:fc4c88fffef8 61 checksum = (checksum ^ data[0]);
takeuchi 4:fc4c88fffef8 62 checksum = (checksum ^ data[1]);
takeuchi 4:fc4c88fffef8 63
takeuchi 2:edd33d3ad0fd 64 ++takeCounter;
takeuchi 2:edd33d3ad0fd 65 data[0] = buffer[(int)takeCounter];
takeuchi 2:edd33d3ad0fd 66 ++takeCounter;
takeuchi 2:edd33d3ad0fd 67 data[1] = buffer[(int)takeCounter];
takeuchi 4:fc4c88fffef8 68 thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00);
takeuchi 4:fc4c88fffef8 69 theta = thetaint / 100.0;
takeuchi 4:fc4c88fffef8 70 if(theta > 180.0) {
takeuchi 4:fc4c88fffef8 71 theta -= 655.0;
takeuchi 4:fc4c88fffef8 72 }
takeuchi 4:fc4c88fffef8 73 theta *= -M_PI / 180.0;
takeuchi 4:fc4c88fffef8 74 if(theta > M_PI) theta -= 2 * M_PI;
takeuchi 4:fc4c88fffef8 75 if(theta < -M_PI) theta += 2 * M_PI;
takeuchi 4:fc4c88fffef8 76 if(theta > M_PI) theta -= 2 * M_PI;
takeuchi 4:fc4c88fffef8 77 if(theta < -M_PI) theta += 2 * M_PI;
takeuchi 4:fc4c88fffef8 78 checksum = (checksum ^ data[0]);
takeuchi 4:fc4c88fffef8 79 checksum = (checksum ^ data[1]);
takeuchi 4:fc4c88fffef8 80
takeuchi 4:fc4c88fffef8 81 ++takeCounter;
takeuchi 4:fc4c88fffef8 82 if(buffer[(int)takeCounter] != checksum) {
takeuchi 4:fc4c88fffef8 83 X = bfrX;
takeuchi 4:fc4c88fffef8 84 Y = bfrY;
takeuchi 4:fc4c88fffef8 85 theta = bfrTheta;
takeuchi 4:fc4c88fffef8 86 } else {
takeuchi 4:fc4c88fffef8 87 bfrX = X;
takeuchi 4:fc4c88fffef8 88 bfrY = Y;
takeuchi 4:fc4c88fffef8 89 bfrTheta = theta;
takeuchi 4:fc4c88fffef8 90 }
tanabe2000 3:47676abdf529 91
takeuchi 1:0229fc98a26f 92 }
takeuchi 2:edd33d3ad0fd 93
takeuchi 2:edd33d3ad0fd 94 void OmniPosition::send()
takeuchi 2:edd33d3ad0fd 95 {
takeuchi 2:edd33d3ad0fd 96 if(resetSend) {
takeuchi 2:edd33d3ad0fd 97 putc('R');
takeuchi 2:edd33d3ad0fd 98 resetSend = false;
takeuchi 2:edd33d3ad0fd 99 } else {
takeuchi 2:edd33d3ad0fd 100 //putc(0);
takeuchi 2:edd33d3ad0fd 101 }
takeuchi 2:edd33d3ad0fd 102 }
takeuchi 2:edd33d3ad0fd 103
takeuchi 4:fc4c88fffef8 104 int OmniPosition::getX()
takeuchi 2:edd33d3ad0fd 105 {
takeuchi 4:fc4c88fffef8 106 return X;
takeuchi 2:edd33d3ad0fd 107 }
takeuchi 2:edd33d3ad0fd 108
takeuchi 4:fc4c88fffef8 109 int OmniPosition::getY()
takeuchi 4:fc4c88fffef8 110 {
takeuchi 4:fc4c88fffef8 111 return Y;
takeuchi 4:fc4c88fffef8 112 }
takeuchi 4:fc4c88fffef8 113
takeuchi 4:fc4c88fffef8 114 double OmniPosition::getTheta()
takeuchi 4:fc4c88fffef8 115 {
takeuchi 4:fc4c88fffef8 116 return -theta;
takeuchi 4:fc4c88fffef8 117 }
takeuchi 2:edd33d3ad0fd 118
takeuchi 2:edd33d3ad0fd 119 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 120 {
takeuchi 2:edd33d3ad0fd 121 resetSend = true;
takeuchi 2:edd33d3ad0fd 122 }
takeuchi 4:fc4c88fffef8 123