Diff: OmniMove.h

Revision:

0:44476ac6ab91

Child:

1:e515f6a4da2e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/OmniMove.h	Wed Mar 24 16:33:58 2021 +0000
@@ -0,0 +1,82 @@
+#ifndef OMUNIMOVE_H
+#define OMUNIMOVE_H
+
+#include "mbed.h"
+
+/*
+OmniMove.h
+オムニ・メカナム等の全方位移動ベクトル演算ライブラリ
+Designer: Watanabe Yuuto
+
+このライブラリは極座標で表したマシンの走行速度、進行方向から
+各車輪の回転速度を演算するライブラリです。
+3~8輪までの車輪数や車輪の位置などを指定できます。
+
+・ホイールの位置関係(4つの場合)
+  注.4つの場合は0,1,2,3とナンバリングする
+       front
+  ┌───────────────┐
+  │     w1←       │
+ l│       y       │r
+ e│       ↑     w0│i
+ f│↓      . →x   ↑│g
+ t│w2             │h
+  │               │t
+  │       →w3     │
+  └───────────────┘
+         back       
+この場合の配置では0番目の車輪軸の角度fstWheelAngは0度となる
+
+・マシンの進行方向thetaについて
+例)theta=0の場合→right方向に進む
+   theta=90の場合→front方向に進む
+   theta=-90の場合→back方向に進む
+   theta=180(or theta=-180)の場合→left方向に進む
+   
+・マシンの回転速度Vrollについて
+　値が正(>0)であれば反時計回り、値が負(<0)であれば時計回りにマシンが回転する
+
+・回転速度の重みroll_ratioについて
+例)roll_ratio=0.5の場合→マシンの直進成分が5割、回転成分が5割になる
+　　roll_ratio=0.8の場合→マシンの直進成分が2割、回転成分が8割になる
+*/
+
+class OmuniMove
+{
+public:
+    /*
+    セットアップ関数
+    nWheel:車輪の個数,fstWheelAng:0番目のx軸と車輪軸のなす角度 */
+    void setup(int nWheel,float fstWheelAng);
+    
+    /*
+    全方位移動入力関数(極座標)
+    r:マシンの速度(-1~1),theta:マシンの進行方向(角度deg ex.90,-45)
+    Vroll:マシンの回転速度(-1~1),roll_ratio:回転速度の重み(0~1)    */
+    void input_polar(float r,float theta,float Vroll,float roll_ratio);
+    
+    /*
+    マシンの傾き角度を考慮した全方位移動入力関数(極座標)
+    MachineAng:マシンの現在角度(角度deg ex.90,-45)  */
+    void input_polar(float r,float theta,float Vroll,float roll_ratio,float MachineAng);
+    
+    /*
+    全方位移動出力関数(アドレスよくわからん人用)
+    引数:nWheel番目の車輪の回転速度  */
+    float output(int nWheel);
+    
+    /*
+    全方位移動出力関数
+    *v:車輪回転速度の配列の先頭アドレス  */
+    void output(float *V);
+
+private:
+    int i,nWheel;
+    float Vx_wheel[8],Vy_wheel[8],Vx,Vy,Vroll;
+    float m_pi;
+
+    float conv_deg(float _rad);
+    float conv_rad(float _deg);
+};
+
+#endif
\ No newline at end of file