フォークシミュレーションプログラム

とりあえずできた。

#include<iostream>
#include<math.h>
#include<stdio.h>
#define     PI      3.141592653589793238463
#define		G		 9.80665
using namespace std;

const double rigid=1000/1e-4; //0.1mmで100kg重
const double S=PI*1.5e-2*1.5e-2; //直径3cmのピストン
const double L0=10e-2;//ストローク長10cm
const double P0=4100e2/60*66; //60psi=4100HPa
const double Patm=1024e2;//1atm
const double g=100;//1m/sの速度の時10kg重のダンピング、結構適当
const double dT=1e-3;//1msごとに計算
const double m1=2;
const double m2=40;
const double velocity=20e3/3600; //20km/s
double Fs(double l, double l_){
	if (l<=0) {
		return(rigid*l);
	}else if (l>L0) {
		return(rigid*(l-L0));
	}else{
		double P=P0*L0/(L0-l);
		double tmp=(P-Patm)*S+g*(l-l_)/dT;
		return(tmp);
	}
}

double y(double t){
	double x=velocity*t;
	double period=0.4;
	double height=0.03;
//	double tmp=height*(sin(2.0*PI*x/period)*sin(2.0*PI*x/period));
	tmp=0.0;
	return(tmp);
}
		
void main(void){
	long int it;
	int i;
	double y1[3];
	double y2[3];
	for(i=0;i<=2;i++) y1[i]=0.0;
	for(i=0;i<=2;i++) y2[i]=L0;

	for(it=0;it*dT<=10;it++){
		double l=L0-(y2[1]-y1[1]);
		double l_=L0-(y2[0]-y1[0]);
		double Fe=m1*G+Fs(l,l_)+m1*(y((i+1)*dT)+y((i-1)*dT)-2.0*y((i)*dT))/dT/dT;
		if(y1[1]>y(i*dT)) Fe=0.0;
	
		y1[2]=2.0*y1[1]-y1[0]+(-G-Fs(l,l_)/m1+Fe/m1)*dT*dT;
		y2[2]=2.0*y2[1]-y2[0]+(-G+Fs(l,l_)/m2)*dT*dT;

		if (it%10==0)
			cout <<it*dT<<" "<<y2[1]<<" "<<" "<<(-G+Fs(l,l_)/m2)<<" "<<l<<" "<<(l-l_)/dT<<"\n";
		y1[0]=y1[1];
		y1[1]=y1[2];
		y2[0]=y2[1];
		y2[1]=y2[2];

	}
}