Revision 1:1765665581cc, committed 2015-09-10

Comitter:

zatakon

Date:

Thu Sep 10 07:37:48 2015 +0000

Parent:

0:402f8c75d394

Commit message:

Changed in this revision

diff -r 402f8c75d394 -r 1765665581cc Odometry.cpp
--- 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;
 }
 
 
+

diff -r 402f8c75d394 -r 1765665581cc Odometry.h
--- a/Odometry.h	Tue Aug 11 13:05:46 2015 +0000
+++ b/Odometry.h	Thu Sep 10 07:37:48 2015 +0000
@@ -10,19 +10,13 @@
 class Robot
 {
   public:
-    Robot(float wheelbase, float width, float countPerMeter);
+    Robot(float width, float countPerMeter);
     
     /* returns radius in [m]. When turning right, radius is negativ
      *    left is positive
-     *
-     * param servoAngle is how is the servo angle in radians. Center 
-     *    is pi/2 (1.571)
     */ 
-    float getRadius(float servoAngle);
-    
-    void countRadius(float servoAngle);
-    
-    float getServoAngle(float R);
+    float getRadius(float v1, float v2);
+    void countRadius(float v1, float v2);
     
     /* returns speed in format 
      *      [0] = linear x
@@ -33,12 +27,9 @@
      */         
     std::vector<float> getSpeed(std::vector<int> encoders);
   
-    std::vector<float> getMotorRatio();
-    
-    std::vector<float> getMotorRatio(float R);
+    std::vector<float> getMotorRatio(float x, float z);
 
   private:  
-    float _whbs;      // wheelbase
     float _w;         // width
     float _R;
     uint32_t _countPerMeter;