#pragma inline /*declare that there is inline assembly */
#include <stdio.h> /*declare prototypes */
#include <math.h>
#include <bios.h>
#include <dos.h>
#include <conio.h>
#include <io.h>
#include <float.h>
#include <stdlib.h>
#include <graphics.h>


extern  void interrupt far newtimer(void); /* declare other functions */
static void interrupt (*oldtimer)(void);
void userscreen(void);
void initialize(void);
void printval(void);
void printconst(void);
void realtimeplot(void);
void print_binary_data(void);
void print_dig_out(void);
/* plotting variables */

float t_data_buffer,t_data_buffer_p,fsamp;
int skip_sample,samp_count,variable,index,sqcount,sqcount_2,maxvar;
int eoc_calls,int_calls,eoc_int;
int pt,xs,ys,shift,j,pt,ngsamp,ii,i,ih;
float xsfloat,ysfloat;
int graph_left,graph_right,graph_bottom,graph_top,graph_centre;
float yslp,xslp,maxx_data[8],plot_freq,hash,hashval,dhash;
char bb[20];
char *variable_name[8],*x_label[8],fname[20];
int gdriver,gmode,mode,xmax,ymax,graph_it,axiscolor;

float update_freq,u_freq,del_time;

 float pi,pidiv2,base_freq,deg_per_rad;
float ts,rl_time,timep;
int ad_data[10];
float ad_volts[10];
int  redraw,count,refresh,cdiv,cdiv_hi,cdiv_lo;
char a,ap;
int dig_word_out,dig_word_in;
int enc_ch_in;
int clk_num,clk_div;
int int_mask,int_mask_new,int_vector;
float clk_freq;
int enc_data_in;
long enc_data[15];
int enc_data_p,enc_change;
float enc_cal,enc_degrees[9];
float enc_d_p[9];
int dd,ad_cnt;
int lb,hb;
char *data87[94];
/* control */

float cal_constant;
float wcut,fcut,kf1,kf2;
int input_ch_number,output_ch_number;
float x,x_d,x_p,x_f,x_pf,x_df;
float cmd_des,cmd_vel,cmd_delta,cmd_final,integral_error;
int x_bias,x_int,volt_int,y_int;
int direction;
float volts,volts_p;
float ad_volts_f;
int sweep;
float sweep_dir[8],sweep_slope[8],sweep_v[8],tsweep;
float sqf;

int tpCon,tpCon_hold;
int square,ramp,tpCon_p,square_p,ramp_p,all_off,alternate_control_type;
int tpCon_p,square_p,ramp_p;
char *controller_name[3];

float pixel_per_sec,plot_time,data_hold,time_hold;
int i_ch,iii;

long t1,t2;
int rl_count;
float Delt,DCount,rl_freq;


int hb,lb,eoc_int,eoc_calls;

FILE *in,*out;

#include <c:\mq3\mq3.drv> /* include A to D driver */



void main(void)
{
reset_ad();
reset_da();

pi = 4.*atan(1.0);

/* plotting data maxima and names */
maxx_data[0] = 5;
maxx_data[1] = 5;

variable_name[0] = "Input Voltage   " ;
variable_name[1] = "Encoder Degrees " ;

x_label[0] = "   V / Div";
x_label[1] = "   V / Div";
x_label[2] = "  Deg/ Div";
variable = 0; /* initial plotting variable */
maxvar = 2; /* maximum plotting variables .. up to eight */

deg_per_rad = 180.0/pi;
pidiv2 = pi/2.0;

_fpreset(); /* reset floating point processor */

initialize(); /* initialize parameters */
userscreen(); /* print user interface screen */
printconst(); /* print constants */

/* setup clock frequency */
set_clk_freq(1,fsamp);

/* setup interrupt service routine */

int_vector = 0xd;/*select interrupt vector */
int_mask_new = 0xdf;/*mask appropriate bit */
disable(); /* diable interrupts */
oldtimer = getvect(int_vector); /*save old isr address */
setvect(int_vector,newtimer); /* setup the new isr */
int_mask = inportb(0x21);
outportb(0x21,int_mask&int_mask_new);
enable(); /* enable interrupts, at this point newtimer starts running */

/* do loop until user hits Q or q... control runs until then */
/* this is in the foreground.. isr is running always */
/* the following code is executed ONLY if there is any free processor rl_time */
/* so make sure your isr execution does not take longer*/
/* than the period of the clock you set */



do
{

a = ' ';

if(kbhit()) /* if user hit the keyboard*/
{
a = getch(); /* get the character */
flushall(); /* flush the keyboard buffer */
}

if( a == 'T')
{
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%f",&t_data_buffer);
if(t_data_buffer<0.5) t_data_buffer = 0.5; /* data buffer not smaller than 0.2 second */
ngsamp = ceil(plot_freq*t_data_buffer);
if(ngsamp > 2000) /* cannot save more than 2000 points!! */
{
t_data_buffer = 2000/plot_freq;
ngsamp = ceil(plot_freq*t_data_buffer);
}
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if( (a == 'Q')||(a == 'q') ) /* wrap up*/
{
disable();
setvect(int_vector,oldtimer);
outportb(0x21,int_mask);
enable();
nosound();
}

if(a == 'i') /* input channel number */
{
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%d",&input_ch_number);
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if( a == 's')
{
sweep_dir[0] = 1;
sweep_dir[1] = 1;
sweep_dir[2] = 1;
sweep_dir[3] = 1;
sweep_dir[4] = 1;
sweep_dir[5] = 1;
sweep_dir[6] = 1;
sweep_dir[7] = 1;
sweep_slope[0] = 1;
sweep_slope[1] = 2;
sweep_slope[2] = 3;
sweep_slope[3] = 4;
sweep_slope[4] = 5;
sweep_slope[5] = 6;
sweep_slope[6] = 7;
sweep_slope[7] = 8;
sweep = -1 * sweep;
}

if(a == 'v') /* output voltage */
{
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%f",&cmd_final);
if(cmd_final > 4.99) cmd_final = 4.99;
if(cmd_final < -4.99) cmd_final = -4.99;
sweep_v[output_ch_number] = cmd_final;
gotoxy(1,21);
printf("                                                            ");
printconst();
}


if(a == 'o') /* output channel number */
{
//sweep_v[output_ch_number] = 0;
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%d",&output_ch_number);
gotoxy(1,21);
printf("                                                            ");
printconst();
sweep_v[output_ch_number] = cmd_final;
}


if(a == 'C')
{
gotoxy(1,23);
printf("ENTER clock number(0,1,2)");
scanf("%d",&clk_num);
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if(a == 'd') /* output channel number */
{
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%x",&dig_word_out);
digout(dig_word_out);
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if(a == 'e') /* encoder channel number */
{
gotoxy(1,23);
printf("ENTER VALUE:  ");
scanf("%d",&enc_ch_in);
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if(a == 'z') /* encoder reset */
{
enc_reset(enc_ch_in);
enc_data_in = 0;
enc_data_p = 0;
}
if(a == 'V') /* select the plotting variable */
{
variable ++;
if(variable == maxvar) variable = 0;
printconst(); /* show constants */
}

if(a == 'F') /* sampling frequency */
{
gotoxy(1,23);
printf("ENTER SAMPLING FREQUENCY:  ");
scanf("%f",&fsamp);
if(fsamp<50) fsamp = 50.0;
if(fsamp>10000) fsamp = 10000.0;
ts = 1/fsamp;
kf1 = wcut*ts/(2+wcut*ts);
kf2 = (wcut*ts-2)/(wcut*ts+2);
sqcount = ceil(fsamp/sqf/2);
sqcount_2 = sqcount*2;
skip_sample = ceil(fsamp/plot_freq);
samp_count = 0;
set_clk_freq(1,fsamp);
gotoxy(1,21);
printf("                                                            ");
printconst();
}

if(a == 'R') /* realtime plotting */
{
realtimeplot();
}


if((rl_time-timep)>del_time) /* frequency of printing realtime data to screen */
{

t1 = biostime(0,0);

if( (t1-t2) > 50)
{
DCount = rl_count;
Delt= (t1-t2)/18.2;
rl_freq = DCount/Delt;
rl_count = 0;
t2 = t1;
}


update_freq = 1./(rl_time-timep);
timep = rl_time;
printval();
nosound();
}

} while ((a!= 'Q')&&(a!='q'));

/*wrap up */
textcolor(LIGHTGRAY);
clrscr();
printf("Quanser Consulting \n");
nosound();

}/*end main*/

void initialize(void)
{

t1 = 10;
t2 = 100;

enc_reset(0);
enc_reset(1);
enc_reset(2);
enc_reset(3);
enc_reset(4);
enc_reset(5);
enc_reset(6);
enc_reset(7);

enc_preload(0,100);
enc_preload(1,(unsigned long)10000);
enc_preload(2,(unsigned long)20000);
enc_preload(3,(unsigned long)30000);
enc_preload(4,(unsigned long)40000);
enc_preload(5,(unsigned long)50000);
enc_preload(6,(unsigned long)60000);
enc_preload(7,(unsigned long)70000);

/* sampling frequency */
fsamp = 100.0;
ts = 1/fsamp;

sweep_v[0] = 1;
sweep_v[1] = 2;
sweep_v[2] = 3;
sweep_v[3] = 4;
sweep_v[4] = -1;
sweep_v[5] = -2;
sweep_v[6] = -3;
sweep_v[7] = -4;



/* setup  printval() frequency */
u_freq = 50; /* refresh realtime data on screen at 5 Hz */
del_time = 1./u_freq;

/* plotting */
plot_freq = 50.0;
skip_sample = ceil(fsamp/plot_freq);
samp_count = 0;

sweep = -1;

t_data_buffer = 1.0; /* duration of circular data buffer in seconds*/
t_data_buffer_p = t_data_buffer;
ngsamp = ceil(plot_freq*t_data_buffer); /* number of samples in one sweep or data buffer*/

/*sensor calibration constant, full range 352 degrees maps to 12 bits */
cal_constant = 80.0/4095.00;
enc_cal = 360.0/(1024.*4.);
x_bias = 2048; /* assume zero bias */

/*  other parameters */
all_off = 1; /* start with motor off */
tpCon = 1; /* controlller type 1 */
alternate_control_type = -1; /* do not alternate controller */
cmd_vel = 30.; /* ramp velocity */
cmd_final = 0; /* commanded set point */
square = 1; /* start with step inputs */
ramp = -1; /* not ramp */
rl_time = 0; /* initialize real rl_time */
count = 0; /* initialize counters */
timep = 0.1;


/* alternating command frequency */

sqf = 0.5;
sqcount = ceil(fsamp/sqf/2);
sqcount_2 = sqcount*2;
cmd_delta = cmd_vel*ts;

/* screen graphics */

/* initialize graphics */

axiscolor = LIGHTBLUE;
detectgraph(&gdriver,&gmode);
initgraph(&gdriver,&gmode,"");
gmode = getgraphmode();
cleardevice();
xmax = getmaxx(); /* screen size */
ymax = getmaxy();
closegraph();

/* set graph size relative to screen size */

graph_left =  2*ceil(xmax/20.);
graph_right = 18*ceil(xmax/20.);
graph_top = ceil(ymax/10.0);
graph_bottom = graph_top*7;
graph_centre = ceil((graph_bottom-graph_top)/2.)+graph_top;
}



void userscreen(void)
{
gotoxy(1,1);
printf("    REAL TIME DATA");
gotoxy(1,2);
printf("REAL TIME(sec)");
gotoxy(1,3);
printf("Sampling Freq ");

gotoxy(1,15);
printf("Digital Output");
gotoxy(1,14);
printf("Digital Input ");


gotoxy(30,1);
printf("     SELECT ONE OF THE FOLLOWING:");
gotoxy(30,2);
printf("[i]: A/D input channel number " );
gotoxy(30,3);
printf("[o]: D/A output channel number");
gotoxy(30,4);
printf("[v]: output volts          ");
gotoxy(30,5);
printf("[d]: Digital word out      ");
gotoxy(30,6);
printf("[e]: Encoder channel in    ");
gotoxy(30,7);
printf("[z]: Encoder reset         ");
gotoxy(30,8);
printf("[F]: Sampling Frequency(Hz)" );
gotoxy(30,9);
printf("[T]: Data buffer duration(Sec)" );
gotoxy(30,10);
printf("[V]: Plotting variable:");
gotoxy(30,11);
printf("[R]: Realtime plot");
gotoxy(30,12);
printf("[Q]: Quit program");
}

void print_binary_data(void)
{
int i,one,xpos;
one = 0x0001;
xpos = 25;

for(i=1;i<=8;i++)
{
gotoxy(xpos-i,14);
if( (one&dig_word_in) == one)
{
textcolor(LIGHTBLUE);
cprintf("1");
}
else
{
textcolor(YELLOW);
cprintf("0");
}

one = one<<1;
}

}


void print_dig_out(void)
{
int i,one,xpos;
one = 0x0001;
xpos = 25;

for(i=1;i<=8;i++)
{
gotoxy(xpos-i,15);
if( (one&dig_word_out) == one)
{
textcolor(LIGHTBLUE);
cprintf("1");
}
else
{
textcolor(YELLOW);
cprintf("0");
}

one = one<<1;
}

textcolor(YELLOW);
}

void printconst(void)
{
textcolor(LIGHTBLUE);
gotoxy(65,2);
cprintf("%6d",input_ch_number);
gotoxy(65,3);
cprintf("%6d",output_ch_number);
gotoxy(65,4);
cprintf("%6.2f",volts);
gotoxy(65,5);
cprintf("%6x",dig_word_out);
gotoxy(65,6);
cprintf("%6d",enc_ch_in);
gotoxy(65,8);
cprintf("%6.2f",fsamp);
gotoxy(65,9);
cprintf("%6.2f",t_data_buffer);
gotoxy(52,10);
textcolor(variable+2);
cprintf("%20s",variable_name[variable]);
}

void printval(void)
{
textcolor (RED);
gotoxy(17,2);
cprintf("%6.1f",rl_time,rl_freq);
gotoxy(17,3);
cprintf("%6.0f  ",rl_freq);

print_binary_data();
print_dig_out();

gotoxy(1,16);
printf("D/A Volts");
gotoxy(8,16);
for(i_ch = 0;i_ch <8;i_ch++)
{
cprintf("%7.1f  " ,sweep_v[i_ch]);
}


gotoxy(1,17);
printf("A/D Volts");
gotoxy(8,17);
for(i_ch = 0;i_ch <8;i_ch++)
{
cprintf("%7.1f  " ,ad_volts[i_ch]);
}

gotoxy(1,18);
printf("A/D INT  ");
gotoxy(8,18);
for(i_ch = 0;i_ch <8;i_ch++)
{
cprintf("%7x  " ,ad_data[i_ch]);
}




gotoxy(1,19);
printf("Enc Deg");
gotoxy(8,19);
for(i_ch = 0;i_ch <8;i_ch++)
{
cprintf("%7.2f  " ,enc_degrees[i_ch]);
}

gotoxy(1,20);
printf("Enc Hex");
gotoxy(8,20);
for(i_ch = 0;i_ch <8;i_ch++)
{
//textcolor(i_ch+1);
cprintf("%9lx" ,enc_data[i_ch]);
}

gotoxy(1,21);
printf("Enc Int");
gotoxy(8,21);
for(i_ch = 0;i_ch <8;i_ch++)
{
//textcolor(i_ch+1);
cprintf("%9ld" ,enc_data[i_ch]);
}


}

extern void interrupt far newtimer(void)
{

asm fsave data87
_clear87();
disable();
/*outportb(0x21,int_mask);*/
int_calls ++;
rl_count++;
plot_time += ts;
rl_time= rl_time+ts;
count++;


/* digital port */

dig_word_in = digin();

digout(dig_word_out);


/* encoder */

for(iii = 0; iii < 8; iii++)
{
enc_data[iii] = enc_in(iii);
enc_degrees[iii] = enc_data[iii]*enc_cal;
}

/* A/D measurement */
for(iii = 0; iii<8; iii++)
{
ad_data[iii] = adin(iii);
ad_volts[iii] = itov(ad_data[iii]);
}


/* output to d to a */

for(iii = 0;iii<8;iii++)
{
if(sweep > 0 )
{
sweep_v[iii]= 4.95*sin(2*3.14*sweep_slope[iii]*rl_time);
}
volt_int = vtoi(sweep_v[iii]);
daout(iii,volt_int);
}

asm frstor data87
outportb(0x20,0x65);/* acknowldges interrupt to 8259 */
/*outportb(0x21,int_mask&0xdf);*/
enable();
}

void realtimeplot(void)
{
flushall();
initgraph(&gdriver,&gmode,"");
cleardevice();
pixel_per_sec = 1.0*(graph_right-graph_left) / t_data_buffer; ;

plot_time = t_data_buffer+.1;

while(a != 'x')
{

if(plot_time > t_data_buffer)
{
if(maxx_data[variable] <.1) maxx_data[variable] = .1;
yslp = 1.*(graph_bottom-graph_top)/(-2*maxx_data[variable]);
cleardevice();
setcolor(axiscolor);
rectangle(graph_left,graph_top,graph_right,graph_bottom);
xs = graph_left;
xsfloat = xs;
ys = graph_bottom;
hash = (graph_right-graph_left)/20.;
moveto(xs,ys);
setcolor(axiscolor);
for (ih=1;ih<=20;ih++)
{
lineto(xs,graph_bottom+10);
xsfloat+=hash;
xs = ceil(xsfloat);
moveto(xs,ys);
}
moveto(graph_right,graph_bottom);
lineto(graph_right,graph_bottom+10);

moveto(graph_right,graph_bottom+20);
gcvt(t_data_buffer,2,bb);
outtext(bb);
moveto(graph_right-20,graph_bottom+30);
outtext(" Seconds");

moveto(graph_left,graph_bottom+60);
setcolor(WHITE);
outtext("Hit x to return to menu");

xs = graph_left;
ys = graph_bottom;
ysfloat = ys;
hash = (graph_bottom-graph_top)/10.;
hashval = -maxx_data[variable];
dhash = maxx_data[variable]/5.0;
moveto(xs,ys);
for (ih=1;ih<=11;ih++)
{
moveto(0,ys);
if(fabs(hashval)<.0001) hashval = 0.;
gcvt(hashval,2,bb);
setcolor(axiscolor);
outtext(bb);
setcolor(axiscolor);
moveto(xs,ys);
if(ih != 11 )lineto(graph_left-10,ys);
ysfloat-=hash;
ys= ceil(ysfloat);
moveto(xs,ys);
hashval = hashval+dhash;
}
moveto(graph_left,graph_top);
lineto(graph_left-10,graph_top);

setcolor(RED);
moveto(graph_left,graph_bottom+20);
outtext("Time Axis: ");
gcvt(t_data_buffer/20.0,2,bb);
outtext(bb);
outtext("  sec/div");
moveto(graph_left,graph_bottom+40);

setcolor(axiscolor);
outtext("Y  Axis : ");
gcvt(maxx_data[variable]/10,2,bb);
outtext(bb);
outtext(x_label[variable]);

plot_time = 0;
time_hold = plot_time;
tpCon_hold = tpCon;
if(variable == 0) data_hold = ad_volts[input_ch_number];
if(variable == 1) data_hold = enc_degrees[enc_ch_in];
setcolor(tpCon_hold+1);
xs =  ceil(graph_left+time_hold*pixel_per_sec);
ys =  ceil(data_hold*yslp)+graph_centre;
moveto(xs,ys);
maxx_data[variable] = 0;
}
else
{
time_hold = plot_time;
tpCon_hold = tpCon;
/*disable();*/
if(variable == 0) data_hold = ad_volts[input_ch_number];
if(variable == 1) data_hold = enc_degrees[enc_ch_in];
/*enable();*/
if(fabs(data_hold) > maxx_data[variable]) maxx_data[variable] = fabs(data_hold);
setcolor(tpCon_hold+1);
xs =  ceil(graph_left+time_hold*pixel_per_sec);
ys =  ceil(data_hold*yslp)+graph_centre;
lineto(xs,ys);
}

if(kbhit())
a=getch();

} /* a not x */

closegraph();
restorecrtmode();
userscreen();
printconst();
a = ' ';
flushall();

}