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