/* LAB2.C */
/* DC motor with proportional feedback */

#include <stdlib.h>
#include <stdio.h>
#include <dos.h>
#include <conio.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 */
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();
	freq=500.; clock=0;
	printf("Enter kp:\n"); 
	scanf("%f",&kp); 
	printf("Enter reference position in degrees, 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.);write_data(); }

/* Periodic task I/O and proportional feedback */
void user_task () {
	r=keybin*twopi/360.;  /* Read input reference & convert to rad */
	y=get_encoder();      /* Get position measurement in rad       */
	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;} }