SIMULATION OF AN AUTIPILOT SYSTEM

#include < stdio.h >
#include < conio.h >
#include < math.h >
#include < stdlib.h >

void main()
 {
     clrscr();
     float a=0.0175,angvel=0,time=0.005,head=0,k=400,i=2,d=5.656;
     float input,error,torque,angacc,h=0.005,u11,u12,u21,u22,u31,u32,u41,u42;
     int m=0;
     input= a*time;
     error=input-head;
     while(time<=5 )
   {
    torque=k*error-d*angvel;
    angacc=torque/i;
    u11=h*angvel;
    u12=h*((k/i)*error-(d/i)*angvel);
    u21=h*(angvel+0.5*u12);
    u22= h*((k/i)*(input-head+0.5*u11)-(d/i)*(angvel+0.5*u12));
    u31=h*(angvel+0.5*u22);
    u32= h*((k/i)*(input-head+0.5*u21)-(d/i)*(angvel+0.5*u22));
    u41=h*(angvel+u32);
    u42=h*((k/i)*(input-head+u31)-(d/i)*(angvel+u32));
    head=head+(u11+2*u21+2*u31+u41)/6;
    angvel=angvel+(u12+2*u22+2*u32+u42)/6;
    time=time+0.005;
    input=a*time;
    error=input-head;
    printf("%6.5f \n",error);

     }
   }