質問編集履歴
1
test
CHANGED
|
File without changes
|
test
CHANGED
|
@@ -9,3 +9,119 @@
|
|
|
9
9
|
このリンクにあるシミュレーションが行いたいのですが、プログラミング例が載っていませんでした。
|
|
10
10
|
|
|
11
11
|
どなたかご教授お願いします。
|
|
12
|
+
|
|
13
|
+
|
|
14
|
+
|
|
15
|
+
|
|
16
|
+
|
|
17
|
+
追記
|
|
18
|
+
|
|
19
|
+
|
|
20
|
+
|
|
21
|
+
ブロック線図を書いてみました。
|
|
22
|
+
|
|
23
|
+
https://i.imgur.com/bQjVTwp.jpg
|
|
24
|
+
|
|
25
|
+
そこからプログラミングに落とそうと思っているのですが、行列の使い方がよくわかりません。
|
|
26
|
+
|
|
27
|
+
|
|
28
|
+
|
|
29
|
+
合っているか分かりませんが、やってみたところまでです。
|
|
30
|
+
|
|
31
|
+
#include <stdio.h>
|
|
32
|
+
|
|
33
|
+
#include <stdlib.h>
|
|
34
|
+
|
|
35
|
+
#include <math.h>
|
|
36
|
+
|
|
37
|
+
|
|
38
|
+
|
|
39
|
+
int main(void)
|
|
40
|
+
|
|
41
|
+
{
|
|
42
|
+
|
|
43
|
+
double l_1=0.5;
|
|
44
|
+
|
|
45
|
+
double l_2=0.5;
|
|
46
|
+
|
|
47
|
+
double m_1=1.0;
|
|
48
|
+
|
|
49
|
+
double m_2=1.0;
|
|
50
|
+
|
|
51
|
+
double x_cmd=0.0;
|
|
52
|
+
|
|
53
|
+
double dx_cmd=0.0;
|
|
54
|
+
|
|
55
|
+
double ddx_ref;
|
|
56
|
+
|
|
57
|
+
double ddth_ref=0.0;
|
|
58
|
+
|
|
59
|
+
double I_a=0.0;
|
|
60
|
+
|
|
61
|
+
double T_dis=0.0;
|
|
62
|
+
|
|
63
|
+
double ddth_res=0.0;
|
|
64
|
+
|
|
65
|
+
double dth_res=0.0;
|
|
66
|
+
|
|
67
|
+
double th_res=0.0;
|
|
68
|
+
|
|
69
|
+
double x_res=0.0;
|
|
70
|
+
|
|
71
|
+
double dx_res=0.0;
|
|
72
|
+
|
|
73
|
+
double J_-1=0.0;
|
|
74
|
+
|
|
75
|
+
double M_n=0.0;
|
|
76
|
+
|
|
77
|
+
double G_r_K_tn=0.0;
|
|
78
|
+
|
|
79
|
+
double Kp=1;
|
|
80
|
+
|
|
81
|
+
double Kv=1;
|
|
82
|
+
|
|
83
|
+
double M=0.0;
|
|
84
|
+
|
|
85
|
+
double J_aco=0.0;
|
|
86
|
+
|
|
87
|
+
double Direct[2][2]={{;
|
|
88
|
+
|
|
89
|
+
|
|
90
|
+
|
|
91
|
+
|
|
92
|
+
|
|
93
|
+
FILE*data;
|
|
94
|
+
|
|
95
|
+
data=fopen("yay.dat","w");
|
|
96
|
+
|
|
97
|
+
|
|
98
|
+
|
|
99
|
+
for(t=0.0; t<=10; t=t+0.001)
|
|
100
|
+
|
|
101
|
+
{
|
|
102
|
+
|
|
103
|
+
x_cmd=0.1;
|
|
104
|
+
|
|
105
|
+
dx_cmd=0;
|
|
106
|
+
|
|
107
|
+
dx_res=dth_res*J_aco;
|
|
108
|
+
|
|
109
|
+
x_res=th_res ///わからん
|
|
110
|
+
|
|
111
|
+
ddx_ref=(x_cmd-x_res)*Kp+(dx_cmd-dx_res)*Kv;
|
|
112
|
+
|
|
113
|
+
ddth_ref=ddx_ref*J_-1;
|
|
114
|
+
|
|
115
|
+
ddth_res=((ddth_ref*M_n)-T_dis)*M^(-1);
|
|
116
|
+
|
|
117
|
+
dth_res+=ddth_res*0.001;
|
|
118
|
+
|
|
119
|
+
th_res+=dth_res*0.001;
|
|
120
|
+
|
|
121
|
+
|
|
122
|
+
|
|
123
|
+
|
|
124
|
+
|
|
125
|
+
|
|
126
|
+
|
|
127
|
+
}
|