NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@9:222f7fcbd05a, 2018-09-18 (annotated)

Committer:
tanabe2000
Date:
Tue Sep 18 15:25:05 2018 +0000
Revision:
9:222f7fcbd05a
Parent:
7:73e542a88106
Child:
10:a21aa2bd05c5
Child:
12:edd4217ad7a5
???????????????

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) :
tanabe2000 9:222f7fcbd05a 4 serial(serialTX, serialRX)
takeuchi 1:0229fc98a26f 5 {
tanabe2000 9:222f7fcbd05a 6 thread.start(callback(this, &OmniPosition::assembleLoop));
tanabe2000 9:222f7fcbd05a 7 serial.baud(115200);
tanabe2000 9:222f7fcbd05a 8 serial.setHeaders(FIRST_HEDDER, SECOND_HEDDER);
tanabe2000 9:222f7fcbd05a 9 serial.startReceive(BUFFER_SIZE);
takeuchi 6:f8dbbe93bc7b 10 }
takeuchi 1:0229fc98a26f 11
takeuchi 6:f8dbbe93bc7b 12 void OmniPosition::assembleLoop()
takeuchi 6:f8dbbe93bc7b 13 {
takeuchi 6:f8dbbe93bc7b 14 while(true) {
tanabe2000 9:222f7fcbd05a 15 serial.getData(rxdata);
tanabe2000 9:222f7fcbd05a 16 for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768;
tanabe2000 9:222f7fcbd05a 17 data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00);
tanabe2000 9:222f7fcbd05a 18 X = data[0];
tanabe2000 9:222f7fcbd05a 19 Y = data[1];
tanabe2000 9:222f7fcbd05a 20 theta = data[2];
takeuchi 1:0229fc98a26f 21 }
takeuchi 5:f8c3aeb4e65f 22 }
takeuchi 5:f8c3aeb4e65f 23
takeuchi 6:f8dbbe93bc7b 24 int16_t OmniPosition::getX()
takeuchi 2:edd33d3ad0fd 25 {
takeuchi 4:fc4c88fffef8 26 return X;
takeuchi 2:edd33d3ad0fd 27 }
takeuchi 2:edd33d3ad0fd 28
takeuchi 6:f8dbbe93bc7b 29 int16_t OmniPosition::getY()
takeuchi 4:fc4c88fffef8 30 {
takeuchi 4:fc4c88fffef8 31 return Y;
takeuchi 4:fc4c88fffef8 32 }
takeuchi 4:fc4c88fffef8 33
takeuchi 6:f8dbbe93bc7b 34 float OmniPosition::getTheta()
takeuchi 4:fc4c88fffef8 35 {
takeuchi 6:f8dbbe93bc7b 36 return (float)(theta / 100.0);
takeuchi 4:fc4c88fffef8 37 }
takeuchi 2:edd33d3ad0fd 38
takeuchi 2:edd33d3ad0fd 39 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 40 {
takeuchi 2:edd33d3ad0fd 41 resetSend = true;
takeuchi 7:73e542a88106 42 if(resetSend) {
tanabe2000 9:222f7fcbd05a 43 // putc('R');
takeuchi 7:73e542a88106 44 resetSend = false;
takeuchi 7:73e542a88106 45 } else {
takeuchi 7:73e542a88106 46 //putc(0);
takeuchi 7:73e542a88106 47 }
takeuchi 2:edd33d3ad0fd 48 }
takeuchi 4:fc4c88fffef8 49