YUTO WATANABE / OmniMove

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

OmniMove.h@9:91ce72a587ad, 2021-08-27 (annotated)

Committer:
DarkFlame
Date:
Fri Aug 27 06:33:57 2021 +0000
Revision:
9:91ce72a587ad
Parent:
7:9b9d488ebcfd
Child:
10:f902cac49aaf
a little fix.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DarkFlame 1:e515f6a4da2e 1 #ifndef OmniMove_H
DarkFlame 1:e515f6a4da2e 2 #define OmniMove_H
DarkFlame 0:44476ac6ab91 3
DarkFlame 0:44476ac6ab91 4 #include "mbed.h"
DarkFlame 0:44476ac6ab91 5
DarkFlame 0:44476ac6ab91 6 /*
DarkFlame 0:44476ac6ab91 7 OmniMove.h
DarkFlame 0:44476ac6ab91 8 オムニ・メカナム等の全方位移動ベクトル演算ライブラリ
DarkFlame 0:44476ac6ab91 9 Designer: Watanabe Yuuto
DarkFlame 0:44476ac6ab91 10
DarkFlame 6:fac3dcaebe83 11 このライブラリは極座標(or直交座標)で表したマシンの走行速度、進行方向から
DarkFlame 0:44476ac6ab91 12 各車輪の回転速度を演算するライブラリです。
DarkFlame 0:44476ac6ab91 13 3~8輪までの車輪数や車輪の位置などを指定できます。
DarkFlame 0:44476ac6ab91 14
DarkFlame 0:44476ac6ab91 15 ・ホイールの位置関係(4つの場合)
DarkFlame 0:44476ac6ab91 16 注.4つの場合は0,1,2,3とナンバリングする
DarkFlame 0:44476ac6ab91 17 front
DarkFlame 0:44476ac6ab91 18 ┌───────────────┐
DarkFlame 0:44476ac6ab91 19 │ w1← │
DarkFlame 0:44476ac6ab91 20 l│ y │r
DarkFlame 0:44476ac6ab91 21 e│ ↑ w0│i
DarkFlame 0:44476ac6ab91 22 f│↓ . →x ↑│g
DarkFlame 0:44476ac6ab91 23 t│w2 │h
DarkFlame 0:44476ac6ab91 24 │ │t
DarkFlame 0:44476ac6ab91 25 │ →w3 │
DarkFlame 0:44476ac6ab91 26 └───────────────┘
DarkFlame 0:44476ac6ab91 27 back
DarkFlame 0:44476ac6ab91 28 この場合の配置では0番目の車輪軸の角度fstWheelAngは0度となる
DarkFlame 0:44476ac6ab91 29
DarkFlame 0:44476ac6ab91 30 ・マシンの進行方向thetaについて
DarkFlame 0:44476ac6ab91 31 例)theta=0の場合→right方向に進む
DarkFlame 0:44476ac6ab91 32 theta=90の場合→front方向に進む
DarkFlame 0:44476ac6ab91 33 theta=-90の場合→back方向に進む
DarkFlame 0:44476ac6ab91 34 theta=180(or theta=-180)の場合→left方向に進む
DarkFlame 0:44476ac6ab91 35
DarkFlame 0:44476ac6ab91 36 ・マシンの回転速度Vrollについて
DarkFlame 0:44476ac6ab91 37 　値が正(>0)であれば反時計回り、値が負(<0)であれば時計回りにマシンが回転する
DarkFlame 0:44476ac6ab91 38
DarkFlame 7:9b9d488ebcfd 39 ・使用例(4輪の場合)
DarkFlame 7:9b9d488ebcfd 40 #include "mbed.h"
DarkFlame 7:9b9d488ebcfd 41 #include "OmniMove/OmniMove.h" //インクルァぁドしてネ
DarkFlame 7:9b9d488ebcfd 42
DarkFlame 7:9b9d488ebcfd 43 OmniMove mekanamu; //TickerやTimerのように宣言
DarkFlame 7:9b9d488ebcfd 44
DarkFlame 7:9b9d488ebcfd 45 int main(){
DarkFlame 9:91ce72a587ad 46 float motor_output[4];//それぞれのモータ出力変数(配列)を用意
DarkFlame 7:9b9d488ebcfd 47
DarkFlame 7:9b9d488ebcfd 48 mekanamu.setup(4,45); //車輪の個数、位置を設定(1度宣言すればいい)
DarkFlame 7:9b9d488ebcfd 49
DarkFlame 7:9b9d488ebcfd 50 while(1) {
DarkFlame 7:9b9d488ebcfd 51
DarkFlame 7:9b9d488ebcfd 52 mekanamu.input_polar(0.5,90,0); //マシンの走行速度、進行方向、回転速度の入力
DarkFlame 7:9b9d488ebcfd 53
DarkFlame 9:91ce72a587ad 54 //配列の先頭アドレスを渡すことでそれぞれのモータ出力変数に値を入力
DarkFlame 9:91ce72a587ad 55 mekanamu.output(motor_output);
DarkFlame 7:9b9d488ebcfd 56
DarkFlame 7:9b9d488ebcfd 57 }
DarkFlame 7:9b9d488ebcfd 58 }
DarkFlame 0:44476ac6ab91 59 */
DarkFlame 0:44476ac6ab91 60
DarkFlame 1:e515f6a4da2e 61 class OmniMove
DarkFlame 0:44476ac6ab91 62 {
DarkFlame 0:44476ac6ab91 63 public:
DarkFlame 0:44476ac6ab91 64 /*
DarkFlame 0:44476ac6ab91 65 セットアップ関数
DarkFlame 3:e08a0ee65f69 66 nWheel:車輪の個数(3~8),fstWheelAng:0番目のx軸と車輪軸のなす角度(deg) */
DarkFlame 6:fac3dcaebe83 67 void setup(int nWheel,float fstWheelAng);
DarkFlame 0:44476ac6ab91 68
DarkFlame 0:44476ac6ab91 69 /*
DarkFlame 0:44476ac6ab91 70 全方位移動入力関数(極座標)
DarkFlame 3:e08a0ee65f69 71 r:マシンの速度(0~1),theta:マシンの進行方向(角度deg ex.90,-45)
DarkFlame 7:9b9d488ebcfd 72 Vroll:マシンの回転速度(-1~1) */
DarkFlame 7:9b9d488ebcfd 73 void input_polar(float r,float theta,float Vroll);
DarkFlame 0:44476ac6ab91 74
DarkFlame 0:44476ac6ab91 75 /*
DarkFlame 0:44476ac6ab91 76 マシンの傾き角度を考慮した全方位移動入力関数(極座標)
DarkFlame 0:44476ac6ab91 77 MachineAng:マシンの現在角度(角度deg ex.90,-45) */
DarkFlame 7:9b9d488ebcfd 78 void input_polar(float r,float theta,float Vroll,float MachineAng);
DarkFlame 0:44476ac6ab91 79
DarkFlame 0:44476ac6ab91 80 /*
DarkFlame 3:e08a0ee65f69 81 全方位移動入力関数(直交座標)
DarkFlame 3:e08a0ee65f69 82 x:マシンのx方向速度(-1~1),y:マシンのy方向速度(-1~1)
DarkFlame 7:9b9d488ebcfd 83 Vroll:マシンの回転速度(-1~1) */
DarkFlame 7:9b9d488ebcfd 84 void input_cartesian(float x,float y,float Vroll);
DarkFlame 3:e08a0ee65f69 85
DarkFlame 3:e08a0ee65f69 86 /*
DarkFlame 3:e08a0ee65f69 87 マシンの傾き角度を考慮した全方位移動入力関数(直交座標)
DarkFlame 3:e08a0ee65f69 88 MachineAng:マシンの現在角度(角度deg ex.90,-45) */
DarkFlame 7:9b9d488ebcfd 89 void input_cartesian(float x,float y,float Vroll,float MachineAng);
DarkFlame 3:e08a0ee65f69 90
DarkFlame 3:e08a0ee65f69 91 /*
DarkFlame 0:44476ac6ab91 92 全方位移動出力関数(アドレスよくわからん人用)
DarkFlame 4:4d94b6148a0a 93 引数:n番目の車輪の回転速度 */
DarkFlame 6:fac3dcaebe83 94 float output_(int n);
DarkFlame 0:44476ac6ab91 95
DarkFlame 0:44476ac6ab91 96 /*
DarkFlame 0:44476ac6ab91 97 全方位移動出力関数
DarkFlame 0:44476ac6ab91 98 *v:車輪回転速度の配列の先頭アドレス */
DarkFlame 6:fac3dcaebe83 99 void output(float *V);
DarkFlame 0:44476ac6ab91 100
DarkFlame 0:44476ac6ab91 101 private:
DarkFlame 0:44476ac6ab91 102 int i,nWheel;
DarkFlame 6:fac3dcaebe83 103 float Vx_wheel[8],Vy_wheel[8],Vx,Vy,Vroll,r,theta;
DarkFlame 3:e08a0ee65f69 104
DarkFlame 6:fac3dcaebe83 105 float limit(float min,float max,float _value);
DarkFlame 6:fac3dcaebe83 106 float conv_deg(float _rad);
DarkFlame 6:fac3dcaebe83 107 float conv_rad(float _deg);
DarkFlame 0:44476ac6ab91 108 };
DarkFlame 0:44476ac6ab91 109
DarkFlame 0:44476ac6ab91 110 #endif