NagaokaRoboticsClub_mbedTeam / OmniPosition

library for omnidirectional planar positioning system

Dependents:   measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2

OmniPosition.cpp@12:edd4217ad7a5, 2018-10-12 (annotated)

Committer:
tanabe2000
Date:
Fri Oct 12 04:38:04 2018 +0000
Revision:
12:edd4217ad7a5
Parent:
9:222f7fcbd05a
Child:
13:913b647071a8
test

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 12:edd4217ad7a5 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 {
tanabe2000 12:edd4217ad7a5 26 serial.getData(rxdata);
tanabe2000 12:edd4217ad7a5 27 for(int i = 0; i < TWO_BYTE_DATA - 1; i++)data[i] = ((rxdata[2*i]<<8)|rxdata[2*i+1]) - 32768;
tanabe2000 12:edd4217ad7a5 28 data[2] = (rxdata[2*2] & 0xFF) | ((rxdata[2*2+1] << 8) & 0xFF00);
tanabe2000 12:edd4217ad7a5 29 X = data[0];
tanabe2000 12:edd4217ad7a5 30 Y = data[1];
tanabe2000 12:edd4217ad7a5 31 theta = data[2];
takeuchi 4:fc4c88fffef8 32 return X;
takeuchi 2:edd33d3ad0fd 33 }
takeuchi 2:edd33d3ad0fd 34
takeuchi 6:f8dbbe93bc7b 35 int16_t OmniPosition::getY()
takeuchi 4:fc4c88fffef8 36 {
takeuchi 4:fc4c88fffef8 37 return Y;
takeuchi 4:fc4c88fffef8 38 }
takeuchi 4:fc4c88fffef8 39
takeuchi 6:f8dbbe93bc7b 40 float OmniPosition::getTheta()
takeuchi 4:fc4c88fffef8 41 {
takeuchi 6:f8dbbe93bc7b 42 return (float)(theta / 100.0);
takeuchi 4:fc4c88fffef8 43 }
takeuchi 2:edd33d3ad0fd 44
takeuchi 2:edd33d3ad0fd 45 void OmniPosition::reset()
takeuchi 2:edd33d3ad0fd 46 {
takeuchi 2:edd33d3ad0fd 47 resetSend = true;
takeuchi 7:73e542a88106 48 if(resetSend) {
tanabe2000 9:222f7fcbd05a 49 // putc('R');
takeuchi 7:73e542a88106 50 resetSend = false;
takeuchi 7:73e542a88106 51 } else {
takeuchi 7:73e542a88106 52 //putc(0);
takeuchi 7:73e542a88106 53 }
takeuchi 2:edd33d3ad0fd 54 }
takeuchi 4:fc4c88fffef8 55