library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
OmniPosition.cpp@2:edd33d3ad0fd, 2018-07-01 (annotated)
- Committer:
- takeuchi
- Date:
- Sun Jul 01 16:39:13 2018 +0900
- Revision:
- 2:edd33d3ad0fd
- Parent:
- 1:0229fc98a26f
- Child:
- 3:47676abdf529
working
Who changed what in which revision?
User | Revision | Line number | New 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 |
1:0229fc98a26f | 4 | RawSerial(serialTX, serialRX, DEFAULT_BAUD), |
takeuchi |
1:0229fc98a26f | 5 | readCounter(SERIAL_BUFFER_SIZE, 0), |
takeuchi |
2:edd33d3ad0fd | 6 | takeCounter(SERIAL_BUFFER_SIZE, 0), |
takeuchi |
2:edd33d3ad0fd | 7 | X(0), |
takeuchi |
2:edd33d3ad0fd | 8 | Y(0) |
takeuchi |
1:0229fc98a26f | 9 | { |
takeuchi |
1:0229fc98a26f | 10 | buffer = new char[SERIAL_BUFFER_SIZE]; |
takeuchi |
2:edd33d3ad0fd | 11 | data = new char[2]; |
takeuchi |
1:0229fc98a26f | 12 | attach(callback(this, &OmniPosition::readData)); |
takeuchi |
2:edd33d3ad0fd | 13 | assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); |
takeuchi |
2:edd33d3ad0fd | 14 | sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ); |
takeuchi |
1:0229fc98a26f | 15 | } |
takeuchi |
1:0229fc98a26f | 16 | |
takeuchi |
1:0229fc98a26f | 17 | void OmniPosition::readData() |
takeuchi |
1:0229fc98a26f | 18 | { |
takeuchi |
1:0229fc98a26f | 19 | buffer[(int)readCounter] = getc(); |
takeuchi |
1:0229fc98a26f | 20 | ++readCounter; |
takeuchi |
1:0229fc98a26f | 21 | } |
takeuchi |
1:0229fc98a26f | 22 | |
takeuchi |
1:0229fc98a26f | 23 | void OmniPosition::assemble() |
UCHITAKE | 0:58910ef3f2b0 | 24 | { |
takeuchi |
1:0229fc98a26f | 25 | //Find header |
takeuchi |
1:0229fc98a26f | 26 | headerCheck = false; |
takeuchi |
1:0229fc98a26f | 27 | headerPoint = 0xff; |
takeuchi |
1:0229fc98a26f | 28 | |
takeuchi |
1:0229fc98a26f | 29 | for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) { |
takeuchi |
1:0229fc98a26f | 30 | if(buffer[i] == HEADER_FIRST_BYTE) { |
takeuchi |
1:0229fc98a26f | 31 | takeCounter = i; |
takeuchi |
1:0229fc98a26f | 32 | ++takeCounter; |
takeuchi |
1:0229fc98a26f | 33 | if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) { |
takeuchi |
1:0229fc98a26f | 34 | headerCheck = true; |
takeuchi |
1:0229fc98a26f | 35 | headerPoint = i; |
takeuchi |
1:0229fc98a26f | 36 | } |
takeuchi |
1:0229fc98a26f | 37 | } |
takeuchi |
1:0229fc98a26f | 38 | } |
takeuchi |
1:0229fc98a26f | 39 | if(headerPoint == 0xff) { |
takeuchi |
1:0229fc98a26f | 40 | return; |
takeuchi |
1:0229fc98a26f | 41 | } |
takeuchi |
1:0229fc98a26f | 42 | |
takeuchi |
1:0229fc98a26f | 43 | //assemble |
takeuchi |
1:0229fc98a26f | 44 | takeCounter = headerPoint; //firstheader |
takeuchi |
1:0229fc98a26f | 45 | ++takeCounter; //secondheader |
takeuchi |
2:edd33d3ad0fd | 46 | |
takeuchi |
2:edd33d3ad0fd | 47 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 48 | data[0] = buffer[(int)takeCounter]; |
takeuchi |
2:edd33d3ad0fd | 49 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 50 | data[1] = buffer[(int)takeCounter]; |
takeuchi |
2:edd33d3ad0fd | 51 | X = ((data[0]<<8)|data[1]) - 32768; |
takeuchi |
2:edd33d3ad0fd | 52 | |
takeuchi |
2:edd33d3ad0fd | 53 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 54 | data[0] = buffer[(int)takeCounter]; |
takeuchi |
2:edd33d3ad0fd | 55 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 56 | data[1] = buffer[(int)takeCounter]; |
takeuchi |
2:edd33d3ad0fd | 57 | Y = ((data[0]<<8)|data[1]) - 32768; |
takeuchi |
1:0229fc98a26f | 58 | } |
takeuchi |
2:edd33d3ad0fd | 59 | |
takeuchi |
2:edd33d3ad0fd | 60 | void OmniPosition::send() |
takeuchi |
2:edd33d3ad0fd | 61 | { |
takeuchi |
2:edd33d3ad0fd | 62 | if(resetSend) { |
takeuchi |
2:edd33d3ad0fd | 63 | putc('R'); |
takeuchi |
2:edd33d3ad0fd | 64 | resetSend = false; |
takeuchi |
2:edd33d3ad0fd | 65 | } else { |
takeuchi |
2:edd33d3ad0fd | 66 | //putc(0); |
takeuchi |
2:edd33d3ad0fd | 67 | } |
takeuchi |
2:edd33d3ad0fd | 68 | } |
takeuchi |
2:edd33d3ad0fd | 69 | |
takeuchi |
2:edd33d3ad0fd | 70 | int OmniPosition::getX() |
takeuchi |
2:edd33d3ad0fd | 71 | { |
takeuchi |
2:edd33d3ad0fd | 72 | return X; |
takeuchi |
2:edd33d3ad0fd | 73 | } |
takeuchi |
2:edd33d3ad0fd | 74 | |
takeuchi |
2:edd33d3ad0fd | 75 | int OmniPosition::getY() |
takeuchi |
2:edd33d3ad0fd | 76 | { |
takeuchi |
2:edd33d3ad0fd | 77 | return Y; |
takeuchi |
2:edd33d3ad0fd | 78 | } |
takeuchi |
2:edd33d3ad0fd | 79 | |
takeuchi |
2:edd33d3ad0fd | 80 | void OmniPosition::reset() |
takeuchi |
2:edd33d3ad0fd | 81 | { |
takeuchi |
2:edd33d3ad0fd | 82 | resetSend = true; |
takeuchi |
2:edd33d3ad0fd | 83 | } |