NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@6:f8dbbe93bc7b, 2018-08-24 (annotated)

Committer:
takeuchi
Date:
Fri Aug 24 14:40:25 2018 +0900
Revision:
6:f8dbbe93bc7b
Parent:
5:f8c3aeb4e65f
Child:
7:73e542a88106
fix buffering

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 6:f8dbbe93bc7b 4 RawSerial(serialTX, serialRX, OP_DEFAULT_BAUD)
takeuchi 1:0229fc98a26f 5 {
takeuchi 6:f8dbbe93bc7b 6 attach(callback(this, &OmniPosition::receiveByte));
takeuchi 6:f8dbbe93bc7b 7 thread.start(callback(this, &OmniPosition::assembleLoop));
takeuchi 5:f8c3aeb4e65f 8 sendTicker.attach(callback(this, &OmniPosition::send), OP_SEND_FREQ);
takeuchi 1:0229fc98a26f 9 }
takeuchi 1:0229fc98a26f 10
takeuchi 6:f8dbbe93bc7b 11 void OmniPosition::receiveByte()
takeuchi 1:0229fc98a26f 12 {
takeuchi 6:f8dbbe93bc7b 13 buf.push_back(getc());
takeuchi 1:0229fc98a26f 14 }
takeuchi 1:0229fc98a26f 15
takeuchi 1:0229fc98a26f 16 void OmniPosition::assemble()
UCHITAKE 0:58910ef3f2b0 17 {
takeuchi 6:f8dbbe93bc7b 18 X = ((buf[2]<<8)|buf[3]) - 32768;
takeuchi 6:f8dbbe93bc7b 19 Y = ((buf[4]<<8)|buf[5]) - 32768;
takeuchi 6:f8dbbe93bc7b 20 theta = (buf[6] & 0xFF) | ((buf[7] << 8) & 0xFF00);
takeuchi 6:f8dbbe93bc7b 21 }
takeuchi 1:0229fc98a26f 22
takeuchi 6:f8dbbe93bc7b 23 void OmniPosition::assembleLoop()
takeuchi 6:f8dbbe93bc7b 24 {
takeuchi 6:f8dbbe93bc7b 25 while(true) {
takeuchi 6:f8dbbe93bc7b 26 if(buf.size() > OP_SERIAL_BUFFER_SIZE) {
takeuchi 6:f8dbbe93bc7b 27 if(buf[0] == OP_HEADER_FIRST_BYTE && buf[1] == OP_HEADER_SECOND_BYTE) {
takeuchi 6:f8dbbe93bc7b 28 assemble();
takeuchi 6:f8dbbe93bc7b 29 buf.erase(buf.begin(), buf.begin() + (OP_SERIAL_BUFFER_SIZE - 1));
takeuchi 6:f8dbbe93bc7b 30 } else {
takeuchi 6:f8dbbe93bc7b 31 buf.erase(buf.begin());
takeuchi 1:0229fc98a26f 32 }
takeuchi 1:0229fc98a26f 33 }
takeuchi 1:0229fc98a26f 34 }
takeuchi 5:f8c3aeb4e65f 35 }
takeuchi 5:f8c3aeb4e65f 36
takeuchi 2:edd33d3ad0fd 37 void OmniPosition::send()
takeuchi 2:edd33d3ad0fd 38 {
takeuchi 2:edd33d3ad0fd 39 if(resetSend) {
takeuchi 2:edd33d3ad0fd 40 putc('R');
takeuchi 2:edd33d3ad0fd 41 resetSend = false;
takeuchi 2:edd33d3ad0fd 42 } else {
takeuchi 2:edd33d3ad0fd 43 //putc(0);
takeuchi 2:edd33d3ad0fd 44 }
takeuchi 2:edd33d3ad0fd 45 }
takeuchi 2:edd33d3ad0fd 46
takeuchi 6:f8dbbe93bc7b 47 int16_t OmniPosition::getX()
takeuchi 2:edd33d3ad0fd 48 {
takeuchi 4:fc4c88fffef8 49 return X;
takeuchi 2:edd33d3ad0fd 50 }
takeuchi 2:edd33d3ad0fd 51
takeuchi 6:f8dbbe93bc7b 52 int16_t OmniPosition::getY()
takeuchi 4:fc4c88fffef8 53 {
takeuchi 4:fc4c88fffef8 54 return Y;
takeuchi 4:fc4c88fffef8 55 }
takeuchi 4:fc4c88fffef8 56
takeuchi 6:f8dbbe93bc7b 57 float OmniPosition::getTheta()
takeuchi 4:fc4c88fffef8 58 {
takeuchi 6:f8dbbe93bc7b 59 return (float)(theta / 100.0);
takeuchi 4:fc4c88fffef8 60 }
takeuchi 2:edd33d3ad0fd 61
takeuchi 2:edd33d3ad0fd 62 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 63 {
takeuchi 2:edd33d3ad0fd 64 resetSend = true;
takeuchi 2:edd33d3ad0fd 65 }
takeuchi 4:fc4c88fffef8 66