NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@5:f8c3aeb4e65f, 2018-08-22 (annotated)

Committer:
takeuchi
Date:
Wed Aug 22 20:21:48 2018 +0900
Revision:
5:f8c3aeb4e65f
Parent:
4:fc4c88fffef8
Child:
6:f8dbbe93bc7b
remove cycleint

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 5:f8c3aeb4e65f 4 RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD),
takeuchi 5:f8c3aeb4e65f 5 readCounter(0),
takeuchi 5:f8c3aeb4e65f 6 takeCounter(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 5:f8c3aeb4e65f 11 buffer = new char[OP_SERIAL_BUFFER_SIZE];
takeuchi 4:fc4c88fffef8 12 data = new char[2];
takeuchi 1:0229fc98a26f 13 attach(callback(this, &OmniPosition::readData));
takeuchi 5:f8c3aeb4e65f 14 assembleTicker.attach(callback(this, &OmniPosition::assemble), OP_RECEIVE_FREQ);
takeuchi 5:f8c3aeb4e65f 15 sendTicker.attach(callback(this, &OmniPosition::send), OP_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 5:f8c3aeb4e65f 21 readCounter = incrementCounter(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 5:f8c3aeb4e65f 30 for(int i = 0; i < OP_SERIAL_BUFFER_SIZE; i++) {
takeuchi 5:f8c3aeb4e65f 31 if(buffer[i] == OP_HEADER_FIRST_BYTE) {
takeuchi 1:0229fc98a26f 32 takeCounter = i;
takeuchi 5:f8c3aeb4e65f 33 takeCounter = incrementCounter(takeCounter);
takeuchi 5:f8c3aeb4e65f 34 if(buffer[(int)takeCounter] == OP_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 5:f8c3aeb4e65f 47 takeCounter = incrementCounter(takeCounter); //secondheader
takeuchi 2:edd33d3ad0fd 48
takeuchi 5:f8c3aeb4e65f 49 takeCounter = incrementCounter(takeCounter); //secondheader
takeuchi 4:fc4c88fffef8 50 data[0] = buffer[(int)takeCounter];
takeuchi 5:f8c3aeb4e65f 51 takeCounter = incrementCounter(takeCounter); //secondheader
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 5:f8c3aeb4e65f 56 takeCounter = incrementCounter(takeCounter); //secondheader
takeuchi 4:fc4c88fffef8 57 data[0] = buffer[(int)takeCounter];
takeuchi 5:f8c3aeb4e65f 58 takeCounter = incrementCounter(takeCounter); //secondheader
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 5:f8c3aeb4e65f 64 takeCounter = incrementCounter(takeCounter); //secondheader
takeuchi 2:edd33d3ad0fd 65 data[0] = buffer[(int)takeCounter];
takeuchi 5:f8c3aeb4e65f 66 takeCounter = incrementCounter(takeCounter); //secondheader
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 5:f8c3aeb4e65f 81 takeCounter = incrementCounter(takeCounter); //secondheader
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 5:f8c3aeb4e65f 94 int OmniPosition::incrementCounter(int counter)
takeuchi 5:f8c3aeb4e65f 95 {
takeuchi 5:f8c3aeb4e65f 96 ++counter;
takeuchi 5:f8c3aeb4e65f 97 if(counter >= OP_SERIAL_BUFFER_SIZE) {
takeuchi 5:f8c3aeb4e65f 98 counter -= OP_SERIAL_BUFFER_SIZE;
takeuchi 5:f8c3aeb4e65f 99 }
takeuchi 5:f8c3aeb4e65f 100 return counter;
takeuchi 5:f8c3aeb4e65f 101 }
takeuchi 5:f8c3aeb4e65f 102
takeuchi 2:edd33d3ad0fd 103 void OmniPosition::send()
takeuchi 2:edd33d3ad0fd 104 {
takeuchi 2:edd33d3ad0fd 105 if(resetSend) {
takeuchi 2:edd33d3ad0fd 106 putc('R');
takeuchi 2:edd33d3ad0fd 107 resetSend = false;
takeuchi 2:edd33d3ad0fd 108 } else {
takeuchi 2:edd33d3ad0fd 109 //putc(0);
takeuchi 2:edd33d3ad0fd 110 }
takeuchi 2:edd33d3ad0fd 111 }
takeuchi 2:edd33d3ad0fd 112
takeuchi 4:fc4c88fffef8 113 int OmniPosition::getX()
takeuchi 2:edd33d3ad0fd 114 {
takeuchi 4:fc4c88fffef8 115 return X;
takeuchi 2:edd33d3ad0fd 116 }
takeuchi 2:edd33d3ad0fd 117
takeuchi 4:fc4c88fffef8 118 int OmniPosition::getY()
takeuchi 4:fc4c88fffef8 119 {
takeuchi 4:fc4c88fffef8 120 return Y;
takeuchi 4:fc4c88fffef8 121 }
takeuchi 4:fc4c88fffef8 122
takeuchi 4:fc4c88fffef8 123 double OmniPosition::getTheta()
takeuchi 4:fc4c88fffef8 124 {
takeuchi 4:fc4c88fffef8 125 return -theta;
takeuchi 4:fc4c88fffef8 126 }
takeuchi 2:edd33d3ad0fd 127
takeuchi 2:edd33d3ad0fd 128 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 129 {
takeuchi 2:edd33d3ad0fd 130 resetSend = true;
takeuchi 2:edd33d3ad0fd 131 }
takeuchi 4:fc4c88fffef8 132