File content as of revision 1:e515f6a4da2e:

#ifndef OmniMove_H
#define OmniMove_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 OmniMove
{
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