NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@8:930369f241fb, 2018-08-24 (annotated)

Committer:
takeuchi
Date:
Fri Aug 24 15:03:39 2018 +0900
Revision:
8:930369f241fb
Parent:
7:73e542a88106 
add checksum

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