Diff: OmniMove.cpp

Revision:

0:44476ac6ab91

Child:

1:e515f6a4da2e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniMove.cpp	Wed Mar 24 16:33:58 2021 +0000
@@ -0,0 +1,47 @@
+#include "mbed.h"
+#include "math.h"
+#include "OmniMove.h"
+
+void OmuniMove::setup(int nWheel,float fstWheelAng){
+    this->nWheel = nWheel;
+    m_pi = 3.1415;
+    
+    for(i = 0;i < nWheel; i++){
+        Vx_wheel[i] = -1 * sin( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
+        Vy_wheel[i] = cos( conv_rad( fstWheelAng + (360 / nWheel) * i ) );
+    }
+}
+
+void OmuniMove::input_polar(float r,float theta,float Vroll,float 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;
+}
+
+void OmuniMove::input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng){
+    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;
+}
+
+float OmuniMove::output(int nWheel){
+    if( nWheel < 8 ){
+        return Vx*Vx_wheel[nWheel] + Vy*Vy_wheel[nWheel] + Vroll;
+    }else{
+        return 0;
+    }
+}
+
+void OmuniMove::output(float *V){
+    for(i = 0;i < nWheel;i++){
+        V[i] = Vx*Vx_wheel[i] + Vy*Vy_wheel[i] + Vroll;
+    }
+}
+
+float OmuniMove::conv_deg(float _rad){
+    return _rad * 180 / m_pi;
+}
+
+float OmuniMove::conv_rad(float _deg){
+    return m_pi * _deg / 180;
+}