2019
Dependents: wheel_test6 wheel_test7 2019NHK_A_manual_red 2019NHK_A_manual_red
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?
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) : |
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 |