NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@7:73e542a88106, 2018-08-24 (annotated)

Committer:
takeuchi
Date:
Fri Aug 24 14:48:35 2018 +0900
Revision:
7:73e542a88106
Parent:
6:f8dbbe93bc7b
Child:
8:930369f241fb
Child:
9:222f7fcbd05a
delete ticker

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