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