Jan Meindl
odometry
Odometry.cpp@1:1765665581cc, 2015-09-10 (annotated)
- Committer:
- zatakon
- Date:
- Thu Sep 10 07:37:48 2015 +0000
- Revision:
- 1:1765665581cc
- Parent:
- 0:402f8c75d394
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| zatakon | 0:402f8c75d394 | 1 | #include "Odometry.h" |
| zatakon | 0:402f8c75d394 | 2 | |
| zatakon | 1:1765665581cc | 3 | Robot::Robot(float width, float countPerMeter) |
| zatakon | 0:402f8c75d394 | 4 | { |
| zatakon | 0:402f8c75d394 | 5 | _w = width; |
| zatakon | 0:402f8c75d394 | 6 | _countPerMeter = countPerMeter; |
| zatakon | 0:402f8c75d394 | 7 | |
| zatakon | 0:402f8c75d394 | 8 | _odom.resize(3); |
| zatakon | 0:402f8c75d394 | 9 | _ratio.resize(2); |
| zatakon | 0:402f8c75d394 | 10 | } |
| zatakon | 0:402f8c75d394 | 11 | |
| zatakon | 1:1765665581cc | 12 | float Robot::getRadius(float v1, float v2) |
| zatakon | 1:1765665581cc | 13 | { |
| zatakon | 1:1765665581cc | 14 | if( abs(v2-v1) < 0.0001 ) |
| zatakon | 1:1765665581cc | 15 | _R = 99.9; |
| zatakon | 1:1765665581cc | 16 | else |
| zatakon | 1:1765665581cc | 17 | _R = (_w/2) - ( (_w * v2) / (v2 - v1)); |
| zatakon | 0:402f8c75d394 | 18 | return _R; |
| zatakon | 0:402f8c75d394 | 19 | } |
| zatakon | 0:402f8c75d394 | 20 | |
| zatakon | 1:1765665581cc | 21 | void Robot::countRadius(float v1, float v2) |
| zatakon | 1:1765665581cc | 22 | { |
| zatakon | 1:1765665581cc | 23 | if( abs(v2-v1) < 0.0001 ) |
| zatakon | 1:1765665581cc | 24 | _R = 99.9; |
| zatakon | 1:1765665581cc | 25 | else |
| zatakon | 1:1765665581cc | 26 | _R = (_w/2) - ( (_w * v2) / (v2 - v1)); |
| zatakon | 0:402f8c75d394 | 27 | } |
| zatakon | 0:402f8c75d394 | 28 | |
| zatakon | 0:402f8c75d394 | 29 | std::vector<float> Robot::getSpeed(std::vector<int> encoders) |
| zatakon | 0:402f8c75d394 | 30 | { |
| zatakon | 1:1765665581cc | 31 | float v1, v2, fi; |
| zatakon | 0:402f8c75d394 | 32 | |
| zatakon | 1:1765665581cc | 33 | v1 = ((float)encoders[0])/(_countPerMeter); // left wheel |
| zatakon | 1:1765665581cc | 34 | v2 = ((float)encoders[1])/(_countPerMeter); // right wheel |
| zatakon | 0:402f8c75d394 | 35 | |
| zatakon | 1:1765665581cc | 36 | if( abs(v2-v1) < 0.0001 ) { |
| zatakon | 1:1765665581cc | 37 | _R = 99.9; |
| zatakon | 0:402f8c75d394 | 38 | } |
| zatakon | 1:1765665581cc | 39 | else { |
| zatakon | 1:1765665581cc | 40 | _R = (_w/2) - ( (_w * v2) / (v2 - v1)); |
| zatakon | 0:402f8c75d394 | 41 | } |
| zatakon | 1:1765665581cc | 42 | |
| zatakon | 1:1765665581cc | 43 | fi = atan((v1-v2)/_w); |
| zatakon | 0:402f8c75d394 | 44 | |
| zatakon | 1:1765665581cc | 45 | _odom[0] = _R * sin(fi); |
| zatakon | 1:1765665581cc | 46 | _odom[1] = -tan(fi) * _odom[0]; |
| zatakon | 1:1765665581cc | 47 | _odom[2] = -fi; |
| zatakon | 1:1765665581cc | 48 | |
| zatakon | 1:1765665581cc | 49 | return _odom; |
| zatakon | 0:402f8c75d394 | 50 | } |
| zatakon | 0:402f8c75d394 | 51 | |
| zatakon | 0:402f8c75d394 | 52 | |
| zatakon | 1:1765665581cc | 53 | std::vector<float> Robot::getMotorRatio(float x, float z) |
| zatakon | 0:402f8c75d394 | 54 | { |
| zatakon | 1:1765665581cc | 55 | float fi = ( abs(z) < 0.0001) ? 0.0001 : z; |
| zatakon | 1:1765665581cc | 56 | |
| zatakon | 1:1765665581cc | 57 | float R = x / sin(fi); |
| zatakon | 1:1765665581cc | 58 | |
| zatakon | 1:1765665581cc | 59 | _ratio[0] = tan(fi) * ( (_w/2) + R ); |
| zatakon | 1:1765665581cc | 60 | _ratio[1] = -tan(fi) * ( (_w/2) - R ); |
| zatakon | 1:1765665581cc | 61 | |
| zatakon | 0:402f8c75d394 | 62 | return _ratio; |
| zatakon | 0:402f8c75d394 | 63 | } |
| zatakon | 0:402f8c75d394 | 64 | |
| zatakon | 0:402f8c75d394 | 65 | |
| zatakon | 1:1765665581cc | 66 |