library for omnidirectional planar positioning system
Dependents: measuring_wheel 2018NHK_gakugaku_robo 2018NHK_gaku_ver2
OmniPosition.cpp@4:fc4c88fffef8, 2018-08-21 (annotated)
- Committer:
- takeuchi
- Date:
- Tue Aug 21 13:41:25 2018 +0900
- Revision:
- 4:fc4c88fffef8
- Parent:
- 3:47676abdf529
- Child:
- 5:f8c3aeb4e65f
fix func
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 |
4:fc4c88fffef8 | 7 | X(0), |
takeuchi |
4:fc4c88fffef8 | 8 | Y(0), |
takeuchi |
4:fc4c88fffef8 | 9 | theta(0.0) |
takeuchi |
1:0229fc98a26f | 10 | { |
takeuchi |
4:fc4c88fffef8 | 11 | buffer = new char[SERIAL_BUFFER_SIZE]; |
takeuchi |
4:fc4c88fffef8 | 12 | data = new char[2]; |
takeuchi |
1:0229fc98a26f | 13 | attach(callback(this, &OmniPosition::readData)); |
takeuchi |
2:edd33d3ad0fd | 14 | assembleTicker.attach(callback(this, &OmniPosition::assemble), RECEIVE_FREQ); |
takeuchi |
2:edd33d3ad0fd | 15 | sendTicker.attach(callback(this, &OmniPosition::send), SEND_FREQ); |
takeuchi |
1:0229fc98a26f | 16 | } |
takeuchi |
1:0229fc98a26f | 17 | |
takeuchi |
1:0229fc98a26f | 18 | void OmniPosition::readData() |
takeuchi |
1:0229fc98a26f | 19 | { |
takeuchi |
1:0229fc98a26f | 20 | buffer[(int)readCounter] = getc(); |
takeuchi |
1:0229fc98a26f | 21 | ++readCounter; |
takeuchi |
1:0229fc98a26f | 22 | } |
takeuchi |
1:0229fc98a26f | 23 | |
takeuchi |
1:0229fc98a26f | 24 | void OmniPosition::assemble() |
UCHITAKE | 0:58910ef3f2b0 | 25 | { |
takeuchi |
1:0229fc98a26f | 26 | //Find header |
takeuchi |
1:0229fc98a26f | 27 | headerCheck = false; |
takeuchi |
1:0229fc98a26f | 28 | headerPoint = 0xff; |
takeuchi |
1:0229fc98a26f | 29 | |
takeuchi |
1:0229fc98a26f | 30 | for(int i = 0; i < SERIAL_BUFFER_SIZE; i++) { |
takeuchi |
1:0229fc98a26f | 31 | if(buffer[i] == HEADER_FIRST_BYTE) { |
takeuchi |
1:0229fc98a26f | 32 | takeCounter = i; |
takeuchi |
1:0229fc98a26f | 33 | ++takeCounter; |
takeuchi |
1:0229fc98a26f | 34 | if(buffer[(int)takeCounter] == HEADER_SECOND_BYTE) { |
takeuchi |
1:0229fc98a26f | 35 | headerCheck = true; |
takeuchi |
1:0229fc98a26f | 36 | headerPoint = i; |
takeuchi |
1:0229fc98a26f | 37 | } |
takeuchi |
1:0229fc98a26f | 38 | } |
takeuchi |
1:0229fc98a26f | 39 | } |
takeuchi |
1:0229fc98a26f | 40 | if(headerPoint == 0xff) { |
takeuchi |
1:0229fc98a26f | 41 | return; |
takeuchi |
1:0229fc98a26f | 42 | } |
takeuchi |
1:0229fc98a26f | 43 | |
takeuchi |
1:0229fc98a26f | 44 | //assemble |
takeuchi |
4:fc4c88fffef8 | 45 | checksum = 0; |
takeuchi |
1:0229fc98a26f | 46 | takeCounter = headerPoint; //firstheader |
takeuchi |
1:0229fc98a26f | 47 | ++takeCounter; //secondheader |
takeuchi |
2:edd33d3ad0fd | 48 | |
takeuchi |
2:edd33d3ad0fd | 49 | ++takeCounter; |
takeuchi |
4:fc4c88fffef8 | 50 | data[0] = buffer[(int)takeCounter]; |
takeuchi |
4:fc4c88fffef8 | 51 | ++takeCounter; |
takeuchi |
4:fc4c88fffef8 | 52 | data[1] = buffer[(int)takeCounter]; |
takeuchi |
4:fc4c88fffef8 | 53 | X = ((data[0]<<8)|data[1]) - 32768; |
takeuchi |
4:fc4c88fffef8 | 54 | checksum = (data[0] ^ data[1]); |
takeuchi |
4:fc4c88fffef8 | 55 | |
takeuchi |
4:fc4c88fffef8 | 56 | ++takeCounter; |
takeuchi |
4:fc4c88fffef8 | 57 | data[0] = buffer[(int)takeCounter]; |
takeuchi |
4:fc4c88fffef8 | 58 | ++takeCounter; |
takeuchi |
4:fc4c88fffef8 | 59 | data[1] = buffer[(int)takeCounter]; |
takeuchi |
4:fc4c88fffef8 | 60 | Y = ((data[0]<<8)|data[1]) - 32768; |
takeuchi |
4:fc4c88fffef8 | 61 | checksum = (checksum ^ data[0]); |
takeuchi |
4:fc4c88fffef8 | 62 | checksum = (checksum ^ data[1]); |
takeuchi |
4:fc4c88fffef8 | 63 | |
takeuchi |
2:edd33d3ad0fd | 64 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 65 | data[0] = buffer[(int)takeCounter]; |
takeuchi |
2:edd33d3ad0fd | 66 | ++takeCounter; |
takeuchi |
2:edd33d3ad0fd | 67 | data[1] = buffer[(int)takeCounter]; |
takeuchi |
4:fc4c88fffef8 | 68 | thetaint = (data[0] & 0xFF) | ((data[1] << 8) & 0xFF00); |
takeuchi |
4:fc4c88fffef8 | 69 | theta = thetaint / 100.0; |
takeuchi |
4:fc4c88fffef8 | 70 | if(theta > 180.0) { |
takeuchi |
4:fc4c88fffef8 | 71 | theta -= 655.0; |
takeuchi |
4:fc4c88fffef8 | 72 | } |
takeuchi |
4:fc4c88fffef8 | 73 | theta *= -M_PI / 180.0; |
takeuchi |
4:fc4c88fffef8 | 74 | if(theta > M_PI) theta -= 2 * M_PI; |
takeuchi |
4:fc4c88fffef8 | 75 | if(theta < -M_PI) theta += 2 * M_PI; |
takeuchi |
4:fc4c88fffef8 | 76 | if(theta > M_PI) theta -= 2 * M_PI; |
takeuchi |
4:fc4c88fffef8 | 77 | if(theta < -M_PI) theta += 2 * M_PI; |
takeuchi |
4:fc4c88fffef8 | 78 | checksum = (checksum ^ data[0]); |
takeuchi |
4:fc4c88fffef8 | 79 | checksum = (checksum ^ data[1]); |
takeuchi |
4:fc4c88fffef8 | 80 | |
takeuchi |
4:fc4c88fffef8 | 81 | ++takeCounter; |
takeuchi |
4:fc4c88fffef8 | 82 | if(buffer[(int)takeCounter] != checksum) { |
takeuchi |
4:fc4c88fffef8 | 83 | X = bfrX; |
takeuchi |
4:fc4c88fffef8 | 84 | Y = bfrY; |
takeuchi |
4:fc4c88fffef8 | 85 | theta = bfrTheta; |
takeuchi |
4:fc4c88fffef8 | 86 | } else { |
takeuchi |
4:fc4c88fffef8 | 87 | bfrX = X; |
takeuchi |
4:fc4c88fffef8 | 88 | bfrY = Y; |
takeuchi |
4:fc4c88fffef8 | 89 | bfrTheta = theta; |
takeuchi |
4:fc4c88fffef8 | 90 | } |
tanabe2000 | 3:47676abdf529 | 91 | |
takeuchi |
1:0229fc98a26f | 92 | } |
takeuchi |
2:edd33d3ad0fd | 93 | |
takeuchi |
2:edd33d3ad0fd | 94 | void OmniPosition::send() |
takeuchi |
2:edd33d3ad0fd | 95 | { |
takeuchi |
2:edd33d3ad0fd | 96 | if(resetSend) { |
takeuchi |
2:edd33d3ad0fd | 97 | putc('R'); |
takeuchi |
2:edd33d3ad0fd | 98 | resetSend = false; |
takeuchi |
2:edd33d3ad0fd | 99 | } else { |
takeuchi |
2:edd33d3ad0fd | 100 | //putc(0); |
takeuchi |
2:edd33d3ad0fd | 101 | } |
takeuchi |
2:edd33d3ad0fd | 102 | } |
takeuchi |
2:edd33d3ad0fd | 103 | |
takeuchi |
4:fc4c88fffef8 | 104 | int OmniPosition::getX() |
takeuchi |
2:edd33d3ad0fd | 105 | { |
takeuchi |
4:fc4c88fffef8 | 106 | return X; |
takeuchi |
2:edd33d3ad0fd | 107 | } |
takeuchi |
2:edd33d3ad0fd | 108 | |
takeuchi |
4:fc4c88fffef8 | 109 | int OmniPosition::getY() |
takeuchi |
4:fc4c88fffef8 | 110 | { |
takeuchi |
4:fc4c88fffef8 | 111 | return Y; |
takeuchi |
4:fc4c88fffef8 | 112 | } |
takeuchi |
4:fc4c88fffef8 | 113 | |
takeuchi |
4:fc4c88fffef8 | 114 | double OmniPosition::getTheta() |
takeuchi |
4:fc4c88fffef8 | 115 | { |
takeuchi |
4:fc4c88fffef8 | 116 | return -theta; |
takeuchi |
4:fc4c88fffef8 | 117 | } |
takeuchi |
2:edd33d3ad0fd | 118 | |
takeuchi |
2:edd33d3ad0fd | 119 | void OmniPosition::reset() |
takeuchi |
2:edd33d3ad0fd | 120 | { |
takeuchi |
2:edd33d3ad0fd | 121 | resetSend = true; |
takeuchi |
2:edd33d3ad0fd | 122 | } |
takeuchi |
4:fc4c88fffef8 | 123 |