本研究では，対象物の並進速度と回転速度の両方を制御して投擲する直打法 について調査し，それをもとに提案した打撃制御について議論した．そして摩 擦不感型衝突アルゴリズムを用いて，打撃制御の理論を構築し，
IPによるシミ ュレーションにて検証した．
またそれと平行して，投擲実験を行うためのロボットアームの対象物把握部 を製作し，マニピュレータの制御システムをもとに把握部の駆動実験を行った．
今後の展望としては，打撃制御アルゴリズムの重力環境下への応用と，実験機 を用いた投擲実験より，アームの制御方法を検討することである．そして打撃 制御により対象物の回転を制御するロボットアームの製作である．さらには，
実際の直打法と同じ制御方法であるスナップ制御の理論構築に繋げていきたい
と考えている．
謝辞
本研究の遂行にあたり終始，御指導，御鞭撻下さいました，法政大学工学部 機械工学科 高島 俊教授には，この場を借りて深く感謝の意を表すとともに，
厚く御礼申し上げます．
また，本研究を進める上で互いに意見しあい，良き励みになった法政大学大 学院工学研究科機械工学専攻 渋谷 純也氏， 関 隆行氏， 中野 陽介氏 には，心から感謝いたします．
最後に，本研究に際して御協力，御助言おいただいた方々には，ここで厚く
御礼申し上げます．
参考文献
[1]
東森・木村・石井・金子，高速視覚情報に基づく棒状物体の動的捕獲戦略，
日本ロボット学会誌
[2]NHK 人間講座 「古の武術」に学ぶ DVD
豊田直也，皿状エンドエフェクタを持つマニピュレータによる物体の搬送，法 政大学大学院工学研究科修士論文
[3]有馬朗人，基礎物理学，
（2001） ，学術図書出版社
[4]
青木弘，工業力学， （
2003） ，養賢堂
[5]吉川恒夫，ロボット制御基礎論，（2003）
，コロナ社
[6]
鈴木方山，中級手裏剣術，（
2005） ，無明奄
[7]
吉川恒夫，ロボット制御基礎理論， （
1988） ，コロナ社
付録
摩擦不感型衝突アルゴリズム
初期値設定;
θ = πê2;
rx=rx; ry=0;
l=0.2;H対象物の長さL; e=0.5;H反発係数L;
k=l lê12;
衝突前速度;
vBx,i−1=5;H衝突前並進速度x方向L; vBy,i−1=0;H衝突前並進速度y方向L; ωB,i−1=70;H衝突前回転速度L;
J 行列の導出 ;
E3=i kjjjj j
1 0 0 0 1 0 0 0 1 y {zzzz z;
H=i
kjj1 0 −ryCos@θD−rxSin@θD 0 1 −rySin@θD+rxCos@θDy{zz; HT=Transpose@HD;
NN=i kjjjj j
1 0 0 0 1 0 0 0 k y {zzzz z; NNN=Inverse@NND; L=H.NNN.HT;
LL=Inverse@LD; n=J−Sin@θD
Cos@θD N; nT=Transpose@nD;
J=E3−H1+eLNNN.HT.LL.n.nT.H;
目標衝突後回転速度と衝突点の関係式;
ωB,i=H0 0 1L.J.
i k jjjjjj
vBx,i−1
vBy,i−1 ωB,i−1
y { zzzzzz
Null
::−7.5i
kjj 1.H0.−300. rxL 1.+0. rx+300. rx2+
0.H0.+0. rxL 1.+0. rx+300. rx2
y {zz+70i
kjj1+1.5 rxi
kjj 1.H0.−300. rxL 1.+0. rx+300. rx2+
0.H0.+0. rxL 1.+0. rx+300. rx2
y {zzy
{zz>>
衝突点導出 ;
ωB,i=0;H目標衝突後回転速度L;
SolveA−7.5`i
kjj 1.`H0.`−300.` rxL 1.`+0.` rx+300.` rx2+
0.`H0.`+0.` rxL 1.`+0.` rx+300.` rx2
y {zz+70i
kjj1+1.5` rxi
kjj 1.`H0.`−300.` rxL 1.`+0.` rx+300.` rx2+
0.`H0.`+0.` rxL 1.`+0.` rx+300.` rx2
y {zzy
{zz ωB,i, rxE
88rx→ −0.0275652<,8rx→0.241851<<
摩擦不感型衝突アルゴリズムを用いた打撃制御
初期値設定;
θ = πê2;
rx=rx; ry=0;
l=3.0;H対象物の長さL; e=0.5;H反発係数L;
k=l lê12;
衝突前速度;
vBx,i−1=0;H衝突前並進速度x方向L; vBy,i−1=0;H衝突前並進速度y方向L; ωB,i−1= ω;H衝突前回転速度L;
J行列の導出;
E3=i kjjjj j
1 0 0 0 1 0 0 0 1 y {zzzz z;
H=i
kjj1 0 −ryCos@θD−rxSin@θD 0 1 −rySin@θD+rxCos@θDy
{zz; HT=Transpose@HD;
NN=i kjjjj j
1 0 0 0 1 0 0 0 k y {zzzz z; NNN=Inverse@NND; L=H.NNN.HT;
LL=Inverse@LD; n=J−Sin@θD
Cos@θD N; nT=Transpose@nD;
J=E3−H1+eLNNN.HT.LL.n.nT.H;
衝突前速度と衝突後速度の関係式 ;
AA=i k jjjjjj
vBx,i−1
vBy,i−1 ωB,i−1
y { zzzzzz; BB=
i k jjjjjj
vBx,i vBy,i ωB,i
y { zzzzzz;
BB=J.AAêê MatrixForm
i
k jjjjjjjj jjjjjjj
1.5ωrxJ1.+0.H0.+0.rxL
0.rx+1.33333rx2 + 1.H1.+0.rxL 1.+0.rx+1.33333rx2N 3.ωH0.+0.rxLrx
1.+0.rx+1.33333rx2
ωJ1+1.5 rxJ1.1.+H0.−1.33333rxL
0.rx+1.33333rx2 + 0.H0.+0.rxL 1.+0.rx+1.33333rx2NN
y
{ zzzzzzzz zzzzzzz
※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※※;
衝突後速度;
ll=3.0;Hア−ムの長さL;
vBx,i=50;H目標衝突後並進速度x方向L; VV=vBx,i−HllωL;H繋ぎ的なL;
vBy,i=0;H目標衝突後並進速度y方向L; ωB,i=10;H目標衝突後回転速度L;
SolveA91.5`ωrxi
kjj 0.`H0.` rx+0.`L
1.3333333333333333` rx2+0.` rx+1.`+ 1.`H0.` rx+1.`L 1.3333333333333333` rx2+0.` rx+1.`
y
{zz VV, 3.`ωH0.` rx+0.`Lrx
1.3333333333333333` rx2+0.` rx+1.` vBy,i, ωi
kjj1.5` rxi
kjj 1.`H0.`−1.3333333333333333` rxL
1.3333333333333333` rx2+0.` rx+1.`+ 0.`H0.` rx+0.`L 1.3333333333333333` rx2+0.` rx+1.`
y {zz+1y
{zz ωB,i=,8rx,ω<E
8ω →14.2153, rx→0.42988<,8ω →21.0021, rx→ −0.634426<
実験機プログラム 制御プログラム
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{【プログラム名】:Vr̲3l̲シリーズ } {【 目 的 】:手裏剣ロボットʼ０７ } {【 作 成 日 】:2007.11.23 } {【 使 用 方 法】初号機プログラム } {【 特 記 事 項】上腕部・下腕部・手首（水平） 軌道追従プログラム } { Ziegler-Nichols の経験的方法 使用 } { 加速度センサテスト } { 手首返し機構 } {【 最終改変日 】:2007.11.23 } { } { Rotary Encoder Channel 0 Channel 1 Channel 2 Channel 3 } { teibai 4 4 4 1 } { D/A Channel 0 1 2 } { A/D Channel 0 1 } { Hosei University, College of Engineering } { -- Dec.3.2007-- } { Programed by Yohei Suzuki } {・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{$R+} {Range checking off}
{$B+} {Boolean complete evaluation on}
{$S+} {Stack checking on}
{$I+} {I/O checking on}
{$N+} {Use numeric coprocessor}
{$M 65500,16384,655360} {Turbo 3 default stack and heap}
PROGRAM RINK̲MANIPULATOR;
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{・ [使用上のご注意] ・}
{・本プログラムは鉛直面内における３リンクマニピュレータ専用です ・}
{・本プログラムを使用する際には用量、用法を守って正しくお使い下さい ・}
{・本プログラムを改造、改良、改善する際にはプログラム名を変更してください・}
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
Uses {・ 使用するユニット ・}
Crt, Graph, Dos, Printer, Acfilex, D̲DAs,D̲En,D̲ADc;
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{・ [デキストラスマニピュレータ・システム構成] ・}
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{・「DA/ボード」：QDA-2998BPC (Micro science Co.Ltd) ・}
{・ ・Ｉ/O ポートアドレス =70D0 〜 70DE ・}
{・ ・入力レンジ =-10[V] 〜 +10[V] ・}
{・ ・チャンネル０ ＝手首部 ・}
{・ ・チャンネル１ ＝上腕部 ・}
{・ ・チャンネル２ ＝下腕部 ・}
{・ ・チャンネル３ ＝把握部 ・}
{・「AD/ボード」：ADM-1698BPC (Micro science Co.Ltd) ・}
{・ ・I/o ポートアドレス =20D0 〜 20DE ・}
{・ ・入力レンジ =0[V] 〜 +5[V] ・}
{・ ・チャンネル０ ＝Ｘ軸 ・}
{・ ・チャンネル１ ＝Ｙ軸 ・}
{・「エンコーダカウンター/ボード」：UDA-4298CPC (Micro science Co.Ltd) ・}
{・ ・Ｉ/O ポートアドレス ・}
{・ ＝ボードＡ＝1AD0 〜 1ADE ・}
{・ ＝ボードＢ＝1BD0 〜 1BDE ・}
{・ ・ボードＡ＝チャンネル１＝手首部 ・}
{・ チャンネル２＝未使用 ・}
{・ ・ボードＢ＝チャンネル１＝上腕部 ・}
{・ ＝チャンネル２＝下腕部 ・}
{・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・・}
{==========================================================================}
{ 定数・変数定義 } { } {==========================================================================}
{--各関節の長さ---}
const g=9.8;
l1=0.3146;
l2=0.3000;
lg3=0.015;
m3=0.046;
sr=0.0075;
myu=0.25;
pi=3.141592;
nn=700;
var
raddo :single;
{--タイマ割り込み用変数---}
var
intvl : integer;
originalmask : byte; {待避アドレス}
tflag : boolean; {制御部実行スイッチ}
prate,trate : word;
int1bsave : pointer; {待避アドレス}
Damy : char;
type
kk6 = array [1..9] of single;
var
ITV :integer;
TV :single;
i,jj :integer;
K,kk :kk6;
k1,k2,k3,k4,k5,k6 :single;
c,ch,key :char;
Amp0,Amp1,Amp2 :single;
Dth0,Dth1,Dth2 :single;
Acc1,Acc2,Acc0 :single;
aac1,aac2,aac0 :single;
U0,U1,U2 :single;
gain0,gain1,gain2 :single;
Uout0,Uout1,Uout2,Uout3 :single;
Fa,Fb,Ke :single;
N0,N1,N2 :single;
Kt0,Kt1,Kt2 :single;
R0,R1,R2 :single;
int0,int1,int2 :single;
{---エンコーダ変数---}
OVR0,OVR1,OVR2 :integer;
Einp0,Einp1,Einp2 :single;
Eout0,Eout1,Eout2 :single;
Angle0,Angle1,Angle2 :single;
teibai0,teibai1,teibai2 :single;
Desire0,Desire1,Desire2 :single;
Desire3,Desire4,Desire5 :single;
no :integer;
swich1,swich2 :boolean;
ss0,ss1,ss2 :single;
dsdx,ds0,ds1,ds2 :single;
{--加速度センサー---}
ADv0,ADv1 :Extended;
Asx,Asy,Fx,Fy :single;
avx,avy,ax,ay,vx,vy :single;
sf,tf :single;
f1,f2,f3,f4,f5,f6,f7,f8,f9 :single;
ff1,ff2,ff3,ff4,ff5,ff6,ff7,ff8,ff9,ff10 :single;
ff11,ff12,ff13,ff14,ff15,ff16,ff17,ff18,ff19 :single;
fpx,fpy :single;
dropping, slipping :single;
{--伝達変数---}
type
store = array [1..350] of single;
kiroku= array[1..nn] of single;
var
joint0,joint1,joint2 :store;
vjoint0,vjoint1,vjoint2 :store;
ajoint0,ajoint1,ajoint2 :store;
sangle0,sangle1,sangle2 :store;
svelo0,svelo1,svelo2 :store;
time,Accel :store;
ac1,ac2,ac3,acc :store;
torqu0,torqu1,torqu2 :store;
rtime,stime,dt :single;
dend,nend,rr :integer;
gbai :single;
ax1,ax2,ax3 :single;
kasx,kasy,ssf :store;
kax,kay,Accc0,Accc1,Accc2 :store;
aax,aay :store;
{--データ保存配列の型指定---}
var
ik,ikk :integer;
{==========================================================================}
{ キー・スキャン } { } {==========================================================================}
var
code,xcode,ycode,zcode :byte;
procedure KeyScan(var code:byte);
var
regs:Registers;
begin
with regs do begin
AX:=$0600;DL:=$FF;
msdos(regs);
code:=AL;
end;
end;
{=========================================================================}
{ ファイルコマンド } { } {=========================================================================}
{--開始---}
{files command by S.Takashima Nov 20,1989}
type
tpdrv = string[2];
var
maxrec: integer;
{---}
procedure Dir(opname:tpfile;var drv:tpdrv);
var
DirInfo :SearchRec;
Flname :string[18];
code :byte;
begin
repeat KeyScan(code);if code<>0 then c:=ReadKey until code=0;
clrscr;
write('Drive ?=');readln(drv);
FindFirst(drv+opname,Archive,DirInfo);
While DosError=0 do begin
Flname:=copy(DirInfo.Name+' ',1,16);
write(Flname);
FindNext(DirInfo);
end;
writeln;
end;
{=========================================================================}
{ データの保存・読み込み } { } {=========================================================================}
{--各関節角度の実験値のデータの保存---}
procedure save;
var
filename:tpfile;
filevar:text;
ia :integer;
begin
opname:='*.for';
filein(filename,1);
assign(filevar,filename);
rewrite(filevar);
for ia:=1 to maxrec do begin
writeln(filevar,sangle1[ia]:8:3,'',sangle2[ia]-(3/5*(sangle1[ia])):8:3 ,'',sangle0[ia]:8:3,
' ',svelo1[ia]:8:3,'',svelo2[ia]:8:3,'',svelo0[ia]:8:3 ,'',accc1[ia]:8:3,'',accc2[ia]:8:3,'',accc0[ia]:8:3);
end;
close(filevar);
end;
{--加速度の実験値のデータの保存---}
procedure save1;
var
filename:tpfile;
filevar:text;
ib :integer;
begin
opname:='*.for';
filein(filename,1);
assign(filevar,filename);
rewrite(filevar);
for ib:=1 to maxrec do begin
writeln(filevar,kax[ib]:8:3,'',kay[ib]:8:3,'', kasx[ib]:8:3,'',kasy[ib]:8:3,'', aax[ib]:8:3,'',aay[ib]:8:3,'', ssf[ib]:8:3,'',accc0[ib]:8:3,'', sangle0[ib]:8:3);
end;
close(filevar);
end;
{--データの読み込み---}
procedure load;
var
filename :tpfile;
filevar :text;
ii :integer;
aaa,bbb,ccc,ddd,eee,fff,ggg,hhh,iii :single;
begin
opname:='*.vrt';
filein(filename,0);
assign(filevar,filename);
reset(filevar);
ii:=1;
while not eof(filevar) do begin
readln(filevar,aaa,bbb,ccc,ddd,eee,fff);
joint1[ii]:=aaa;
vjoint1[ii]:=bbb;
joint2[ii]:=ccc;
vjoint2[ii]:=ddd;
joint0[ii]:=eee;
vjoint0[ii]:=fff;
ii:=ii+1;
end;
maxrec:=ii-1;
writeln('maxrec=',maxrec:8);
close(filevar);
end;
{---}
{End of Files command}
{--終了---}
{=========================================================================}
{ エンコーダ・スタート } { } {=========================================================================}
procedure encoder̲start;
begin
OVR0:=0;OVR1:=0;OVR2:=0;
Ereset(Eout0,Eout1,Eout2);
Ereset(Eout0,Eout1,Eout2);
estart;
end;
{=======================================================================}
{ 各関節角度の読み込み } { } {=======================================================================}
procedure EncAngle(var Angle0,Angle1,Angle2:single);
begin
Encin(Einp0,Einp1,Einp2);
Angle0:= Einp0*teibai0;
Angle1:= Einp1*teibai1;
Angle2:= Einp2*teibai2;
end;
{======================================================================}
{ ポテンショメータの設定 } { } {======================================================================}
procedure dspangle;
var
peek :integer;
begin
clrscr;TextCursor(Nodispcursor);
if swich2 then begin textcolor(cyan);
gotoxy(2,2);write('各リンクを下の値にセットして下しゃい。');
gotoxy(2,3);write(' ==> 手 首 部 =',joint0[1]:10:5,'(rad) ',joint1[1]/raddo:10:5,'(deg)');
gotoxy(2,4);write(' ==> 上 腕 部 =',joint1[1]:10:5,'(rad) ',joint2[1]/raddo:10:5,'(deg)');
gotoxy(2,5);write(' ==> 下 腕 部 =',joint2[1]:10:5,'(rad) ',joint0[1]/raddo:10:5,'(deg)');
writeln;
textcolor(white);
end;
TextColor(yellow);
writeln;
writeln(' Ｃｈ−０：手首部の角度');
writeln(' Ｃｈ−１：上腕部の角度');
writeln(' Ｃｈ−２：下腕部の角度');
writeln(' 確認し終わったら ESC キーを押して下しゃい．');
TextColor(White);
estart;
repeat begin
KeyScan(code);
if code<>0 then xcode:=code;
EncAngle(Angle0,Angle1,Angle2);
gotoxy(5,10);
EncAngle(Angle0,Angle1,Angle2);
writeln('Ｃｈ−０(rad.)=',Angle0:8:4);
gotoxy(5,11);
writeln('Ｃｈ−０(deg.)=',Angle0*180/pi:8:4);
gotoxy(5,13);
writeln('Ｃｈ−１(rad.)=',Angle1:8:4);
gotoxy(5,14);
writeln('Ｃｈ−１(deg.)=',Angle1*180/pi:8:4);
gotoxy(5,16);
writeln('Ｃｈ−２(rad.)=',Angle2:8:4);
gotoxy(5,17);
writeln('Ｃｈ−２(deg.)=',Angle2*180/pi:8:4);
end;
until xcode=$1b ; {ESC 1}
TextCursor(DispCursor);
end;
{==========================================================================}
{ 割り込み時実行する procedure } { } {==========================================================================}
var
cct:longint;
procedure countup;
interrupt; {割り込み発生時に行なう命令の宣言}
begin
tflag:=true; {制御部実行スイッチ＝ＯＮ}
cct:=cct+1;
Dout(Uout0,Uout1,Uout2,Uout3);
port[$00] := $20; {割り込みコントローラに割り込み終了コマンド送信}
end;
{========================================================================}
{ タイマ割込み系への初期化 } { } {========================================================================}
procedure interruptset;
begin
prate := round($6000/10*intvl);
trate := prate;
{--割り込みベクタのセット---}
GetIntvec($08,int1bsave); {タイマー割込ベクタに格納されているアドレス待避}
SetIntvec($08,@countup); {タイマー割込ベクタに countup のアドレスを入れる}
{--カウンタモードのセット---}
port[$77] := $36;
port[$71] := (trate and $ff); {割り込み周期の下位 8 ビット}
port[$71] := (trate shr 8); {割り込み周期の上位 8 ビット}
{--割り込みコントローラのマスク解除---}
originalmask := port[$02];
port[$02]:= (originalmask and $fe);
end;
{========================================================================}
{ タイマ割込み系からの復元 } { } {========================================================================}
procedure interruptreset;
begin
{--マスタ側ポートの元のアドレスを復元---}
port[$02] := originalmask;
{--タイマー割込みベクタに元のアドレスを復元---}
SetIntvec($08,int1bsave);
end;
{========================================================================}
{ データ保存用変数 } { } {========================================================================}
procedure hozon;
begin
sangle1[ik]:= angle1; {angle :エンコーダからの角度}
sangle2[ik]:= angle2; {Desire:目標角度}
sangle0[ik]:= angle0;
svelo1[ik]:= Dth1;
svelo2[ik]:= Dth2;
svelo0[ik]:= Dth0;
kasx[ik]:=fpx;
kasy[ik]:=fpy;
kax[ik]:=fx;
kay[ik]:=fy;
Accc0[ik]:=Acc0;
Accc1[ik]:=Acc1;
Accc2[ik]:=Acc2;
aax[ik]:=ax;
aay[ik]:=ay;
ssf[ik]:=sf;
end;
{========================================================================}
{ グラフィック表示 } { } {========================================================================}
procedure draw;
var
gg :integer;
x1,x2,y1,y2 :integer;
bx1,bx2,by1,by2 : integer;
grandX,grandY,gbai :integer;
begin
grandX:=400;
grandY:=200;
gbai:=250;
for gg:=1 to maxrec do begin
x1:=round(grandX+l1*gbai*sin(joint1[gg]));
y1:=round(grandY+l1*gbai*cos(joint1[gg]));
x2:=round(x1+l2*gbai*sin(joint1[gg]+joint2[gg]));
y2:=round(y1+l2*gbai*cos(joint1[gg]+joint2[gg]));
setlinestyle(0,0,1);
setcolor(white); line(grandX,grandY-250,grandX,grandY+250);
line(grandX-180,grandY,grandX+180,grandY);
setlinestyle(0,0,3);
setcolor(black); moveto(grandX,grandY); lineto(bx1,by1);
setcolor(cyan); moveto(grandX,grandY); lineto(x1,y1);
setcolor(black); moveto(bx1,by1); lineto(bx2,by2);
setcolor(yellow); moveto(x1,y1); lineto(x2,y2);
bx1:=x1;by1:=y1;bx2:=x2;by2:=y2;
end;
end;
{=========================================================================}
{ 制御の実行１ （手裏剣把握） } { } {=========================================================================}
procedure prestartup;
begin clrscr;
textcursor(dispcursor);
textcolor(cyan);
writeln('手裏剣把握');writeln(' ');
TextColor(yellow);
clrscr;
writeln('手裏剣把握プログラム');
writeln('');
writeln('手裏剣をセットし「Ｓ」を押してください：');
writeln('');
writeln('終わったら「RETURN」を押してください');
TextCursor(Nodispcursor);
TextColor(White);
repeat
c:=ReadKey;
until (c='s') or (c='S');
Dout(0.0,0.0,0.0,2.0);
end;
{=========================================================================}
{ 制御の実行２ （初期角度設定） } { } {=========================================================================}
var kkkk:single;
procedure startup;
var
Bth1,Bth2,Bth0 :single;
Dangle1,Dangle2,Dangle0 :single;
xd :single;
yn :integer;
rttf,rttf2 :longint;
Bdesire1,Bdesire2,Bdesire0 :single;
c :char;
tff,tff2 :single;
label esc;
begin clrscr;
textcursor(dispcursor);
textcolor(cyan);
writeln('初期設定');writeln(' ');
{ textcolor(white);
writeln('各関節角度を入力しんゃい');
writeln('');
write('angle1=');readln(dangle1);
writeln('');
write('angle2=');readln(dangle2);
writeln('');
write('angle0=');readln(dangle0);
}
Dangle0:=joint0[1];
Dangle1:=joint1[1];
Dangle2:=joint2[1];
Desire1:=Dangle1;
Desire2:=(Dangle2)+(3/5*dangle1);
Desire0:=Dangle0;
writeln;
Bdesire1:=Desire1;
Bdesire2:=Desire2;
Bdesire0:=Desire0;
TextColor(yellow);
clrscr;
writeln('初期値設定制御実行プログラム');
writeln('');
writeln('「Ｓ」を押すとスタートすんよ：');
writeln('');
writeln('終わったら「RETURN」を押しんしゃい');
encoder̲start;
EncAngle(Angle0,Angle1,Angle2);
gotoxy(40,4);write ('angle1=',angle1/raddo:8:4);
gotoxy(40,5);write ('joint1=',dangle1:8:4);
gotoxy(40,7);write ('angle2=',angle2/raddo:8:4);
gotoxy(40,8);write ('joint2=',dangle2:8:4);
gotoxy(40,10);write ('angle0=',angle0/raddo:8:4);
gotoxy(40,11);write ('joint0=',dangle0:8:4);
TextCursor(Nodispcursor);
TextColor(White);
repeat
c:=ReadKey;
until (c='s') or (c='S');
EncAngle(Angle0,Angle1,Angle2);
Bth1:=angle1; Bth2:=angle2; Bth0:=angle0;
angle1:=Bth1; angle2:=Bth2; angle0:=Bth0;
gotoxy(40,4);write ('desired angle1 =');
gotoxy(40,5);write ('angle1 =');
gotoxy(40,7);write ('desired angle2 =');
gotoxy(40,8);write ('angle2 =');
gotoxy(40,10);write ('desired angle0 =');
gotoxy(40,11);write ('angle0 =');
jj:=0;ik:=0;
tflag:=true;
Uout1:=0;
Uout2:=0;
Uout0:=0;
intvl:=5; ITV:=intvl;TV:=1000/ITV;
cct:=0; xcode:=0;
interruptset;
ds0:=angle0;
ds1:=angle1;
ds2:=angle2;
dsdx:=0.005; ss0:=0; ss1:=0; ss2:=0;
{***Control start**************************}
repeat
if tflag then begin
KeyScan(code);
if code<>0 then xcode:=code;
EncAngle(angle0,angle1,angle2);
Dth0:=(angle0-Bth0)*TV;
Dth1:=(angle1-Bth1)*TV;
Dth2:=(angle2-Bth2)*TV;
Bth0:=angle0;
Bth1:=angle1;
Bth2:=angle2;
if abs(Desire0-ds0)<0.005{0.005}
then begin ds0:=Desire0;k[7]:=3;end else
begin ds0:=ds0+(Desire0-angle0)/abs(Desire0-angle0)*dsdx;k[7]:=0;end;
if abs(Desire1-ds1)<0.01
then begin ds1:=Desire1;k[8]:=30;end else
begin ds1:=ds1+(Desire1-angle1)/abs(Desire1-angle1)*dsdx;k[8]:=0;end;
if abs(Desire2-ds2)<0.01
then begin ds2:=Desire2;k[9]:=30;end else
begin ds2:=ds2+(Desire2-angle2)/abs(Desire2-angle2)*dsdx;k[9]:=0;end;
ss0:=ss0+(ds0-angle0)/TV*k[7];
ss1:=ss1+(ds1-angle1)/TV*k[8];
ss2:=ss2+(ds2-angle2)/TV*k[9];
U0:= ({k[5]}0.4*(ds0-angle0)+{k[6]}0.025*(-Dth0){+ss0});
U1:=({k[1]}50.0*(ds1-angle1)+{k[2]}0.9*(-Dth1){+ss1});
U2:= ({k[3]}45.0*(ds2-angle2)+{k[4]}0.5*(-Dth2){+ss2});
Uout0:=U0*R0/gain0/Kt0*N0;
Uout1:=U1*R1/gain1/Kt1*N1;
Uout2:=U2*R2/gain2/Kt2*N2;
Uout3:=2.0;
{ U0:=-(k[1]*(angle0-desire0)+k[2]*(Dth0));
U1:=-(k[3]*(angle1-desire1)+k[4]*(Dth1));
U2:=-(k[5]*(angle2-desire2)+k[6]*(Dth2));
Uout0:=U0*R0/gain0/Kt0*N0;
Uout1:=U1*R1/gain1/Kt1*N1;
Uout2:=U2*R2/gain2/Kt2*N2;
}
if abs(U1)>10 then begin
gotoxy(40,21);write('U1=',u1:8:3);
end;
if abs(U2)>10 then begin
gotoxy(40,22);write('U2=',u2:8:3);
end;
if abs(U0)>10 then begin
gotoxy(40,23);write('U0=',u0:8:3);
end;
gotoxy(57,4);write (Desire1/raddo:8:2);
gotoxy(57,5);write (angle1/raddo:8:4);
gotoxy(57,7);write (Desire2/raddo:8:2);
gotoxy(57,8);write (angle2/raddo:8:4);
gotoxy(57,10);write (Desire0/raddo:8:2);
gotoxy(57,11);write (angle0/raddo:8:4);
gotoxy(1,12);write ('ds0=',ds0/raddo:8:2);
gotoxy(1,13);write ('ds1=',ds1/raddo:8:4);
gotoxy(1,14);write ('ds2=',ds2/raddo:8:2);
tflag:=false;
end;
until (xcode=$d) ;{...RETURN key}
esc:
interruptreset;
Desire1:=Bdesire1;
Desire2:=Bdesire2;
Desire0:=Bdesire0;
dspangle;
TextCursor(DispCursor);
end;
{====================================================================}
{ 制御の実行 } { } {====================================================================}
procedure start;
label exit1;
var
Bth1,Bth2,Bth3,Bth4 :single;
Bth0,bth5 :single;
yn,kk1,kk2 :single;
hh,ff,jj :integer;
a1,a2,a3,b1,b2,b3,c1,c2 :single;
aa1,aa2,aa3,bb1,bb2,bb3 :single;
btt1,btt2,btt3 :single;
ttime,time :single;
vv1,vv2,vv0,bv1,bv2,bv0 :single;
BBh1,BBh2,BBh0 :single;
dropping, slipping :single;
begin clrscr;
textcursor(dispcursor);
draw;
swich2:=false;
prestartup;
startup;
TextColor(yellow);
clrscr;
writeln('制御実行プログラム');
writeln('');
writeln('「Ｓ」を押すとスタートすんよ：');
writeln('');
writeln('終わったら「RETURN」を押しんゃい');
EncAngle(Angle0,Angle1,Angle2);
TextCursor(Nodispcursor);
TextColor(White);
repeat
c:=ReadKey;
until (c='s') or (c='S');
EncAngle(Angle0,Angle1,Angle2);
bth3:=angle0;
bth4:=angle1;
bth5:=angle2;
bth2:=angle2;
bth1:=angle1;
bth0:=angle0;
angle0:=bth0;
angle1:=bth1;
angle2:=bth2;
ttime:=0.015;btt1:=0.01;btt3:=0.005;
jj:=0; ik:=1; vv1:=0; vv2:=0; vv0:=0;
hh:=1; ff:=-1; ikk:=1;
tflag:=true;
intvl:=5; ITV:=intvl; TV:=1000/ITV;
interruptset;
{***Control start**************************}
repeat
if tflag then begin
KeyScan(code);
if code<>0 then xcode:=code;
ff:=ff+1;
time:=ff*0.005;
EncAngle(angle0,angle1,angle2);
hozon;
a1:=(angle1-Bth1)*(ttime-Btt3)-(angle1-Bth4)*(ttime-Btt1);
b1:=(angle1-Bth1)*((ttime*ttime)-(Btt3*Btt3))-(angle1-Bth4)*((ttime*ttime)-(Btt1*Btt1 ));
a2:=(angle2-Bth2)*(ttime-Btt3)-(angle2-Bth5)*(ttime-Btt1);
b2:=(angle2-Bth2)*((ttime*ttime)-(Btt3*Btt3))-(angle2-Bth5)*((ttime*ttime)-(Btt1*Btt1 ));
a3:=(angle0-Bth0)*(ttime-Btt3)-(angle0-Bth3)*(ttime-Btt1);
b3:=(angle0-Bth0)*((ttime*ttime)-(Btt3*Btt3))-(angle0-Bth3)*((ttime*ttime)-(Btt1*Btt1 ));
c1:=((ttime*ttime)-(btt1*btt1))*(ttime-btt3)-((ttime*ttime)-(btt3*btt3))*(ttime-btt1)
;
c2:=(ttime-btt1)*((ttime*ttime)-(Btt3*Btt3))-(ttime-Btt3)*((ttime*ttime)-(Btt1*Btt1))
;
c1:=0.00000025;
c2:=-0.00000025;
aa1:=a1/c1;
bb1:=b1/c2;
aa2:=a2/c1;
bb2:=b2/c2;
aa3:=a3/c1;
bb3:=b3/c2;
Dth1:=aa1*2*ttime+bb1;
Dth2:=aa2*2*ttime+bb2;
Dth0:=aa3*2*ttime+bb3;
Acc1:=aa1*2;
Acc2:=aa2*2;
Acc0:=aa3*2;
{ ff19:=ff18;
ff18:=ff17;
ff17:=ff16;
ff16:=ff15;
ff15:=ff14;
ff14:=ff13;
ff13:=ff12;
ff12:=ff11;
ff11:=ff10;
ff10:=ff9;
ff9:=ff8;
ff8:=ff7;
ff7:=ff6;
ff6:=ff5;
ff5:=ff4;
ff4:=ff3;
ff3:=ff2;
ff2:=ff1;
ff1:=Acc0;
Acc0:=aa3*2;
if abs(ff19-Acc0)>=0 then
Acc0:=(ff19+ff18+ff17+ff16+ff15+ff14+ff13+ff12+ff11+ff10+
ff9+ff8+ff7+ff6+ff5+ff4+ff3+ff2+ff1+Acc0)/20;
}
bth3:=bth0;
Bth4:=Bth1;
Bth5:=Bth2;
Bth1:=angle1;
Bth2:=angle2;
bth0:=angle0;
if (ff mod 2=0) and (hh<maxrec) then begin