/* LAB3.C */
/* DC motor with proportional control and sin. input */

#include <stdlib.h>
#include <stdio.h>
#include <dos.h>
#include <conio.h>
#include <math.h>
#include <timer.h>
#include <inout.h>

/* Declaration of user variables and constants */
#define ampgain 5.        /* Amplifier gain */
#define maxpoints 4000    /* Number of points to be stored on disk */
#define twopi 6.2831852   /* 2 pi */
#define tablelength 2000   /* Length of sin table */
float sintable[tablelength],flindex,mag,om,angle,phase;
int index,ncycles;
FILE *fp; char fname[]="DATA.M";
float ref[maxpoints],vol[maxpoints],pos[maxpoints];
float keybin,y,r,e,u,uda,kp,kd,ki,kdd,kid,em,dst,ist;
int clock,npoints;

/* Keyboard routine */
void user_interface() {
	scanf("%f",&keybin); }

/* Initialization routine */
void user_init() {
	put_da(0,0.); encoder_init();
	for (index=0;index<tablelength;index++) {
		angle=(float)(index)*twopi/(float)(tablelength);
		sintable[index]=sin(angle);}
	angle=0.;freq=500.; clock=0;
	printf("Enter kp:\n"); 
	scanf("%f",&kp); 
	printf("Enter the magnitude of the reference input in degrees:\n"); 
	scanf("%f",&mag);
	mag=mag*twopi/360.;
	printf("Enter the phase in degrees:\n"); 
	scanf("%f",&phase);
	angle=phase/360.;    /* Scale the phase so that 360 deg. = 1 */
	printf("Enter the frequency in Hz, type Ctrl-Break to stop:\n");
	scanf("%f",&keybin); }

/* Data storage on disk -- File readable by MATLAB, simply typing "data" */
void write_data() {
	npoints=clock;
	fp=fopen(fname,"w");
	fprintf(fp,"t=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",clock/freq);
	fprintf(fp,"];\n"); 
	fprintf(fp,"ref=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",ref[clock]);
	fprintf(fp,"];\n"); 
	fprintf(fp,"vol=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",vol[clock]);
	fprintf(fp,"];\n");
	fprintf(fp,"pos=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",pos[clock]);
	fprintf(fp,"];\n"); }

/* Termination routines */
void user_terminate() {}
void user_abort() {
	put_da(0,0.);put_da(1,0.);write_data(); }

/* Periodic task I/O and PID control */
void user_task () {
	om=keybin;        /* Read input reference in Hz */
	y=get_encoder();  /* Get position measurement in rad */
	angle=angle+om/freq;
	ncycles=(int)(angle);
	if (angle<0) {ncycles=ncycles-1;}
	angle=angle-(float)(ncycles);
	flindex=angle*tablelength;
	index=(int)(flindex);
	if (index<0) {index=0;}
	if (index>tablelength-1) {index=tablelength-1;}
	r=mag*sintable[index];
	e=r-y; u=kp*e;
	uda=u/ampgain; put_da(0,uda);
	if (clock<maxpoints) {      /* Data storage in arrays */
		ref[clock]=r;vol[clock]=u,pos[clock]=y;;++clock;} }