簡易リンクシミュレータ なるものをちょいとこしらえました。
データの打ち込みがめんどくさいですけど、 そこそこ 使えるかも（？
#define POS_X 0
#define POS_Y 1
#define LINK_LEN_1 2
#define LINK_LEN_2 3
#define SPEED 8
#define CENTER_X 2
#define CENTER_Y 3
#define LINK_1 0
#define LINK_2 1
#define MODE 3
#define FREE 0
#define PIN 1
#define MOTOR 2
#define MOTOR_R 3
#define END_DATA 2
#define PEN 7
#define ON_ 1
#define OFF_ 0
#define _MODE data2(id,MODE)
#define _SPEED data1(id,SPEED)
#define _POS_X data1(id,POS_X)
#define _POS_Y data1(id,POS_Y)
#define _CENTER_X data1(id,CENTER_X)
#define _CENTER_Y data1(id,CENTER_Y)
#define _PEN data2(id,PEN)
#define _LINK_1 data2(id,LINK_1)
#define _LINK_2 data2(id,LINK_2)
#define _LINK_1_LEN data2(id,4)=1:data1(id,LINK_LEN_1)
#define _LINK_2_LEN data2(id,9)=1:data1(id,LINK_LEN_2)
#define END_OF_DATA data2(id,END_DATA)=END_DATA
goto *main
*datum
//////////////////////リンクデータ開始//////////////////////////
name="テオヤンセン リンク"
id=0
_MODE=MOTOR_R //モータを定義する
_SPEED=10.0 //スピード [DEG per TURN]
_POS_X=50.0 //モータ作業点初期位置 X
_POS_Y=25.0 //モータ作業点初期位置 Y
_CENTER_X=_POS_X-10 //モータ軸X
_CENTER_Y=_POS_Y+10 //モータ軸Y
id=1
_MODE=PIN //固定点を定義する
_POS_X=0.0 //固定点位置 X
_POS_Y=0.0 //固定点位置 Y
id=2
_MODE=FREE //自由点を定義する
_POS_X=0.0 //自由点初期位置 X
_POS_Y=50.0 //自由点初期位置 Y
_LINK_1=0 //接続される点a
_LINK_2=1 //接続される点b
id=3
_MODE=FREE //自由点を定義する
_POS_X=-50.0 //自由点初期位置 X
_POS_Y=0.0 //自由点初期位置 Y
_LINK_1=2 //接続される点a
_LINK_2=1 //接続される点b
id=4
_MODE=FREE //自由点を定義する
_POS_X=0.0 //自由点初期位置 X
_POS_Y=-75.0 //自由点初期位置 Y
_LINK_1=0 //接続される点a
_LINK_2=1 //接続される点b
id=5
_MODE=FREE //自由点を定義する
_POS_X=-40.0 //自由点初期位置 X
_POS_Y=-75.0 //自由点初期位置 Y
_LINK_1=4 //接続される点a
_LINK_2=3 //接続される点b
id=6
_MODE=FREE //自由点を定義する
_POS_X=0.0 //自由点初期位置 X
_POS_Y=-75.0-100.0 //自由点初期位置 Y
_LINK_1=4 //接続される点a
_LINK_2=5 //接続される点b
_PEN=ON_
id=6
/*
name="ごく普通のリンク"
id=0
_MODE=MOTOR_R //モータを定義する
_SPEED=10.0 //スピード [DEG per TURN]
_POS_X=0.0 //モータ作業点初期位置 X
_POS_Y=100.0 //モータ作業点初期位置 Y
_CENTER_X=0.0 //モータ軸X
_CENTER_Y=0.0 //モータ軸Y
id=1
_MODE=PIN //固定点を定義する
_POS_X=200.0 //固定点位置 X
_POS_Y=0.0 //固定点位置 Y
id=2
_MODE=FREE //自由点を定義する
_POS_X=200.0 //自由点初期位置 X
_POS_Y=100.0 //自由点初期位置 Y
_LINK_1=0 //接続される点a
_LINK_1_LEN=200.0 //点aとの距離
_LINK_2=1 //接続される点b
_LINK_2_LEN=250.0 //点bとの距離
id=3
_MODE=FREE //自由点を定義する
_POS_X=0.0 //自由点初期位置 X
_POS_Y=260.0 //自由点初期位置 Y
_LINK_1=0 //接続される点a
_LINK_1_LEN=200.0 //点aとの距離
_LINK_2=2 //接続される点b
_LINK_2_LEN=200.0 //点bとの距離
_PEN=ON_
id=3
*/
////////////////////////////////////////////////////////////////
id+
END_OF_DATA //終了
id=0
//////////////////////リンクデータ終了//////////////////////////
title "簡易リンクシミュレータ -"+name+"-"
return
#define napier 2.718281828459045235360287471352
#define once repeat
#define end_once break %c loop
#define ctype stogx(%1) (( (%1)-offsetx )*(bai)+ginfo_winx/2)
#define ctype stogy(%1) (((%1)-offsety )*(-1.0)*(bai)+ginfo_winy/2)
#define ctype pp(%1) ((%1)*(%1))
#define ctype leng(%1,%2,%3, %4) (sqrt( pp((%1)-(%3))+pp((%2)-(%4))))
#define pi 3.1415926535897932384626433832795028841971
#define mmousex ((ginfo(0)-(ginfo(4)+(ginfo(10)-ginfo(12))/2)))
#define mmousey ((ginfo(1)-(ginfo(5)+(ginfo(11)-ginfo(13))-(ginfo(10)-ginfo(12))/2)))
*hankei
id=0
repeat
if data2(id,2)=2:id=0:break
if data2(id,3)=0{
if data2(id,4)=0{
data1(id,2)=leng(data1(data2(id,0),0),data1(data2(id,0),1),data1(id,0),data1(id,1))
data2(id,4)=1
}
if data2(id,9)=0{
data1(id,3)=leng(data1(data2(id,1),0),data1(data2(id,1),1),data1(id,0),data1(id,1))
data2(id,9)=1
}
}
id+
loop
return
*mem
bai=1.0 //表示倍率
offsetx=0 //表示中心 X
offsety=0 //表示中心 Y
gsel 3
color
boxf
gsel 0
waku=1
max=16
rate=1
c=4
ddim data1,max,9
dim data2,max,10
/*
data1
x
y
r1
r2
rad
data2
ID1
ID2
flag
0 未演算
1 演算済
2 エンド
att
0 自由
1 固定
2 モータ
*/
return
*draw
mes_ =""
repeat
color 128
line -1, stogy(0.0),ginfo_winx,stogy(0.0)
line stogx(0.0), -1,stogx(0.0),ginfo_winy
if data2(id,2)=2:id=0:break
data2(id,2)=0
if data2(id,3)=2||data2(id,3)=3{
color 0,128,0
line stogx(data1(id,0)),stogy(data1(id,1)),stogx(data1(id,2)),stogy(data1(id,3))
}
if data2(id,3)=0{
color 0,0,0
s1=data2(id,0)
s2=data2(id,1)
a1=data1(s1,0)
b1=data1(s1,1)
a2=data1(s2,0)
b2=data1(s2,1)
muri_lo=0
muri_lo1=0
randomize 287465827
randomize (id+33)*91+90+rnd(354265320)
du=rnd(256)
du=rnd(256)
du=rnd(256)
aa=rnd(3)
bb=3;rnd(3)
r=(rnd(128)+128)*(aa=0||bb=0)
g=(rnd(128)+128)*(aa=1||bb=1)
b=(rnd(128)+128)*(aa=2||bb=2)
if data2(id,6)=1:muri_lo=1
if data2(id,6)=2:muri_lo=2
if data2(id,5)=1:color r,g,b:muri_lo1=1
if data2(id,5)=2:color r,g,b:muri_lo1=2
line stogx(data1(id,0)),stogy(data1(id,1)),stogx(a1),stogy(b1)
line stogx(data1(id,0)),stogy(data1(id,1)),stogx(a2),stogy(b2)
if muri_lo{
color r,g,b
if muri_lo1=1:mes_=""+mes_+"点"+id+" において長さが足りません\n"
if muri_lo1=2:mes_=""+mes_+"点"+id+" において長すぎです。\n"
circle stogx(data1(data2(id,0),0)-data1(id,2)),stogy(data1(data2(id,0),1)-data1(id,2)),stogx(data1(data2(id,0),0)+data1(id,2)),stogy(data1(data2(id,0),1)+data1(id,2)),0
circle stogx(data1(data2(id,1),0)-data1(id,3)),stogy(data1(data2(id,1),1)-data1(id,3)),stogx(data1(data2(id,1),0)+data1(id,3)),stogy(data1(data2(id,1),1)+data1(id,3)),0
}
}
color 255,255,255
circle stogx(data1(id,0))-c,stogy(data1(id,1))-c,stogx(data1(id,0))+c,stogy(data1(id,1))+c,1
color 0,0,255
circle stogx(data1(id,0))-c,stogy(data1(id,1))-c,stogx(data1(id,0))+c,stogy(data1(id,1))+c,0
if data2(id,3)=1{
color 255,255,255
circle stogx(data1(id,0))-c,stogy(data1(id,1))-c,stogx(data1(id,0))+c,stogy(data1(id,1))+c,1
color 255,128,0
circle stogx(data1(id,0))-c,stogy(data1(id,1))-c,stogx(data1(id,0))+c,stogy(data1(id,1))+c,0
}
if data2(id,3)=2||data2(id,3)=3{
color 255,255,255
circle stogx(data1(id,2))-c,stogy(data1(id,3))-c,stogx(data1(id,2))+c,stogy(data1(id,3))+c,1
color 0,128,0
circle stogx(data1(id,2))-c,stogy(data1(id,3))-c,stogx(data1(id,2))+c,stogy(data1(id,3))+c,0
}
if pen_flag=1{
gsel 3
color
boxf
gsel 0
pen_flag=0
}
if data2(id,PEN)=ON_{
gsel 3
randomize 356745267875
randomize (id+33)*91+90+rnd(654356564)
du=rnd(256)
du=rnd(256)
du=rnd(256)
du=rnd(256)
aa=rnd(3)
bb=4;rnd(3)
r=(rnd(128)+128)*(aa=0||bb=0)
g=(rnd(128)+128)*(aa=1||bb=1)
b=(rnd(128)+128)*(aa=2||bb=2)
color r,g,b
if data2(id,8)=0:data2(id,8)=1: data1(id,6)=data1(id,0):data1(id,7)=data1(id,1)
line stogx(data1(id,6)),stogy(data1(id,7)),stogx(data1(id,0)),stogy(data1(id,1))
data1(id,6)=data1(id,0):data1(id,7)=data1(id,1)
gsel 0
}
color 0,0,255
pos stogx(data1(id,0))+10, stogy(data1(id,1))
mes id
id+
loop
pos 0,0
gmode 2,800,600
gcopy 3,0,0,800,600
color 255,0,0
pos waku+5,waku+5
mes mes_
pos ginfo_winx-120,5
mes "倍率"+bai
mes_ =""
color 0,0,0
if muri{
color 255,0,0
}
boxf 0,0,ginfo_winx,waku
boxf 0,0,waku,ginfo_winy
boxf 0,ginfo_winy-waku-1,ginfo_winx,ginfo_winy
boxf ginfo_winx-waku-1,0,ginfo_winx,ginfo_winx
return
*main
screen 0,800,600
buffer 3,800,600
gsel 3
color
boxf
gsel 0
onexit *exit
onclick gosub *oncli
gosub *mem
gosub *datum
gosub *hankei
id=0
rad=0.0
pos 0+3,ginfo_winy-21-3
objsize 88,21
button gosub "実行/停止",*strt_
loop_once=1
gosub *mainloop
repeat
wait 5
if shutu=1:shutu=0:break
loop
goto *strt
*mainloop
id=0
repeat
if data2(id,2)=0{
once
if data2(id,3)=2{
if data2(id,4)=0{
data2(id,4)=1
data1(id,4)=sqrt((data1(id,0)-data1(id,2))*(data1(id,0)-data1(id,2))+(data1(id,1)-data1(id,3))*(data1(id,1)-data1(id,3)))
data1(id,5)=pi+atan((data1(id,0)-data1(id,2)),(data1(id,1)-data1(id,3)))
}
r=data1(id,4)
rad=data1(id,5)
data1(id,0)=r*cos(rad)+data1(id,2)
data1(id,1)=r*sin(rad)+data1(id,3)
data1(id,5)+= pi*data1(id,8)/180.0
data2(id,2)=1
hen=1
break
}
if data2(id,3)=3{
if data2(id,4)=0{
data2(id,4)=1
data1(id,4)=sqrt((data1(id,0)-data1(id,2))*(data1(id,0)-data1(id,2))+(data1(id,1)-data1(id,3))*(data1(id,1)-data1(id,3)))
data1(id,5)=pi+atan((data1(id,0)-data1(id,2)),(data1(id,1)-data1(id,3)))
}
r=data1(id,4)
rad=data1(id,5)
data1(id,0)=r*cos(rad)+data1(id,2)
data1(id,1)=r*sin(rad)+data1(id,3)
data1(id,5)-= pi*data1(id,8)/180.0
data2(id,2)=1
hen=1
break
}
if data2(id,3)=0{
if ((((data2(data2(id,LINK_1),2)=1)|| (data2(data2(id,LINK_1),MODE))=PIN))&&(((data2(data2(id,LINK_2),2)=1)||(data2(data2(id,LINK_2),MODE))=PIN))){
s1=data2(id,LINK_1)
s2=data2(id,LINK_2)
a1=data1(s1,0)
b1=data1(s1,1)
a2=data1(s2,0)
b2=data1(s2,1)
r1=data1(id,2)
r2=data1(id,3)
l=0.0
l=leng(a1,b1,a2,b2)
rad_a=atan(sqrt(pp(2.0*r1*l)-pp(pp(r1)+pp(l)-pp(r2))),(pp(r1)+pp(l)-pp(r2)))
rad_b=atan(b2-b1,a2-a1)
nuke=0
iti=data2(id,5)
if l>=(r1+r2):muri=1:data2(id,5)=1:data2(id,6)=1:nuke=1:if iti!=1:hen=1
if abs(r1-r2)>=l:muri=1:data2(id,5)=2:data2(id,6)=2:nuke=1:if iti!=2:hen=1
if nuke=1:data2(id,2)=1:break
data2(id,5)=0
my1=sin(-rad_a+rad_b)*r1+b1
mx1=cos(-rad_a+rad_b)*r1+a1
my2=sin(rad_a+rad_b)*r1+b1
mx2=cos(rad_a+rad_b)*r1+a1
if leng(mx1,my1,data1(id,0),data1(id,1))<leng(mx2,my2,data1(id,0),data1(id,1)){
data1(id,0)=mx1
data1(id,1)=my1
}else{
data1(id,0)=mx2
data1(id,1)=my2
}
hen=1
data2(id,2)=1
break
}
}
if data2(id,3)=1{
hen=1
data2(id,2)=1
}
end_once
}
if data2(id,2)=2{
id=0
if hen=1{
hen=0:continue
}else{
color 255,255,255
boxf
color 0,0,0
gosub *draw
muri=0
redraw 1
if loop_once:loop_once=0:return
wait 5
redraw 0
mww=mousew
if (mww/120)>0{
bai*=(1.0+0.5)
pen_flag=1
offsetx=1.0*(-ginfo_winx/2+mmousex)/bai/napier+offsetx
offsety=-1.0*(-ginfo_winy/2+mmousey)/bai/napier+offsety
}
if (mww/120)<0{
bai*=(1.0/(1.0+0.5))
pen_flag=1
offsetx=-1.0*(-ginfo_winx/2+mmousex)/bai/napier+offsetx
offsety=1.0*(-ginfo_winy/2+mmousey)/bai/napier+offsety
}
getkey click_,1
if click=1&&click_once=0:click_once=1{
offsetx_=offsetx
offsety_=offsety
mou_x1=mmousex
mou_y1=mmousey
}
if click{
pen_flag=1
offsetx=-1.0*((mmousex)-mou_x1)/bai+offsetx_
offsety=1.0*((mmousey)-mou_y1)/bai+offsety_
;dialog offsetx
}
if click_=0:click_once=0:click=0{
}
continue
}
}
id+
if shutu=1:shutu=0:break
loop
goto *strt
*strt_
shutu=1
return
*strt
if switch_=0:switch_=1{
if obj_make:clrobj obj_id:obj_make=0
goto *mainloop
}
switch_=0
pos 88+3,ginfo_winy-21-3
objsize 88,21
button "初期化",*shoki
obj_id=stat
obj_make=1
repeat
wait 5
if shutu=1:shutu=0:break
loop
goto *strt
*shoki
id=0
hen=0
obj_make=0
clrobj obj_id
gosub *mem:gosub *datum:gosub *hankei
switch_=0
loop_once=1
gosub *mainloop
repeat
wait 5
if shutu=1:shutu=0:break
loop
goto *strt
*exit
end
*oncli
click=1
return
