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