Diff: Odometry.cpp

Revision:

0:402f8c75d394

Child:

1:1765665581cc

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Odometry.cpp	Tue Aug 11 13:05:46 2015 +0000
@@ -0,0 +1,90 @@
+#include "Odometry.h"
+
+Robot::Robot(float wheelbase, float width, float countPerMeter)
+{
+    _whbs = wheelbase;
+    _w = width;
+    _countPerMeter = countPerMeter;
+    
+    _odom.resize(3);
+    _ratio.resize(2);
+}
+
+float Robot::getRadius(float servoAngle)
+{    
+    if( abs(servoAngle - (M_PI/2) ) < 0.05 )
+        _R = 999.9;       
+    else {
+        float alfa = servoAngle;
+        float beta = alfa - (M_PI/2);
+        _R = (_whbs / sin(beta)) * sin(alfa);         
+    }
+    return _R;
+}
+
+void Robot::countRadius(float servoAngle)
+{    
+    if( abs(servoAngle - (M_PI/2) ) < 0.05 )
+        _R = 999.9;       
+    else {
+        float alfa = servoAngle;
+        float beta = alfa - (M_PI/2);
+        _R = (_whbs / sin(beta)) * sin(alfa);         
+    }
+}
+
+float Robot::getServoAngle(float R)
+{    
+    R = -R;
+    float beta = atan(_whbs / R);
+    float servoAngle = (M_PI / 2) - beta;
+    return servoAngle;
+}
+
+std::vector<float> Robot::getSpeed(std::vector<int> encoders) 
+{    
+    std::vector<float> data(3);
+    float v, v1, v2, fi;
+    
+    v1 =  ((float)encoders[0] * 125.0)/(_countPerMeter); // left wheel
+    v2 =  ((float)encoders[1] * 125.0)/(_countPerMeter); // right wheel
+    
+    v = (v1+v2)/2;
+    
+    if( abs(v) < 0.001 ) {
+        _odom[0] = 0.0;
+        _odom[1] = 0.0;
+        _odom[2] = 0.0;
+    }
+    else {
+        fi = v/_R;
+        _odom[0] = _R*sin(fi);
+        _odom[1] = _R*(1-cos(fi));
+        _odom[2] = fi;
+    }
+
+    data[0] = _odom[0];
+    data[1] = _odom[1];
+    data[2] = _odom[2];
+    
+    return data;
+}
+
+
+std::vector<float> Robot::getMotorRatio()
+{
+  _ratio[0] = (1 - (_w/(2*_R)));
+  _ratio[1] = (1 + (_w/(2*_R)));
+
+  return _ratio;
+}
+
+std::vector<float> Robot::getMotorRatio(float R)
+{
+  _ratio[0] = (1 - (_w/(2*R)));
+  _ratio[1] = (1 + (_w/(2*R)));
+
+  return _ratio;
+}
+
+