const unsigned char MSG0[41] = "DC Motor Control Lab: ECE 461           ";
const unsigned char MSG1[41] = "rev 11/16/17   JSG                      ";
const unsigned char MSG2[41] = "Sampling Rate = 20ms                    ";
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;
   unsigned int P0;
   int DIR;



// Subroutine Declarations
#include <pic18.h>

// Subroutines

// 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 Wait_ms(unsigned int X)
{
   unsigned int i, j;

   for (i=0; i<X; i++)
      for (j=0; j<617; j++);
   }

void D2A(float V)    // -20.00V to +20.00V
{
   int DATA;
   unsigned int i;
   if(V > 20) V = 20;
   if(V < -20) V = -20;

   DATA = 102.4*V + 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;
   float REF;
   float SPEED;
   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;

   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;
   D2A(U);
        
   while(1) {

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

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

         SPEED = DIR*(31415.6 / P1);

// Analog Control:  U = 0.1*Error
         U = 0.1*(REF - SPEED);

// Digital Control
//       E1 = E0;
//       E0 = REF - SPEED;
//       U = U + E0 * 0.002;

         D2A(U);

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