Diff: Odometry.cpp

Revision:

1:1765665581cc

Parent:

0:402f8c75d394

--- a/Odometry.cpp	Tue Aug 11 13:05:46 2015 +0000
+++ b/Odometry.cpp	Thu Sep 10 07:37:48 2015 +0000
@@ -1,8 +1,7 @@
 #include "Odometry.h"
 
-Robot::Robot(float wheelbase, float width, float countPerMeter)
+Robot::Robot(float width, float countPerMeter)
 {
-    _whbs = wheelbase;
     _w = width;
     _countPerMeter = countPerMeter;
     
@@ -10,81 +9,58 @@
     _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);         
-    }
+float Robot::getRadius(float v1, float v2)
+{   
+    if( abs(v2-v1) < 0.0001 )
+        _R = 99.9;
+    else
+        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
     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;
+void Robot::countRadius(float v1, float v2)
+{   
+    if( abs(v2-v1) < 0.0001 )
+        _R = 99.9;
+    else 
+        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
 }
 
 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
+    float v1, v2, fi;
     
-    v = (v1+v2)/2;
+    v1 =  ((float)encoders[0])/(_countPerMeter); // left wheel
+    v2 =  ((float)encoders[1])/(_countPerMeter); // right wheel
     
-    if( abs(v) < 0.001 ) {
-        _odom[0] = 0.0;
-        _odom[1] = 0.0;
-        _odom[2] = 0.0;
+    if( abs(v2-v1) < 0.0001 ) {
+        _R = 99.9;
     }
-    else {
-        fi = v/_R;
-        _odom[0] = _R*sin(fi);
-        _odom[1] = _R*(1-cos(fi));
-        _odom[2] = fi;
+    else {    
+        _R = (_w/2) - ( (_w * v2) / (v2 - v1));
     }
-
-    data[0] = _odom[0];
-    data[1] = _odom[1];
-    data[2] = _odom[2];
+    
+    fi = atan((v1-v2)/_w);
     
-    return data;
+    _odom[0] = _R * sin(fi);
+    _odom[1] = -tan(fi) * _odom[0];
+    _odom[2] = -fi;
+    
+    return _odom;
 }
 
 
-std::vector<float> Robot::getMotorRatio()
+std::vector<float> Robot::getMotorRatio(float x, float z)
 {
-  _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)));
-
+  float fi = ( abs(z) < 0.0001) ? 0.0001 : z;
+  
+  float R = x / sin(fi);
+  
+  _ratio[0] = tan(fi) * ( (_w/2) + R );
+  _ratio[1] = -tan(fi) * ( (_w/2) - R );
+  
   return _ratio;
 }
 
 
+