C code for implementation of a controller for antenna positioning in Airborne SatCom Terminal.

antenna_position_controller

float kp,ki,t,wf,K,K1,kdrive,kpc,kant,kpp,kip,zc,pc;
float m,m1,m2,m3,m4,m5,m6,m7,m8,m9;
float A1,B1,A2,B2,A3,B3,A4,B4,as,eb;
int i;

float pi(float kp,float ki,float t,float x,float y,float r)
{
	float op1,op2,a,b;
	int d;
	a=(2*kp)+(ki*t);
	b=(2*kp)-(ki*t);
	d=2;
	op1=(2*r)+(a*y)-(b*x);
	op2=(op1)/d;
	return(op2);
}

float plant1(float A,float B,float t,float x,float y,float r)
{
	float op1,op2,a,b;
	a=((2*A)+(B*t));
	b=((2*A)-(B*t));
	op1=(b*r)+(t*y)+(t*x);
	op2=(op1)/a;
	return(op2);
}

float integral(float t,float x,float y,float r)
{
	float op1,op2;
	op1=(r)+(t*y)+(t*x);
	op2=(op1)/2;
	return(op2);
}

float lead(float zc,float pc,float t,float x,float y,float r)
{
	float op1,op2,a,b,c,d;
	a=2-(t*zc);
	b=2+(t*zc);
	c=2-(t*pc);
	d=2+(t*pc);
	op1=(d*r)+(a*y)-(b*x);
	op2=(op1)/c;
	return(op2);
}

struct inputs
{
	float input;
	float err;
	float pastip;
	float presip;
	float pastop;
}speed, current, volt, torque,theta,drive,theta1,ant,theta2,pos,pos1;

main(void)
{
	kp=0.01;
	ki=3;
	kpc=1000;
	kdrive=212.2;
	kant=8.7e3;
	t=0.0000035;
	K=0.0407;
	K1=0.04068;
	kpp=170;
	kip=10;
	zc=45.5;
	pc=45.75;
	
	i=0;
	eb=0;
	m1=0;//m1 is feedback current
	drive.input=0;//tload
	m2=0;
	m5=0;
	
	speed.pastip=0;
	speed.pastop=0;
	volt.pastip=0;
	volt.pastop=0;
	torque.pastip=0;
	torque.pastop=0;
	theta.pastip=0;
	theta.pastop=0;
	drive.pastip=0;
	drive.pastop=0;
	theta1.pastip=0;
	theta1.pastop=0;
	ant.pastip=0;
	ant.pastop=0;
	theta2.pastip=0;
	theta2.pastop=0;
	pos.pastip=0;
	pos.pastop=0;
	pos1.pastip=0;
	pos1.pastop=0;
	
	A1=0.0024;
	A2=17.5e-7;
	B1=19.5;
	B2=0.5e-6;
	A3=0.12;
	B3=7.064;
	A4=68.7e-3;
	B4=34.226;
	
	pos.input=0.6;
	
	for(i=0;i<=2000;i++)
	{
		pos.err=pos.input-m5;
		pos.presip=pos.err;
		m8=pi(kpp,kip,t,pos.pastip,pos.presip,pos.pastop);
		pos.pastip=pos.presip;
		pos.pastop=m8;
		
		pos1.presip=(50)*(m8);
		m9=lead(zc,pc,t,pos1.pastip,pos1.presip,pos1.pastop);
		pos1.pastip=pos1.presip;
		pos1.pastop=m9;
		speed.input=m9;		
		
		speed.err=speed.input-m2;
		speed.presip=speed.err;
		m=pi(kp,ki,t,speed.pastip,speed.presip,speed.pastop);
		speed.pastip=speed.presip;
		speed.pastop=m;
		current.input=m;
		
		current.err=current.input-m1;
		volt.input=(kpc)*(current.err);
		volt.err=volt.input-eb;
		
		volt.presip=volt.err;
		m1=plant1(A1,B1,t,volt.pastip,volt.presip,volt.pastop);
		volt.pastip=volt.presip;
		volt.pastop=m1;
				
		torque.err=(K*m1)-((drive.input)/650.7);
		torque.presip=torque.err;
		m2=plant1(A2,B2,t,torque.pastip,torque.presip,torque.pastop);
		torque.pastip=torque.presip;
		torque.pastop=m2;
		
		eb=(K1)*m2;
		
		theta.presip=m2;
		m3=integral(t,theta.pastip,theta.presip,theta.pastop);
		theta.pastip=theta.presip;
		theta.pastop=m3;
		
		drive.input=(((m3)/(650.7))-m5)*kdrive;
		drive.err=drive.input-as;
		drive.presip=drive.err;
		m4=plant1(A3,B3,t,drive.pastip,drive.presip,drive.pastop);
		drive.pastip=drive.presip;
		drive.pastop=m4;
		
		theta1.presip=m4;
		m5=integral(t,theta1.pastip,theta1.presip,theta1.pastop);
		theta1.pastip=theta1.presip;
		theta1.pastop=m5;
		
		as=(m5-m7)*(kant);
		ant.presip=as;
		m6=plant1(A4,B4,t,ant.pastip,ant.presip,ant.pastop);
		ant.pastip=ant.presip;
		ant.pastop=m6;
		
		theta2.presip=m6;
		m7=integral(t,theta2.pastip,theta2.presip,theta2.pastop);
		theta2.pastip=theta2.presip;
		theta2.pastop=m7;			
	}
}