Diff: OmniMove.cpp

Revision:

3:e08a0ee65f69

Parent:

2:4c4ff6bf6282

Child:

4:4d94b6148a0a

--- a/OmniMove.cpp	Thu Mar 25 02:34:43 2021 +0000
+++ b/OmniMove.cpp	Thu Mar 25 11:49:16 2021 +0000
@@ -3,8 +3,7 @@
 #include "OmniMove.h"
 
 void OmniMove::setup(int nWheel,float fstWheelAng){
-    this->nWheel = nWheel;
-    m_pi = 3.1415;
+    this->nWheel = limit(3,8,nWheel);
     
     for(i = 0;i < nWheel; i++){
         Vx_wheel[i] = -1 * sin( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
@@ -13,15 +12,45 @@
 }
 
 void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio){
+    r = limit(0,1,r);
+    Vroll = limit(-1,1,Vroll);
+    roll_ratio = limit(0,1,roll_ratio);
     Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
     Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
-    this->Vroll = Vroll * roll_ratio;
+    Vroll = Vroll * roll_ratio;
 }
 
 void OmniMove::input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng){
+    r = limit(0,1,r);
+    Vroll = limit(-1,1,Vroll);
+    roll_ratio = limit(0,1,roll_ratio);
     Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
     Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
-    this->Vroll = Vroll * roll_ratio;
+    Vroll = Vroll * roll_ratio;
+}
+
+void OmniMove::input_cartesian(float x,float y,float Vroll,float roll_ratio){
+    x = limit(-1,1,x);
+    y = limit(-1,1,y);
+    Vroll = limit(-1,1,Vroll);
+    roll_ratio = limit(0,1,roll_ratio);
+    r = limit(0,1,hypotf(x,y));
+    theta = conv_deg(atan2(y,x));
+    Vx = r * cos( conv_rad( theta ) ) * ( 1 - roll_ratio );
+    Vy = r * sin( conv_rad( theta ) ) * ( 1 - roll_ratio );
+    Vroll = Vroll * roll_ratio;
+}
+
+void OmniMove::input_cartesian(float x,float y,float Vroll,float roll_ratio,float MachineAng){
+    x = limit(-1,1,x);
+    y = limit(-1,1,y);
+    Vroll = limit(-1,1,Vroll);
+    roll_ratio = limit(0,1,roll_ratio);
+    r = limit(0,1,hypotf(x,y));
+    theta = conv_deg(atan2(y,x));
+    Vx = r * cos( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
+    Vy = r * sin( conv_rad( theta - MachineAng ) ) * ( 1 - roll_ratio );
+    Vroll = Vroll * roll_ratio;
 }
 
 float OmniMove::output_(int nWheel){
@@ -38,10 +67,20 @@
     }
 }
 
+float OmniMove::limit(float min,float max,float _value){
+    if(_value > max){
+        return max;
+    }else if(_value < min){
+        return min;
+    }else{
+        return _value;
+    }
+}
+
 float OmniMove::conv_deg(float _rad){
-    return _rad * 180 / m_pi;
+    return _rad * 180 / 3.141592;
 }
 
 float OmniMove::conv_rad(float _deg){
-    return m_pi * _deg / 180;
+    return 3.141592 * _deg / 180;
 }