質問編集履歴
1
title
CHANGED
|
File without changes
|
body
CHANGED
|
@@ -3,4 +3,62 @@
|
|
|
3
3
|
|
|
4
4
|
http://cmorito.blogspot.jp/2012/05/2.html
|
|
5
5
|
このリンクにあるシミュレーションが行いたいのですが、プログラミング例が載っていませんでした。
|
|
6
|
-
どなたかご教授お願いします。
|
|
6
|
+
どなたかご教授お願いします。
|
|
7
|
+
|
|
8
|
+
|
|
9
|
+
追記
|
|
10
|
+
|
|
11
|
+
ブロック線図を書いてみました。
|
|
12
|
+
https://i.imgur.com/bQjVTwp.jpg
|
|
13
|
+
そこからプログラミングに落とそうと思っているのですが、行列の使い方がよくわかりません。
|
|
14
|
+
|
|
15
|
+
合っているか分かりませんが、やってみたところまでです。
|
|
16
|
+
#include <stdio.h>
|
|
17
|
+
#include <stdlib.h>
|
|
18
|
+
#include <math.h>
|
|
19
|
+
|
|
20
|
+
int main(void)
|
|
21
|
+
{
|
|
22
|
+
double l_1=0.5;
|
|
23
|
+
double l_2=0.5;
|
|
24
|
+
double m_1=1.0;
|
|
25
|
+
double m_2=1.0;
|
|
26
|
+
double x_cmd=0.0;
|
|
27
|
+
double dx_cmd=0.0;
|
|
28
|
+
double ddx_ref;
|
|
29
|
+
double ddth_ref=0.0;
|
|
30
|
+
double I_a=0.0;
|
|
31
|
+
double T_dis=0.0;
|
|
32
|
+
double ddth_res=0.0;
|
|
33
|
+
double dth_res=0.0;
|
|
34
|
+
double th_res=0.0;
|
|
35
|
+
double x_res=0.0;
|
|
36
|
+
double dx_res=0.0;
|
|
37
|
+
double J_-1=0.0;
|
|
38
|
+
double M_n=0.0;
|
|
39
|
+
double G_r_K_tn=0.0;
|
|
40
|
+
double Kp=1;
|
|
41
|
+
double Kv=1;
|
|
42
|
+
double M=0.0;
|
|
43
|
+
double J_aco=0.0;
|
|
44
|
+
double Direct[2][2]={{;
|
|
45
|
+
|
|
46
|
+
|
|
47
|
+
FILE*data;
|
|
48
|
+
data=fopen("yay.dat","w");
|
|
49
|
+
|
|
50
|
+
for(t=0.0; t<=10; t=t+0.001)
|
|
51
|
+
{
|
|
52
|
+
x_cmd=0.1;
|
|
53
|
+
dx_cmd=0;
|
|
54
|
+
dx_res=dth_res*J_aco;
|
|
55
|
+
x_res=th_res ///わからん
|
|
56
|
+
ddx_ref=(x_cmd-x_res)*Kp+(dx_cmd-dx_res)*Kv;
|
|
57
|
+
ddth_ref=ddx_ref*J_-1;
|
|
58
|
+
ddth_res=((ddth_ref*M_n)-T_dis)*M^(-1);
|
|
59
|
+
dth_res+=ddth_res*0.001;
|
|
60
|
+
th_res+=dth_res*0.001;
|
|
61
|
+
|
|
62
|
+
|
|
63
|
+
|
|
64
|
+
}
|