/////////////////////////////////////////////////////////////////////////////// Val{ jp1_1(default=0,min=-1000000,max=1000000,step=0,disp=1) jp2_1(default=0,min=-1000000,max=1000000,step=0,disp=1) jp3_1(default=0,min=-1000000,max=1000000,step=0,disp=1) ang_1(default=0,min=-45,max=45,step=0,disp=0) ang0_1(default=-165,min=-210,max=-120,step=0,disp=0) ang1_1(default=0,min=-45,max=45,step=0,disp=0) ang2_1(default=120,min=90,max=120,step=0,disp=0) ang3_1(default=0,min=-90,max=90,step=0,disp=0) ang4_1(default=0,min=-90,max=90,step=0,disp=0) ang5_1(default=180,min=120,max=180,step=0,disp=0) ang6_1(default=180,min=120,max=180,step=0,disp=0) ang11_1(default=0,min=-15,max=15,step=2,disp=0) ang12_1(default=0,min=-15,max=15,step=2,disp=0) ang13_1(default=0,min=-15,max=15,step=2,disp=0) col_1(default=#0000FF,step=0,disp=0) beam_1(default=0,min=0,max=1000000000000,step=1000000000000,disp=0) eff_1(default=0,min=0,max=5,step=0,disp=0) key0_1 (default=-1,min=-1,max=2,step=2,disp=0) // 前進/ピッチダウン key1_1 (default=-1,min=-1,max=2,step=2,disp=0) // 後進/ピッチアップ key2_1 (default=-1,min=-1,max=2,step=2,disp=0) // 左ヨー/左ロール key3_1 (default=-1,min=-1,max=2,step=2,disp=0) // 右ヨー/右ロール key4_1 (default=-1,min=-1,max=2,step=2,disp=0) // 左スライド/左ヨー key5_1 (default=-1,min=-1,max=2,step=2,disp=0) // 右スライド/右ヨー key6_1 (default=-1,min=-1,max=2,step=2,disp=0) // 上昇/回転スロー key7_1 (default=-1,min=-1,max=2,step=2,disp=0) // 下降 key8_1 (default=-1,min=-1,max=2,step=2,disp=0) // エンジンストップ key9_1 (default=-1,min=-1,max=2,step=2,disp=0) // 射撃 key10_1(default=-1,min=-1,max=2,step=2,disp=0) // モード切替 key11_1(default=-1,min=-1,max=2,step=2,disp=0) // オートバランサー key12_1(default=-1,min=-1,max=2,step=2,disp=0) // 自動操縦 key13_1(default=-1,min=-1,max=2,step=2,disp=0) // スピードダウン key14_1(default=-1,min=-1,max=2,step=2,disp=0) // スピードアップ } /////////////////////////////////////////////////////////////////////////////// Key{ 0:key0_1 (step=2) 1:key1_1 (step=2) 2:key2_1 (step=2) 3:key3_1 (step=2) 4:key4_1 (step=2) 6:key5_1 (step=2) 5:key6_1 (step=2) 8:key7_1 (step=2) 9:key8_1 (step=2) 10:key9_1 (step=2) 12:key10_1(step=2) 7:key11_1(step=2) 13:key12_1(step=2) 14:key13_1(step=2) 16:key14_1(step=2) } /////////////////////////////////////////////////////////////////////////////// Body{ core(name=C_0,color=col_1){ s:frame(angle=165,color=col_1){ s:arm(angle=15,power=beam_1,option=40000,color=col_1){} s:chip(angle=ang0_1,color=col_1){ s:trimf(angle=90,option=1,color=col_1){ s:rudderf(angle=145,option=1,color=col_1){ s:rudder(angle=115,color=col_1){ w:chip(angle=ang12_1,color=col_1){} } } } s:chip(name=C_1,color=col_1){ n:trimf(angle=ang_1,option=1,color=col_1){ s:frame(angle=ang5_1,option=1,color=col_1){ s:jet(name=JET1_1,angle=ang6_1,power=jp1_1,effect=eff_1,color=col_1){} } } n:rudder(angle=-240,color=col_1){ n:trim(angle=ang13_1,color=col_1){} } n:rudder(angle=240,color=col_1){ n:trim(angle=ang13_1,color=col_1){} } s:rudderf(angle=ang2_1,option=1,color=col_1){ n:trimf(angle=ang3_1,option=1,color=col_1){ n:jet(name=JET2_1,angle=180,power=-jp2_1,effect=eff_1,color=col_1){} } } s:rudderf(angle=-ang2_1,option=1,color=col_1){ n:trimf(angle=ang4_1,option=1,color=col_1){ n:jet(name=JET3_1,angle=180,power=-jp3_1,effect=eff_1,color=col_1){} } } s:rudderf(angle=120,option=1,color=col_1){ n:trim(angle=ang11_1,color=col_1){} } s:rudderf(angle=-120,option=1,color=col_1){ n:trim(angle=-ang11_1,color=col_1){} } s:arm(color=col_1){} } } } } } /////////////////////////////////////////////////////////////////////////////// Lua{ F_MAX={} -- 前進最大速度[m/s] B_MAX={} -- 後進最大速度[m/s] S_MAX={} -- スライド最大速度[m/s] U_MAX={} -- 上昇最大速度[m/s] D_MAX={} -- 下降最大速度[m/s] P_MAX={} -- ピッチ最大角度[degree] R_MAX={} -- ロール最大角度[degree] Y_MAX={} -- ヨー最大角速度[degree/s] JNAME,YJNAME={},{} JPKX,JPKY,JPKZ={},{},{} JPIX,JPIY,JPIZ={},{},{} JNUM,YJNUM,YJP={},{},{} ENGINE={} EPOWER={} TSPEED={} AUTO=0 MODE=0 BAL=0 ------------------------------------------------------------------------------- -- モデルに応じて書き換えること ----------------------------------------------- JNAME[C_1]={JET1_1,JET2_1,JET3_1} YJNAME[C_1]={JET1_1,JET2_1,JET3_1} ------------------------------------------------------------------------------- F_MAX[C_1]=200/3.6 B_MAX[C_1]=50/3.6 S_MAX[C_1]=50/3.6 U_MAX[C_1]=50/3.6 D_MAX[C_1]=50/3.6 P_MAX[C_1]=15 R_MAX[C_1]=15 Y_MAX[C_1]=90 YJP[C_1]={} JPKX[C_1],JPKY[C_1],JPKZ[C_1]={},{},{} ENGINE[C_1]=0 EPOWER[C_1]=0 TSPEED[C_1]=3 -- 時刻文字列生成 ------------------------------------------------------------- function time_string(t) local hh,mm,ss ss=math.mod(t/30,60) mm=math.mod(math.floor(t/1800),60) hh=math.floor(t/108000) return string.format("%02d:%02d:%05.2f",hh,mm,ss) end -- 角度補正 ------------------------------------------------------------------- function correct_deg(a) while(a<-180) do a=a+360 end while(a>180) do a=a-360 end return a end -- ラジアン補正 --------------------------------------------------------------- function correct_rad(a) while(a<-math.pi) do a=a+math.pi*2 end while(a>math.pi) do a=a-math.pi*2 end return a end -- RGB→24bit数値化 ----------------------------------------------------------- function rgb(r,g,b) return 65536*r+256*g+b end -- 2次元回転変換 -------------------------------------------------------------- function rot2D(x,y,r) return x*math.cos(r)+y*math.sin(r),y*math.cos(r)-x*math.sin(r) end -- 3次元回転変換 -------------------------------------------------------------- function rot3D(lx,ly,lz,ax,ay,az) ly,lx=ly*math.cos(az)-lx*math.sin(az),lx*math.cos(az)+ly*math.sin(az) lz,ly=lz*math.cos(ax)-ly*math.sin(ax),ly*math.cos(ax)+lz*math.sin(ax) lz,lx=lz*math.cos(ay)-lx*math.sin(ay),lx*math.cos(ay)+lz*math.sin(ay) return lx,ly,lz end -- クォータニオン乗算 --------------------------------------------------------- function q_mul(q0,q1) local q={} q[0]=q0[0]*q1[0]-q0[1]*q1[1]-q0[2]*q1[2]-q0[3]*q1[3] q[1]=q0[0]*q1[1]+q1[0]*q0[1]+q0[2]*q1[3]-q0[3]*q1[2] q[2]=q0[0]*q1[2]+q1[0]*q0[2]+q0[3]*q1[1]-q0[1]*q1[3] q[3]=q0[0]*q1[3]+q1[0]*q0[3]+q0[1]*q1[2]-q0[2]*q1[1] return q end -- 姿勢ベクトル→クォータニオン変換 ------------------------------------------- function cal_quaternion(v) local xx,xy,xz=v[1][1],v[1][2],v[1][3] local yx,yy,yz=v[2][1],v[2][2],v[2][3] local zx,zy,zz=v[3][1],v[3][2],v[3][3] local x=math.max(0,1+xx-yy-zz)/4 local y=math.max(0,1-xx+yy-zz)/4 local z=math.max(0,1-xx-yy+zz)/4 local w=math.max(0,1-x-y-z) w,x,y,z=math.sqrt(w),math.sqrt(x),math.sqrt(y),math.sqrt(z) if(zy-yz<0) then x=-x end if(xz-zx<0) then y=-y end if(yx-xy<0) then z=-z end return w,-x,-y,-z end -- クォータニオンから角速度ベクトルを求める ----------------------------------- function cal_avel(q) local s=math.sqrt(math.max(0,1-q[1]^2)) local c=math.acos(q[1])*2 local wx,wy,wz if(s>0) then c=c/s wx,wy,wz=q[2]*c,q[3]*c,q[4]*c else wx,wy,wz=0,0,0 end return wx,wy,wz end -- オイラー角から姿勢ベクトルを求める ----------------------------------------- function cal_axis(ax,ay,az) local xx,xy,xz=rot3D(1,0,0,ax,ay,az) local yx,yy,yz=rot3D(0,1,0,ax,ay,az) local zx,zy,zz=rot3D(0,0,1,ax,ay,az) return xx,xy,xz,yx,yy,yz,zx,zy,zz end -- 姿勢ベクトルを角速度ベクトルで回転 ----------------------------------------- function rot_axis(v,w) local u={} for i=1,3 do u[i]=v[1][i]*w[1]+v[2][i]*w[2]+v[3][i]*w[3] end local r=math.sqrt(u[1]^2+u[2]^2+u[3]^2) local c=r if(r>0) then r=1/r u[1],u[2],u[3]=u[1]*r,u[2]*r,u[3]*r end c=c/2 local cc,cs=math.cos(c),math.sin(c) local qt,q0,q1,pq={},{},{},{} q0[0],q0[1],q0[2],q0[3]=cc,u[1]*cs,u[2]*cs,u[3]*cs q1[0],q1[1],q1[2],q1[3]=q0[0],-q0[1],-q0[2],-q0[3] local a={{},{},{}} for i=1,3 do pq[0],pq[1],pq[2],pq[3]=0,v[i][1],v[i][2],v[i][3] qt=q_mul(q_mul(q0,pq),q1) for j=1,3 do a[i][j]=qt[j] end end return a,u end -- ローカルベクトル→ワールドベクトル変換 ------------------------------------- function getwvec(lx,ly,lz,cn) local wx=_XX(cn)*lx+_YX(cn)*ly+_ZX(cn)*lz local wy=_XY(cn)*lx+_YY(cn)*ly+_ZY(cn)*lz local wz=_XZ(cn)*lx+_YZ(cn)*ly+_ZZ(cn)*lz return wx,wy,wz end -- ワールドベクトル→ローカルベクトル変換 ------------------------------------- function getlvec(wx,wy,wz,cn) local lx=_XX(cn)*wx+_XY(cn)*wy+_XZ(cn)*wz local ly=_YX(cn)*wx+_YY(cn)*wy+_YZ(cn)*wz local lz=_ZX(cn)*wx+_ZY(cn)*wy+_ZZ(cn)*wz return lx,ly,lz end -- TOPチップサーチ ------------------------------------------------------------ function top_search() local t,n=0,0 for i=0,_CHIPS()-1 do if(_PARENT(i)<0) then t=i n=n+1 TOPLIST[n]=t end TOP[i]=t end return n end -- グローバル変数初期化 ------------------------------------------------------- function init(cn) if(BASEH==nil) then TOP={} -- TOPチップ番号 TOPLIST={} -- TOPチップ番号リスト MASS={} -- 質量 GX0,GY0,GZ0={},{},{} -- 前フレーム重心座標 GH0={} -- 前フレーム地上高 GVX,GVY,GVZ={},{},{} -- 重心速度（ワールド座標系） GAX,GAY,GAZ={},{},{} -- 重心加速度（ワールド座標系） GVHX,GVHY,GVHZ={},{},{} -- 重心水平速度（コア座標系） GAHX,GAHY,GAHZ={},{},{} -- 重心水平加速度（コア座標系） GXX0,GXY0,GXZ0={},{},{} -- 前フレームX軸ベクトル GYX0,GYY0,GYZ0={},{},{} -- 前フレームY軸ベクトル GZX0,GZY0,GZZ0={},{},{} -- 前フレームZ軸ベクトル GWX,GWY,GWZ={},{},{} -- コア角速度 GBX,GBY,GBZ={},{},{} -- コア角加速度 GIX,GIY,GIZ={},{},{} -- 重心周りの慣性モーメント TGTX,TGTY,TGTZ={},{},{} -- 自動操縦用目標座標 BASEH=_GY(0) -- 基準高度 AUTO=0 -- 自動操縦フラグ -- ローカル慣性モーメント(INDEX=_TYPE(cn)の戻り値) -------------------- IX,IY,IZ={},{},{} IX[ 0],IY[ 0],IZ[ 0]=0.77700012922286987,1.51200044155120850,0.77700012922286987 -- CORE IX[ 1],IY[ 1],IZ[ 1]=0.77700012922286987,1.51200044155120850,0.77700012922286987 -- CHIP IX[ 2],IY[ 2],IZ[ 2]=0.77700012922286987,1.51200044155120850,0.77700012922286987 -- RUDDER IX[ 3],IY[ 3],IZ[ 3]=1.51091003417968750,0.47712945938110352,1.51091003417968750 -- RIM IX[ 4],IY[ 4],IZ[ 4]=0.85765480995178223,1.65404880046844480,0.85765480995178223 -- WHEEL IX[ 5],IY[ 5],IZ[ 5]=0.85765480995178223,1.65404880046844480,0.85765480995178223 -- RLW IX[ 6],IY[ 6],IZ[ 6]=0.77700012922286987,1.51200044155120850,0.77700012922286987 -- TRIM IX[ 7],IY[ 7],IZ[ 7]=0.85765480995178223,1.65404880046844480,0.85765480995178223 -- JET IX[ 8],IY[ 8],IZ[ 8]=3.10800051689147950,6.04800176620483400,3.10800051689147950 -- WEIGHT IX[10],IY[10],IZ[10]=1.55400025844573970,3.02400088310241700,1.55400025844573970 -- ARM IX[33],IY[33],IZ[33]=0.38062506914138794,0.75600022077560425,0.38062506914138794 -- FRAME IX[34],IY[34],IZ[34]=0.38062506914138794,0.75600022077560425,0.38062506914138794 -- RUDDERF IX[35],IY[35],IZ[35]=0.38062506914138794,0.75600022077560425,0.38062506914138794 -- TRIMF end TOPNUM=top_search() -- TOPチップ数 local i i=cn if(GX0[i]==nil) then GX0[i],GY0[i],GZ0[i]=_GX(i),_GY(i),_GZ(i) GH0[i]=2 GVX[i],GVY[i],GVZ[i]=getwvec(_VX(i),_VY(i),_VZ(i),i) GXX0[i],GXY0[i],GXZ0[i]=_XX(i),_XY(i),_XZ(i) GYX0[i],GYY0[i],GYZ0[i]=_YX(i),_YY(i),_YZ(i) GZX0[i],GZY0[i],GZZ0[i]=_ZX(i),_ZY(i),_ZZ(i) GWX[i],GWY[i],GWZ[i]=_WX(i),_WY(i),_WZ(i) TGTX[i],TGTY[i],TGTZ[i]=_GX(i),_GY(i),_GZ(i) end end -- 物理量計算 ----------------------------------------------------------------- function cal_physics(cn) local x,y,z,h=_GX(cn),_GY(cn),_GZ(cn),_H(cn) local vx,vy,vz=(x-GX0[cn])*30,(y-GY0[cn])*30,(z-GZ0[cn])*30 if(h>=0)and(h<10) then vy,GH0[cn]=math.min(vy,(h-GH0[cn])*30),h end if(GH0[cn]<1) then vy=math.min(vy,GH0[cn]-1) end GVX[cn],GAX[cn],GX0[cn]=vx,(vx-GVX[cn])*30,x GVY[cn],GAY[cn],GY0[cn]=vy,(vy-GVY[cn])*30,y GVZ[cn],GAZ[cn],GZ0[cn]=vz,(vz-GVZ[cn])*30,z GVHZ[cn],GVHX[cn]=rot2D(GVZ[cn],GVX[cn],_AY(cn)) GAHZ[cn],GAHX[cn]=rot2D(GAZ[cn],GAX[cn],_AY(cn)) GVHY[cn],GAHY[cn]=GVY[cn],GAY[cn] local axis={{getlvec(GXX0[cn],GXY0[cn],GXZ0[cn],cn)}, {getlvec(GYX0[cn],GYY0[cn],GYZ0[cn],cn)}, {getlvec(GZX0[cn],GZY0[cn],GZZ0[cn],cn)}} local q={cal_quaternion(axis)} local wx,wy,wz=cal_avel(q) wx,wy,wz=-wx*30,-wy*30,-wz*30 GWX[cn],GBX[cn],GXX0[cn],GXY0[cn],GXZ0[cn]=wx,(wx-GWX[cn])*30,_XX(cn),_XY(cn),_XZ(cn) GWY[cn],GBY[cn],GYX0[cn],GYY0[cn],GYZ0[cn]=wy,(wy-GWY[cn])*30,_YX(cn),_YY(cn),_YZ(cn) GWZ[cn],GBZ[cn],GZX0[cn],GZY0[cn],GZZ0[cn]=wz,(wz-GWZ[cn])*30,_ZX(cn),_ZY(cn),_ZZ(cn) end -- 質量計算 ------------------------------------------------------------------- function cal_mass(cn) local s=TOP[cn] local m=0 for i=s,_CHIPS()-1 do if(TOP[i]~=s) then break end m=m+_M(i) end MASS[cn]=m end -- チップ単体の慣性モーメント計算 --------------------------------------------- function cal_cmoment(cn,cc) local xx,xy,xz=getlvec(_XX(cn),_XY(cn),_XZ(cn),cc) local yx,yy,yz=getlvec(_YX(cn),_YY(cn),_YZ(cn),cc) local zx,zy,zz=getlvec(_ZX(cn),_ZY(cn),_ZZ(cn),cc) local i=_TYPE(cn) local ix=IX[i]*xx^2+IY[i]*yx^2+IZ[i]*zx^2 local iy=IX[i]*xy^2+IY[i]*yy^2+IZ[i]*zy^2 local iz=IX[i]*xz^2+IY[i]*yz^2+IZ[i]*zz^2 return ix,iy,iz end -- モデル全体の慣性モーメント計算 --------------------------------------------- function cal_moment(cn) local s=TOP[cn] local gx,gy,gz=_GX(cn),_GY(cn),_GZ(cn) local ix,iy,iz=0,0,0 local jx,jy,jz local x,y,z for i=s,_CHIPS()-1 do if(TOP[i]~=s) then break end x,y,z=_X(i)-gx,_Y(i)-gy,_Z(i)-gz x,y,z=getlvec(x,y,z,cn) x,y,z=x^2,y^2,z^2 jx,jy,jz=cal_cmoment(i,cn) ix=ix+_M(i)*(y+z)+jx iy=iy+_M(i)*(z+x)+jy iz=iz+_M(i)*(x+y)+jz end GIX[cn],GIY[cn],GIZ[cn]=ix,iy,iz end -- PID制御 -------------------------------------------------------------------- PID_I,PID_V={},{} function pid_ctrl(cn,id,v,k,l,o) if(PID_I[cn]==nil) then PID_I[cn],PID_V[cn]={},{} end if(PID_I[cn][id]==nil) then PID_I[cn][id],PID_V[cn][id]=0,0 end local p,i,d p=v*k[1] i=PID_I[cn][id]+v*k[2] i=math.min(l[2],math.max(l[1],i)) d=(v-PID_V[cn][id])*k[3] PID_I[cn][id]=i PID_V[cn][id]=v if(o~=nil) then out(o,string.format("P=%.2f, I=%.2f, D=%.2f, R=%.2f",p,i,d,p+i+d)) end return p+i+d end -- ジェットパワー係数 --------------------------------------------------------- function cal_jpk(cn) local gx,gy,gz=_GX(cn),_GY(cn),_GZ(cn) local jx,jy,jz={},{},{} local x,y,z local n=1 while(JNAME[cn][n]~=nil) do x=_X(JNAME[cn][n])-_GX(cn) y=_Y(JNAME[cn][n])-_GY(cn) z=_Z(JNAME[cn][n])-_GZ(cn) jx[n],jy[n],jz[n]=getlvec(x,y,z,cn) n=n+1 end n=n-1 JNUM[cn]=n local ax,ay,az=0,0,0 local px,py,pz=0,0,0 local mx,my,mz=0,0,0 for i=1,n do if(jx[i]>0) then px=px+jx[i] else mx=mx+jx[i] end ax=ax+math.abs(jx[i]) if(jz[i]>0) then pz=pz+jz[i] else mz=mz+jz[i] end az=az+math.abs(jz[i]) end JPIX[cn],JPIZ[cn]=1/az,1/ax for i=1,n do if(jx[i]>0) then jx[i]=2*mx/ax else jx[i]=2*px/ax end if(jz[i]>0) then jz[i]=2*mz/az else jz[i]=2*pz/az end end for i=1,n do jy[i]=math.abs(jx[i]*jz[i]) end for i=1,n do JPKX[cn][i],JPKY[cn][i],JPKZ[cn][i]=jx[i],jy[i],jz[i] end local m=1 while(YJNAME[cn][m]~=nil) do x=_X(YJNAME[cn][m])-_GX(cn) y=_Y(YJNAME[cn][m])-_GY(cn) z=_Z(YJNAME[cn][m])-_GZ(cn) jx[m],jy[m],jz[m]=getlvec(x,y,z,cn) m=m+1 end m=m-1 YJNUM[cn]=m for i=1,m do ay=ay+math.sqrt(jx[i]^2+jz[i]^2) end JPIY[cn]=1/ay for i=1,n do YJP[cn][i]=0 for j=1,m do if(JNAME[cn][i]==YJNAME[cn][j]) then YJP[cn][i]=1 break end end end end -- 高度調整 ------------------------------------------------------------------- function y_ctrl(cn) local t=-MASS[cn]*0.5 local k={20*t,t,t} local l={1000*t,-1000*t} if(KEY6_1>0) then t=math.min(U_MAX[cn],GVY[cn]+U_MAX[cn]*0.1) elseif(KEY7_1>0) then t=math.max(-D_MAX[cn],GVY[cn]-D_MAX[cn]*0.1) else t=0 end t=GVY[cn]-t local pid=pid_ctrl(cn,0,t,k,l) if(_YY(cn)<0) then pid,PID_I[cn][1],PID_V[cn][1]=0,0,0 end -- 破綻防止 local jp={} for i=1,JNUM[cn] do jp[i]=JPKY[cn][i]*pid end return jp end -- 左右速度調整 --------------------------------------------------------------- function x_ctrl(cn) local t=GIZ[cn]*JPIZ[cn] local k={100*t,t,500*t} local l={-10,10} if(KEY4_1>0) then t=(GVHX[cn]-S_MAX[cn])*R_MAX[cn]/S_MAX[cn] elseif(KEY5_1>0) then t=(GVHX[cn]+S_MAX[cn])*R_MAX[cn]/S_MAX[cn] else t=GVHX[cn]*3 end t=math.min(90,math.max(-90,t)) t=_EZ(cn)-math.rad(t) local pid=pid_ctrl(cn,1,t,k,l) local jp={} for i=1,JNUM[cn] do jp[i]=JPKX[cn][i]*pid end return jp end -- 前後速度調整 --------------------------------------------------------------- function z_ctrl(cn) local t=-GIX[cn]*JPIX[cn] local k={100*t,t,500*t} local l={-10,10} if(KEY0_1>0) then t=(GVHZ[cn]+F_MAX[cn])*P_MAX[cn]/F_MAX[cn] elseif(KEY1_1>0) then t=(GVHZ[cn]-B_MAX[cn])*P_MAX[cn]/B_MAX[cn] else t=GVHZ[cn]*3 end t=math.min(90,math.max(-90,t)) t=_AX(cn)+math.rad(t) local pid=pid_ctrl(cn,3,t,k,l) local jp={} for i=1,JNUM[cn] do jp[i]=JPKZ[cn][i]*pid end return jp end -- ヨー調整 ------------------------------------------------------------------- function yaw_ctrl(cn,yaw,jp) local yy=0 for i=1,JNUM[cn] do yy=yy+jp[i]*YJP[cn][i] end yy=math.min(1,math.max(-1,yy)) local t=-0.001*GIY[cn]*JPIY[cn]*yy local k={2*t,t,2*t} local l={-5,5} if(KEY2_1>0) then t=-Y_MAX[cn] elseif(KEY3_1>0) then t=Y_MAX[cn] else t=0 end t=math.deg(GWY[cn])-t local pid=pid_ctrl(cn,2,t,k,l) yaw=yaw+math.min(2,math.max(-2,pid-yaw)) yaw=math.min(15,math.max(-15,yaw)) return yaw end -- 自動操縦（ホバー） --------------------------------------------------------- function automatic(cn) local lx,ly,lz=TGTX[cn]-_GX(cn),TGTY[cn]-_GY(cn),TGTZ[cn]-_GZ(cn) if(_H(cn)>=0)and(_H(cn)<10) then TGTY[cn]=math.max(TGTY[cn],_GY(cn)+1-_H(cn)) end local dst=math.sqrt(lx^2+ly^2+lz^2) if(dst<10) then TGTX[cn]=math.random()*200-100 TGTY[cn]=math.random()*45+5+BASEH TGTZ[cn]=math.random()*220-110 if(_H(cn)>=0)and(_H(cn)<10) then TGTY[cn]=math.max(TGTY[cn],_GY(cn)+1-_H(cn)) end lx,ly,lz=TGTX[cn]-_GX(cn),TGTY[cn]-_GY(cn),TGTZ[cn]-_GZ(cn) end lx,ly,lz=getlvec(lx,ly,lz,cn) for i=0,8 do _G["KEY"..i.."_1"]=0 end local lr=math.deg(math.atan2(-lx,-lz)) local h=TGTY[cn]-_GY(cn) if(lz<-5)and(math.abs(lr)<60) then KEY0_1=1 end -- if(lx> 5) then KEY4_1=1 end -- if(lx<-5) then KEY5_1=1 end if(lr<-30) then KEY2_1=1 end if(lr> 30) then KEY3_1=1 end if(h>2) then KEY6_1=1 end if(h<-2) then KEY7_1=1 end ENGINE[cn]=1 _SETCOLOR(COL_1) _MOVE3D(_GX(cn),_GY(cn),_GZ(cn)) _LINE3D(TGTX[cn],TGTY[cn],TGTZ[cn]) end -- ホバー制御 ----------------------------------------------------------------- function hover(cn,yaw) local jp={} if(AUTO>0) then automatic(C_1) end if(KEY6_1>0) then ENGINE[cn]=1 elseif(KEY8_1>0) then ENGINE[cn]=0 end if(ENGINE[cn]>0) then local js={} jp=y_ctrl(cn) js=x_ctrl(cn) for i=1,JNUM[cn] do jp[i]=jp[i]+js[i] end js=z_ctrl(cn) for i=1,JNUM[cn] do jp[i]=jp[i]+js[i] end else for i=1,JNUM[cn] do jp[i]=0 end end yaw=yaw_ctrl(cn,yaw,jp) return jp,yaw end -- FBW 水平高度維持制御 ------------------------------------------------------- function h_fbw(cn,pitch,roll) local t=GIX[cn]*0.003 local k={10*t,t,300*t} local l={-0.5,0.5} t=_AX(cn)-math.atan2(-_VY(cn),-_VZ(cn))*_YY(cn) local pid=pid_ctrl(cn,10,t,k,l) pitch=pitch-math.min(5,math.max(-5,pitch-pid)) t=GIZ[cn]*0.00005 k={10*t,t,2*t} l={-10,10} t=math.deg(GWZ[cn])+math.deg(_EZ(cn))*2 local pid=pid_ctrl(cn,13,t,k,l) roll=roll-math.min(5,math.max(-5,roll-pid)) return pitch,roll end -- FBW WX制御 ----------------------------------------------------------------- function x_fbw(cn,pitch) local t=GIX[cn]*0.00005 local k={10*t,t,5*t} local l={-5,5} if(KEY0_1>0) then t=-60 elseif(KEY1_1>0) then t=60 else t=0 end if(KEY6_1>0) then t=t*2 end t=math.deg(GWX[cn])-t local pid=pid_ctrl(cn,11,t,k,l) pitch=pitch-math.min(5,math.max(-5,pitch-pid)) return pitch end -- FBW WZ制御 ----------------------------------------------------------------- function z_fbw(cn,roll) local t=GIZ[cn]*0.00005 local k={6*t,t,3*t} local l={-5,5} if(KEY2_1>0) then t=-120 elseif(KEY3_1>0) then t=120 else t=0 end if(KEY6_1>0) then t=t*2 end t=math.deg(GWZ[cn])-t local pid=pid_ctrl(cn,13,t,k,l) roll=roll-math.min(5,math.max(-5,roll-pid)) return roll end -- FBW WY制御 ----------------------------------------------------------------- function y_fbw(cn,yaw) local t=-GIY[cn]*0.00002 local k={5*t,4*t,3*t} local l={-15,15} if(KEY4_1>0) then t=-10 elseif(KEY5_1>0) then t=10 else t=0 end if(KEY6_1>0) then t=t*2 end t=math.deg(GWY[cn])-t local pid=pid_ctrl(cn,12,t,k,l) yaw=yaw-math.min(5,math.max(-5,yaw-pid)) return yaw end -- Velocity Vector ------------------------------------------------------------ function velocity_vector(cn) _SETCOLOR(COL_1) local k=0.1 local x,y,z=k*GVX[cn]+_GX(cn),k*GVY[cn]+_GY(cn),k*GVZ[cn]+_GZ(cn) local xx,xy,xz=_XX(cn)*0.5,_XY(cn)*0.5,_XZ(cn)*0.5 local yx,yy,yz=_YX(cn)*0.3,_YY(cn)*0.3,_YZ(cn)*0.3 local zx,zy,zz=_ZX(cn),_ZY(cn),_ZZ(cn) _MOVE3D(x-zx,y-zy,z-zz) _LINE3D(x+zx-xx,y+zy-xy,z+zz-xz) _LINE3D(x+zx+xx,y+zy+xy,z+zz+xz) _LINE3D(x-zx,y-zy,z-zz) _LINE3D(x+zx-yx,y+zy-yy,z+zz-yz) _LINE3D(x+zx+yx,y+zy+yy,z+zz+yz) _LINE3D(x-zx,y-zy,z-zz) end -- 飛行制御 ------------------------------------------------------------------- function plane_fbw(cn,pitch,yaw,roll) jp={} if(KEY11_1==1) then BAL=math.mod(BAL+1,2) end if(KEY6_1>0) then ENGINE[cn]=1 elseif(KEY8_1>0) then ENGINE[cn]=0 end if(KEY13_1==1) then TSPEED[cn]=math.max(1,TSPEED[cn]-1) end if(KEY14_1==1) then TSPEED[cn]=math.min(6,TSPEED[cn]+1) end if(ENGINE[cn]>0) then local t=MASS[cn] local k={10*t,8*t,6*t} local l={-10000,10000} t=TSPEED[cn]*250/9+_VZ(cn) local pid=pid_ctrl(cn,20,t,k,l) EPOWER[cn]=math.min(50000,math.max(0,pid)) jp[1]=EPOWER[cn]*JPKZ[C_1][1] jp[2]=-EPOWER[cn]*JPKZ[C_1][2] jp[3]=-EPOWER[cn]*JPKZ[C_1][3] end if(KEY0_1>0)or(KEY1_1>0)or(KEY2_1>0)or(KEY3_1>0)or(BAL==0) then pitch=x_fbw(cn,pitch) roll=z_fbw(cn,roll) else pitch,roll=h_fbw(cn,pitch,roll) end yaw=y_fbw(cn,yaw) velocity_vector(cn) return jp,pitch,yaw,roll end -- メイン --------------------------------------------------------------------- function main() local cn cn=C_1 if(_TICKS()<2) then init(cn) cal_mass(cn) cal_moment(cn) cal_jpk(cn) end cal_physics(cn) --------------------------------------------------------------------------- local jp={} if(KEY12_1==1) then AUTO=math.mod(AUTO+1,2) end if(KEY10_1==1) then MODE=math.mod(MODE+1,2) AUTO=0 end --------------------------------------------------------------------------- if(MODE>0) then -- 飛行モード jp,ANG11_1,ANG12_1,ANG13_1=plane_fbw(C_1,ANG11_1,ANG12_1,ANG13_1) ANG_1=0 ANG0_1=ANG0_1-math.min(5,math.max(-5,ANG0_1+165)) ANG1_1=ANG1_1-math.min(5,math.max(-5,ANG1_1)) ANG2_1=math.max( 90,ANG2_1-1) ANG3_1=math.max(-90,ANG3_1-3) ANG4_1=math.min( 90,ANG4_1+3) local a1,a2 if(GH0[C_1]<0.1) then a1,a2=135,160 else a1,a2=150,120 end ANG5_1=ANG5_1-math.min(1,math.max(-1,ANG5_1-a1)) ANG6_1=ANG6_1-math.min(2,math.max(-2,ANG6_1-a2)) EFF_1=1 else -- ホバーモード jp,ANG_1=hover(C_1,ANG_1) ANG0_1=ANG0_1-math.min(5,math.max(-5,ANG0_1-math.deg(_AX(C_1))+165)) ANG1_1=ANG1_1-math.min(5,math.max(-5,ANG0_1-ANG_1)) ANG2_1=math.min(120,ANG2_1+1) ANG3_1=math.min(ANG_1,ANG3_1+3) ANG4_1=math.max(ANG_1,ANG4_1-3) ANG5_1=math.min(180,ANG5_1+1) ANG6_1=math.min(180,ANG6_1+2) EFF_1=0 end if(KEY9_1>0) then BEAM_1=1000000000000 end for i=1,JNUM[C_1] do _G["JP"..i.."_1"]=jp[i] end --------------------------------------------------------------------------- cn=C_1 local sp=math.sqrt(GVX[cn]^2+GVY[cn]^2+GVZ[cn]^2)*3.6 out(0,string.format("FPS=%.2f, Chips=%d, Weight=%.2f[kg]",_FPS(),_CHIPS(),MASS[cn])) out(1,"Speed=",sp,"[km/h], X=",_GX(cn),", Y=",_GY(cn),", Z=",_GZ(cn)) if(MODE>0) then out(2,"Pitch : Up/Down, Roll : Left/Right, Yaw : Z/C") out(3,"Engine-ON : X, Engine-OFF : D") local s="" for i=1,6 do if(i==TSPEED[cn]) then s=s.."["..i.."]" else s=s.." "..i.." " end end out(4,string.format("Speed-Level : Q<%s>E",s)) out(5,"Rotate-Quickly : +X") out(6,"Shot : V, Mode : F") if(BAL~=0) then out(7,"Auto-Balancer : A [ON]") else out(7,"Auto-Balancer : A [OFF]") end else out(2,"Pitch : Up/Down, Yaw : Left/Right, Roll : Z/C") out(3,"Up/Down : X/S, Engine-OFF : D") out(4,"Shot : V, Mode : F") out(5,"Auto : G") end cn=_ZOOM(45) end }