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@7:9b9d488ebcfd, 2021-04-12 (annotated)

Committer:
DarkFlame
Date:
Mon Apr 12 10:38:12 2021 +0000
Revision:
7:9b9d488ebcfd
Parent:
6:fac3dcaebe83
Child:
9:91ce72a587ad
roll_ratio deleted.

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 7:9b9d488ebcfd 46 float motor_output0,motor_output1,motor_output2,motor_output3;//それぞれのモータ出力変数を用意
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 7:9b9d488ebcfd 54 //それぞれのモータ出力変数に値を入力
DarkFlame 7:9b9d488ebcfd 55 motor_output0 = mekanamu.output_(0);
DarkFlame 7:9b9d488ebcfd 56 motor_output1 = mekanamu.output_(1);
DarkFlame 7:9b9d488ebcfd 57 motor_output2 = mekanamu.output_(2);
DarkFlame 7:9b9d488ebcfd 58 motor_output3 = mekanamu.output_(3);
DarkFlame 7:9b9d488ebcfd 59
DarkFlame 7:9b9d488ebcfd 60 }
DarkFlame 7:9b9d488ebcfd 61 }
DarkFlame 0:44476ac6ab91 62 */
DarkFlame 0:44476ac6ab91 63
DarkFlame 1:e515f6a4da2e 64 class OmniMove
DarkFlame 0:44476ac6ab91 65 {
DarkFlame 0:44476ac6ab91 66 public:
DarkFlame 0:44476ac6ab91 67 /*
DarkFlame 0:44476ac6ab91 68 セットアップ関数
DarkFlame 3:e08a0ee65f69 69 nWheel:車輪の個数(3~8),fstWheelAng:0番目のx軸と車輪軸のなす角度(deg) */
DarkFlame 6:fac3dcaebe83 70 void setup(int nWheel,float fstWheelAng);
DarkFlame 0:44476ac6ab91 71
DarkFlame 0:44476ac6ab91 72 /*
DarkFlame 0:44476ac6ab91 73 全方位移動入力関数(極座標)
DarkFlame 3:e08a0ee65f69 74 r:マシンの速度(0~1),theta:マシンの進行方向(角度deg ex.90,-45)
DarkFlame 7:9b9d488ebcfd 75 Vroll:マシンの回転速度(-1~1) */
DarkFlame 7:9b9d488ebcfd 76 void input_polar(float r,float theta,float Vroll);
DarkFlame 0:44476ac6ab91 77
DarkFlame 0:44476ac6ab91 78 /*
DarkFlame 0:44476ac6ab91 79 マシンの傾き角度を考慮した全方位移動入力関数(極座標)
DarkFlame 0:44476ac6ab91 80 MachineAng:マシンの現在角度(角度deg ex.90,-45) */
DarkFlame 7:9b9d488ebcfd 81 void input_polar(float r,float theta,float Vroll,float MachineAng);
DarkFlame 0:44476ac6ab91 82
DarkFlame 0:44476ac6ab91 83 /*
DarkFlame 3:e08a0ee65f69 84 全方位移動入力関数(直交座標)
DarkFlame 3:e08a0ee65f69 85 x:マシンのx方向速度(-1~1),y:マシンのy方向速度(-1~1)
DarkFlame 7:9b9d488ebcfd 86 Vroll:マシンの回転速度(-1~1) */
DarkFlame 7:9b9d488ebcfd 87 void input_cartesian(float x,float y,float Vroll);
DarkFlame 3:e08a0ee65f69 88
DarkFlame 3:e08a0ee65f69 89 /*
DarkFlame 3:e08a0ee65f69 90 マシンの傾き角度を考慮した全方位移動入力関数(直交座標)
DarkFlame 3:e08a0ee65f69 91 MachineAng:マシンの現在角度(角度deg ex.90,-45) */
DarkFlame 7:9b9d488ebcfd 92 void input_cartesian(float x,float y,float Vroll,float MachineAng);
DarkFlame 3:e08a0ee65f69 93
DarkFlame 3:e08a0ee65f69 94 /*
DarkFlame 0:44476ac6ab91 95 全方位移動出力関数(アドレスよくわからん人用)
DarkFlame 4:4d94b6148a0a 96 引数:n番目の車輪の回転速度 */
DarkFlame 6:fac3dcaebe83 97 float output_(int n);
DarkFlame 0:44476ac6ab91 98
DarkFlame 0:44476ac6ab91 99 /*
DarkFlame 0:44476ac6ab91 100 全方位移動出力関数
DarkFlame 0:44476ac6ab91 101 *v:車輪回転速度の配列の先頭アドレス */
DarkFlame 6:fac3dcaebe83 102 void output(float *V);
DarkFlame 0:44476ac6ab91 103
DarkFlame 0:44476ac6ab91 104 private:
DarkFlame 0:44476ac6ab91 105 int i,nWheel;
DarkFlame 6:fac3dcaebe83 106 float Vx_wheel[8],Vy_wheel[8],Vx,Vy,Vroll,r,theta;
DarkFlame 3:e08a0ee65f69 107
DarkFlame 6:fac3dcaebe83 108 float limit(float min,float max,float _value);
DarkFlame 6:fac3dcaebe83 109 float conv_deg(float _rad);
DarkFlame 6:fac3dcaebe83 110 float conv_rad(float _deg);
DarkFlame 0:44476ac6ab91 111 };
DarkFlame 0:44476ac6ab91 112
DarkFlame 0:44476ac6ab91 113 #endif