Jan Meindl / Odometry

odometry

Odometry.cpp@0:402f8c75d394, 2015-08-11 (annotated)

Committer:
zatakon
Date:
Tue Aug 11 13:05:46 2015 +0000
Revision:
0:402f8c75d394
Child:
1:1765665581cc
??????????; ??????????; ??????????; ?????????

Who changed what in which revision?

UserRevisionLine numberNew contents of line
zatakon 0:402f8c75d394 1 #include "Odometry.h"
zatakon 0:402f8c75d394 2
zatakon 0:402f8c75d394 3 Robot::Robot(float wheelbase, float width, float countPerMeter)
zatakon 0:402f8c75d394 4 {
zatakon 0:402f8c75d394 5 _whbs = wheelbase;
zatakon 0:402f8c75d394 6 _w = width;
zatakon 0:402f8c75d394 7 _countPerMeter = countPerMeter;
zatakon 0:402f8c75d394 8
zatakon 0:402f8c75d394 9 _odom.resize(3);
zatakon 0:402f8c75d394 10 _ratio.resize(2);
zatakon 0:402f8c75d394 11 }
zatakon 0:402f8c75d394 12
zatakon 0:402f8c75d394 13 float Robot::getRadius(float servoAngle)
zatakon 0:402f8c75d394 14 {
zatakon 0:402f8c75d394 15 if( abs(servoAngle - (M_PI/2) ) < 0.05 )
zatakon 0:402f8c75d394 16 _R = 999.9;
zatakon 0:402f8c75d394 17 else {
zatakon 0:402f8c75d394 18 float alfa = servoAngle;
zatakon 0:402f8c75d394 19 float beta = alfa - (M_PI/2);
zatakon 0:402f8c75d394 20 _R = (_whbs / sin(beta)) * sin(alfa);
zatakon 0:402f8c75d394 21 }
zatakon 0:402f8c75d394 22 return _R;
zatakon 0:402f8c75d394 23 }
zatakon 0:402f8c75d394 24
zatakon 0:402f8c75d394 25 void Robot::countRadius(float servoAngle)
zatakon 0:402f8c75d394 26 {
zatakon 0:402f8c75d394 27 if( abs(servoAngle - (M_PI/2) ) < 0.05 )
zatakon 0:402f8c75d394 28 _R = 999.9;
zatakon 0:402f8c75d394 29 else {
zatakon 0:402f8c75d394 30 float alfa = servoAngle;
zatakon 0:402f8c75d394 31 float beta = alfa - (M_PI/2);
zatakon 0:402f8c75d394 32 _R = (_whbs / sin(beta)) * sin(alfa);
zatakon 0:402f8c75d394 33 }
zatakon 0:402f8c75d394 34 }
zatakon 0:402f8c75d394 35
zatakon 0:402f8c75d394 36 float Robot::getServoAngle(float R)
zatakon 0:402f8c75d394 37 {
zatakon 0:402f8c75d394 38 R = -R;
zatakon 0:402f8c75d394 39 float beta = atan(_whbs / R);
zatakon 0:402f8c75d394 40 float servoAngle = (M_PI / 2) - beta;
zatakon 0:402f8c75d394 41 return servoAngle;
zatakon 0:402f8c75d394 42 }
zatakon 0:402f8c75d394 43
zatakon 0:402f8c75d394 44 std::vector<float> Robot::getSpeed(std::vector<int> encoders)
zatakon 0:402f8c75d394 45 {
zatakon 0:402f8c75d394 46 std::vector<float> data(3);
zatakon 0:402f8c75d394 47 float v, v1, v2, fi;
zatakon 0:402f8c75d394 48
zatakon 0:402f8c75d394 49 v1 = ((float)encoders[0] * 125.0)/(_countPerMeter); // left wheel
zatakon 0:402f8c75d394 50 v2 = ((float)encoders[1] * 125.0)/(_countPerMeter); // right wheel
zatakon 0:402f8c75d394 51
zatakon 0:402f8c75d394 52 v = (v1+v2)/2;
zatakon 0:402f8c75d394 53
zatakon 0:402f8c75d394 54 if( abs(v) < 0.001 ) {
zatakon 0:402f8c75d394 55 _odom[0] = 0.0;
zatakon 0:402f8c75d394 56 _odom[1] = 0.0;
zatakon 0:402f8c75d394 57 _odom[2] = 0.0;
zatakon 0:402f8c75d394 58 }
zatakon 0:402f8c75d394 59 else {
zatakon 0:402f8c75d394 60 fi = v/_R;
zatakon 0:402f8c75d394 61 _odom[0] = _R*sin(fi);
zatakon 0:402f8c75d394 62 _odom[1] = _R*(1-cos(fi));
zatakon 0:402f8c75d394 63 _odom[2] = fi;
zatakon 0:402f8c75d394 64 }
zatakon 0:402f8c75d394 65
zatakon 0:402f8c75d394 66 data[0] = _odom[0];
zatakon 0:402f8c75d394 67 data[1] = _odom[1];
zatakon 0:402f8c75d394 68 data[2] = _odom[2];
zatakon 0:402f8c75d394 69
zatakon 0:402f8c75d394 70 return data;
zatakon 0:402f8c75d394 71 }
zatakon 0:402f8c75d394 72
zatakon 0:402f8c75d394 73
zatakon 0:402f8c75d394 74 std::vector<float> Robot::getMotorRatio()
zatakon 0:402f8c75d394 75 {
zatakon 0:402f8c75d394 76 _ratio[0] = (1 - (_w/(2*_R)));
zatakon 0:402f8c75d394 77 _ratio[1] = (1 + (_w/(2*_R)));
zatakon 0:402f8c75d394 78
zatakon 0:402f8c75d394 79 return _ratio;
zatakon 0:402f8c75d394 80 }
zatakon 0:402f8c75d394 81
zatakon 0:402f8c75d394 82 std::vector<float> Robot::getMotorRatio(float R)
zatakon 0:402f8c75d394 83 {
zatakon 0:402f8c75d394 84 _ratio[0] = (1 - (_w/(2*R)));
zatakon 0:402f8c75d394 85 _ratio[1] = (1 + (_w/(2*R)));
zatakon 0:402f8c75d394 86
zatakon 0:402f8c75d394 87 return _ratio;
zatakon 0:402f8c75d394 88 }
zatakon 0:402f8c75d394 89
zatakon 0:402f8c75d394 90