const unsigned char MSG0[41] = "ECE 761 Lab:  Digital Control           ";
const unsigned char MSG1[41] = "rev 4/15/19   JSG                       ";
const unsigned char MSG2[41] = "Sampling Rate = 50ms                    ";
const unsigned char MSG3[41] = "   REF    Speed  (rad/sec)              ";



// Global Variables

   unsigned char TIC;
   int Q, zQ;
   unsigned int COUNTER;
   unsigned int DELAY;
   unsigned int P1;
   unsigned char nP;
   int T0, T1;
   unsigned int P0;
   int DIR;



// Subroutine Declarations
#include <pic18.h>
#include        "function.h"


// Subroutines
#include        "function.c"

// Interrupt Service Routines
void interrupt IntServe(void)
{
   if (TMR2IF) {  // 1ms
      if (DELAY) DELAY -= 1;
      if (COUNTER) COUNTER -= 1;
      else COUNTER = 1000;
      if (COUNTER < 100) RA2 = 1; else RA2 = 0;
      TMR2IF = 0;
   }
   
   if (TMR0IF) {
      TMR0IF = 0;
      }

   if (INT0IF) {
      INTEDG0 = !INTEDG0;
      if (RB1 == INTEDG0) {
         Q = Q - 1;
         DIR = +1;
         RA1 = 1;
         RA0 = 0;
         }   
      else {
         Q = Q + 1;
         DIR = -1;
         RA1 = 0;
         RA0 = 1;
         }   
      INT0IF = 0;
      }   
   if (INT1IF) {
      if (INTEDG1) {
         P1 = TMR1;
         TMR1 = 0;
         TMR1 = 0;
         }
   
      INTEDG1 = !INTEDG1;
      if (RB0 == INTEDG1) Q = Q + 1;      
      else Q = Q - 1;
      INT1IF = 0;
      }   
   if (TMR1IF) {
      RA1 = 0;
      RA0 = 0;
      P1 = 0xFFFF;
      DIR = 0;
      TMR1IF = 0;
   }   
   }

 

void D2A(float V)    // -10.00V to +10.00V
{
   int DATA, X, i;
   if(V > 10) V = 10;
   if(V < -10) V = -10;

   X = V * 213;
   DATA = X + 2048;
   if (DATA > 4095) DATA = 4095;
   if (DATA < 0) DATA = 0;


   RC1 = 0;

   for (i=0; i<16; i++) {
      if (DATA & 0x8000) RC3 = 1; else RC3 = 0;
      RC2 = 1;
      DATA = DATA << 1;
      RC2 = 0;
      }
   RC1 = 1;
   }


void SCI_Float(float X)
{
   unsigned int A[5], i;
   int DATA;

   if (X < 0) {
      X = -X;
      while(!TRMT);  TXREG = '-';
   }
   else {
      while(!TRMT);  TXREG = ' ';
   }

   DATA = X * 100;
      
   for (i=0; i<5; i++)
   {
      A[i] = DATA % 10;
      DATA = DATA / 10;
   }
   while(!TRMT); TXREG = A[4] + '0';
   while(!TRMT); TXREG = A[3] + '0';
   while(!TRMT); TXREG = A[2] + '0';
   while(!TRMT); TXREG = '.';
   while(!TRMT); TXREG = A[1] + '0';
   while(!TRMT); TXREG = A[0] + '0';
   while(!TRMT); TXREG = ' ';
   }


void SCI_CRLF(void)
{
   while(!TRMT); TXREG = 13;
   while(!TRMT); TXREG = 10;
}

   
// Main Routine

void main(void)
{
   unsigned int i, j, npt;
   int  R, DATA;
   float TIME;
   float REF;
   float SPEED;
   float ANGLE;
   float U;
   float E0, E1;

   TRISA = 0;
   TRISB = 0x80;
   TRISC = 0;
   TRISD = 0;
   TRISE = 0;
   ADCON1 = 0x0F;
   
// D2A_Init();
   TRISC1 = 0;  // CS
   TRISC2 = 0;  // CLK
   TRISC3 = 0;  // DATA 
  


// Initialize Serial Port to 9600 baud
   TRISC = TRISC | 0xC0;
   TXIE = 0;
   RCIE = 1;
   BRGH = 0;
   BRG16 = 1;
   SYNC = 0;
   SPBRG = 255;
   TXSTA = 0x22;
   RCSTA = 0x90;
   PEIE = 1;

// set up Timer1 
   TMR1CS = 0;
   T1CON = 0xB1;
   TMR1ON = 1;
   TMR1IE = 1;
   TMR1IP = 1;
   PEIE = 1;

// set up Timer2 for 1ms
   T2CON = 0x4D;
   PR2 = 249;   // 40MHz
   TMR2ON = 1;
   TMR2IE = 1;
   TMR2IP = 1;
   PEIE = 1;

// Turn on INT0 interrupt
   INT0IE = 1;
   TRISB0 = 1;
   TRISB1 = 1;
   INTEDG0 = 1;
// Turn on INT1 interrupt
   INT1IE = 1;
   TRISB0 = 1;
   TRISB1 = 1;
   INTEDG1 = 1;

   T1 = 0;
   TIME = 0;

   Wait_ms(500);

// Send welcome message
   SCI_CRLF();
   SCI_CRLF();
   SCI_CRLF();
   for (i=0; i<40; i++) {
      while(!TRMT);  TXREG = MSG0[i];
   }
   SCI_CRLF();
   for (i=0; i<40; i++) {
      while(!TRMT);  TXREG = MSG1[i];
   }
   SCI_CRLF();
   for (i=0; i<40; i++) {
      while(!TRMT);  TXREG = MSG2[i];
   }
   SCI_CRLF();
   for (i=0; i<40; i++) {
      while(!TRMT);  TXREG = MSG3[i];
   }
   SCI_CRLF();

   Wait_ms(1000);
   
// turn on all interrupts
   GIE = 1;

   U = 0;
   E0 = 0;
   E1 = 0;

   while(1) {

         while(DELAY);
         DELAY = 50;  // sampling rate in ms

         if (RB7) REF = 30; else REF = -30;

         SPEED = DIR*(31415.6 / P1);

         E1 = E0;
         E0 = REF - SPEED;

         U = 0.27*E0;

//         U = U + 0.0113*E0;

//         U = U + 0.0225*E0;

//         U = U + 0.227 * ( E0 - 0.74*E1 );


         if(U>10) U = 10;
         if(U<-10) U = -10;

         D2A(U);

         SCI_Float(U);
         SCI_Float(REF);
         SCI_Float(SPEED);
         SCI_CRLF();
         }



   }