/* LAB4.C */
/* DC motor with proportional+integral control of velocity */

#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],pos[maxpoints],vel[maxpoints];
float keybin,y,r,e,u,uda,kp,kf,ki,kid,ist,oldy,om;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, ki, kf:\n"); 
	/* PI parameters */
	scanf("%f",&kp); scanf("%f",&ki); scanf("%f",&kf);
	kid=ki/freq;ist=0.;oldy=0.;
	printf("Enter reference velocity in rpm, 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,"pos=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",pos[clock]);
	fprintf(fp,"];\n");
	fprintf(fp,"vel=[\n");
	for (clock=0;clock<npoints;clock++) fprintf(fp,"%f\n",vel[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/60; /* Read input reference in rpm -> rad/s */
	y=get_encoder();   /* Get position measurement in rad      */
	om=(y-oldy)*freq;
	oldy=y;
	e=r-om; ist=ist+e;
	u=kf*kp*r-kp*om+kid*ist;       /* PI control law */
	uda=u/ampgain; put_da(0,uda);
	if (clock<maxpoints) {      /* Data storage in arrays */
		ref[clock]=r;pos[clock]=y;vel[clock]=om;++clock;} }