/*
 * File:         lab.c
 * Version:      1.2
 * Date:         September 10, 1997
 * Description:  Template C program for ECE410 / ECE557 real-time
 *               digital servomotor control.
 *
 *         1. Make your own directory on the C: drive
 *         2. Copy onto this directory.
 *         3. MODIFY Control and Desired subroutines.
 *         4. type 'tcc lab' to compile.
 *         5. type 'lab' to run.
 */

#include <math.h>
#include <time.h>
#include <stdio.h>
#include <dos.h>
#include <conio.h>

#define  BASE_ADDRESS  0x218   /* address of a/d in pc memory          */
#define  DATA_SIZE     1000    /* number of data samples saved to file */
#define  G             8807    /* integer representing letter G        */
#define  X             11640   /* integer representing letter X        */
#define  F             8550    /* integer representing letter F        */

/*----------------------------------------------------------------*/

/*
 * Prototypes...
 */

static void interrupt my_isr();
static void interrupt (*old_isr)();
int         ad_in(int);
void        da_out(int,int);
int         v_to_i(float);
float       i_to_v(int);
void        SetFreq(void);
void        Calibrate(void);
void        CheckCalibrate(void);
float       Control(float,float);
float       Desired(float);

/*----------------------------------------------------------------*/

/*
 * Global variables...
 */
int     ad_ch=2;
int     count=0;
float   ydata[DATA_SIZE], rdata[DATA_SIZE];
float   k0, k3;
float   freq = 250.0;

/*----------------------------------------------------------------*/

int 
main(void)

{
  int     i, key, status;
  FILE   *file_ptr;           /* file for data output */

/*
 * First, clear the D/A output to zero...
 */

  outportb(BASE_ADDRESS,0x00);    
  da_out(0,2047);

/*
 * Next, set-up the screen & prompt user for information...
 */
	
  clrscr();
  gotoxy(1,1);
  printf("                      ECE 410 / ECE 557 Digital Control Lab\n");
  printf("                           Digital Servomotor Control\n");

  Calibrate();
  CheckCalibrate();

/*
 * Next, replace the interrupt vector at 0x1c with the address of
 * the ISR that implements the controller, saving the old pointer, and
 * then set the DOS timer to generate that interrupt at the
 * correct frequency.
 */

  gotoxy(1,11);
  printf("Starting real-time controller...\n"); 
  printf("   Installing ISR...");
  disable();              /* Disable interrupts for now, */   
  old_isr=getvect(0x1c);  /* save old isr vector,        */
  setvect(0x1c,my_isr);   /* put in new isr vector,      */
  enable();               /* and re-enable interrupts.   */
  printf("done.\n");

  printf("   Initializing DOS timer to generate interrupts...");
  SetFreq();
  printf("done.\n\n");

  gotoxy(1,18);
  printf("The following keys control the experiment...\n");
  printf("   g --- to set t=0 and (re)start the experiment.\n");
  printf("   f --- to change the sample frequency.\n");
  printf("   x --- to quit and save data to a disk file.\n");
  gotoxy(1,22);

/*
 * Now sit in the loop, sampling the keyboard.  
 */

  while(1){

    key = bioskey(0);

    /*
     * If the G key is pressed, set t=0, which forces the system 
     * to restart the experiment, saving the new data...
     */

    if(key==G){
      count=0;   
    } 

    /*
     * Otherwise, if the F key is pressed, the user is promped for
     * a new sample rate.  This can be done "on the fly," at any
     * time during any experiment.
     */

    if(key==F){
      key = 0;
      gotoxy(1,15);
      printf("Enter the sampling frequency in Hz (default is 250Hz)... ");
      scanf("%f",&freq);
      printf("   New sampling frequency is...%12.2f\n",freq);
      SetFreq();
      gotoxy(1,22);
    }

    /*
     * If the X key is pressed, we quit the program by zeroing the 
     * output, un-installing the ISR, saving the data to a disk file...
     */

    if(key==X){
      key = 0;
      da_out(0,2047);         /* zero the output,                 */
      disable();              /* and "uninstall" the ISR.         */
      setvect(0x1c,old_isr);
      enable();

      file_ptr=fopen("coollab.m","w");
      for(i = 0; i < count; i++){
	fprintf(file_ptr,"%10.4f %10.4f %10.4f \n",
		(float)i/freq, rdata[i], ydata[i]);
      }
      fclose(file_ptr);
      exit(0);
    }
  }
}

/*--------------------------------------------------------------------------*/

int 
ad_in(int chn)

{
  outportb(BASE_ADDRESS + 1,chn);
  while((inportb(BASE_ADDRESS) & 0x80) == 0);
  return (inport(BASE_ADDRESS + 2));
}

/*--------------------------------------------------------------------------*/

void 
da_out(int chn, int iv)

/* 
 * Outputs the integer iv to the D/A channel number chn.
 */

{
  outport(BASE_ADDRESS+2+2*chn,iv);
}

/*--------------------------------------------------------------------------*/

float 
i_to_v(int vi)

/* 
 * Converts the integer vi, which comes in via the A/D port, to a float.
 */

{
  return(-5+(10.0/4095.0)*vi);
}

/*--------------------------------------------------------------------------*/

int 
v_to_i(float vi)

/*
 * Converts a float vi to the integer for output to the D/A.
 * The float should be between -5 and 5 to be within the linear
 * range of the D/A; floats outside this range are saturated.
 */

{
  if( (int)((5.0+vi)*4095.0/10.0) > 4095 ){       /* THIS WAS MISSING SOME () */
    return(4095);
  } else if ( (int)((5.0+vi)*4095.0/10.0) < 0 ){  /* THIS WAS MISSING SOME () */
    return(0);
  } else {
    return((int)((5.0+vi)*4095.0/10.0));          /* THIS WAS MISSING SOME () */
  }
}

/*--------------------------------------------------------------------------*/

void
SetFreq()

/*
 * Sets the DOS timer to generate interrupts at a frequency given by
 * the GLOBAL float freq, in Hz.  Since freq is used in the ISR, it
 * must be gloval.
 */

{
  int     n, lb, hb;

  n = 1.193e6 / freq;     /* Determine the word to be sent to the DOS */
  lb = n & 0x00ff;        /* timer interrupt, where freq is a float   */
  hb = (n & 0xff00) >> 8; /* determining the sample freq in Hz.       */

  outportb(0x43,0x36);    /* And then output it to the DOS timer port */
  outportb(0x40,lb);
  outportb(0x40,hb);
}

/*--------------------------------------------------------------------------*/

void 
Calibrate()

/*
 * Calibrates the A/D conversion by determining the gain k3 and offset
 * k0.  These constants satisfy yhat = k3 * y + k0, where y is the
 * shaft angle in DEGREES, and yhat is the A/D channel 0 reading,
 * which is an integer between 0 and 4095.  Thus, after reading yhat
 * from the A/D, y = (yhat - k0) / k3 gives the reading in DEGREES. 
 */

{
  float y1, y2;
  int   y1hat, y2hat;

  gotoxy(1,4);
  printf("Calibrating k3 and k0...\n");
  printf("   Enter the servomotor angle in DEGREES...");
  scanf("%f",&y1);    
  y1hat = ad_in(ad_ch);
  y2=y1;

  while(y2 == y1){
    gotoxy(1,6);
    printf("   Move the shaft, and enter the new angle in DEGREES...");
    scanf("%f",&y2);
    y2hat = ad_in(ad_ch);
  }

  k3 = ( (float)(y2hat-y1hat) / (y2-y1) );
  k0 = (float)y1hat - k3 * y1;

  printf("   Done with calibration.  k0 = %f , k3 = %f (1/degree)\n",k0, k3);
}

/*--------------------------------------------------------------------------*/

void  
CheckCalibrate(void)

/*
 * Prints to the screen the shaft angle in DEGREES, to check that the
 * constants k0 and k3 are right, and to allow the user to move the
 * shaft to the origin before the controller, implemented in the ISR
 * below, is turned-on.
 */

{
  int yhat;
  float y;
  
  gotoxy(1,8);
  printf("Move the shaft back to 0, and strike any key when ready.\n");  
  printf("   The angle is...");

  while(bioskey(1) == 0){
    yhat = ad_in(ad_ch);
    y = ((float)yhat - k0) / k3;
    gotoxy(30,9);
    printf("%12.2f",y);
  }
  bioskey(0);
}

/*--------------------------------------------------------------------------*/

static void 
interrupt my_isr()

/*
 * This is the ISR.  It is called automatically every (1/freq) seconds,
 * when the DOS timer is properly initialized.  Any variables must be
 * global.
 */

{
  int   status, yhat;
  float y, t, r, u;

  if( count < DATA_SIZE ){
    count++;
  }

  status=_status87();

  yhat = ad_in(ad_ch);                       /* Input the shaft angle,     */
  y = ((float)yhat - k0) / k3;           /* and convert it to DEGREES. */

  t = (float)count / freq;               /* Compute the time,          */
  r = Desired(t);                        /* and the desired input r(t).*/

  u = Control(r, y);                     /* Compute the control,       */
  da_out(0,v_to_i( u ));                 /* and output it via D/A.     */

  ydata[count-1] = y;                    /* Save the data to vectors,  */
  rdata[count-1] = r;                    /* for output to data file.   */

  _control87(status,0xffff);

}

/*--------------------------------------------------------------------------*/

float
Control(float r, float y)

/*
 * This subroutine computes the closed-loop control u(t), which gets
 * output through the D/A and applied to the power amplifier.  Note:
 * to implement dynamic controllers, use the static float type, so
 * the subroutine "remembers" the state.
 */

{

/* *** YOUR CONTROLLER HERE *** */

  return(0.0);

}

/*--------------------------------------------------------------------------*/

float
Desired(float t)

/*
 * This routine computes the closed-loop system input r(t) as a function
 * of time t.  It is a 50 degree step at t=1 second.
 */

{
  if ( t < 1.0 ){
    return(0.0);
  } else {
    return( 50.0 );
  }
}