#include 
 #include 

 
 /*Output compare interrupts for the motors*/

 #pragma interrupt_handler OC3ISR;
 #pragma interrupt_handler OC2ISR;
 #pragma interrupt_handler OC4ISR;


 /*input capture interrupt for the remote control*/

 #pragma interrupt_handler IC1ISR;


 /*timer overflow interrupt to give relaxation time between end of remote signal and time to next remote signal*/

 #pragma interrupt_handler TOIISR;

 
 /*IC1 is LOW --> signal is ON)*/

 #define REMOTE_ON ((PORTA & 0x04) == 0x00) 

 
 /* Servo 1: Innermost pivot
  * Servo 2: Outermost pivot
  * Servo 3: Pen */
 
 unsigned int overflows = 0;
 
 unsigned int t_h_1 = 0;
 unsigned int t_h_2 = 0;
 unsigned int t_h_3 = 0;

 unsigned int t_l_1 = 40000L;
 unsigned int t_l_2 = 40000L;
 unsigned int t_l_3 = 40000L;
 
 
 /* array storing instructions to draw a smiley face */

 int smile[100] = {'u',0,-20,	'd',0,0, 'd',-20,0, 'd',0,40, 'd',20,0, 'u',0,0, 'u',20,-10, 'd',0,0, 'u',0,0, 'u',0,-20,'d',0,0 };
 

 /* declaration of global constants for machine states, maximum program length, and instruction length */
 
 #define NORMAL  0
 #define PREPROG 1
 #define MAXPLEN 20
 #define INSTLEN 15


 /* variable to keep track of machine state (NORMAL or PREPROG) */

 char drawstate = 0;


 /* keep track of which instruction is being executed in a programmed drawing */

 char drawinst;


 /* counter to wait to execute next instruction in pre-programmed mode */

 int dcount;
 

 /* states for the servos can take on values between 1 and 1000, translating
  * to the full range of motion of each servo */

 int state_1 = 500;
 int state_2 = 500;
 int state_3 = 500;


 /* output compare interrupt service routines for pins OC2, OC3, and OC4.
  * OC4ISR controls the servo for the pen, OC3ISR controls the outermost
  * pivot servo, and OC2ISR controls the innermost pivot servo. */
 
 void OC4ISR () {
   TFLG1 &= 0x10;
   if((TCTL1 & 0x04)==0x04){
     TOC4+=t_h_3;
     TCTL1&=0xFB;  
   } else {
     TOC4+=t_l_3;
     TCTL1|=0x04;    
   }
 }
 
 void OC3ISR () {
   TFLG1 &= 0x20;
   if((TCTL1 & 0x10)==0x10){
     TOC3+=t_h_2;
     TCTL1&=0xEF;  
   } else {
     TOC3+=t_l_2;
     TCTL1|=0x10;    
   }
 }
 
 void OC2ISR () {
   TFLG1 &= 0x40;
   if((TCTL1 & 0x40)==0x40){
     TOC2+=t_h_1;
     TCTL1&=0xBF;  
   } else {
     TOC2+=t_l_1;
     TCTL1|=0x40;    
   }
 }
 

 /* Use the timer overflow to clock a period of about 500ms between the end of a remote signal and 
  * the time until the next remote signal can be processed. This is done so that the output compares
  * have enough time to move the servos into their proper positions before they are disabled during the
  * remote decoding process */

 void TOIISR(){

   /* reset timer overflow flag */

   TFLG2 &= 0x80;


   if(drawstate == NORMAL) {		// plotter is not currently drawing pre-programmed picture

     if(overflows++ > 15) {		// (re-)enable interrupts if still disabled after ~500ms

       /* enable input capture for remote control signal reception */

       TFLG1 &= 0x04;
       TMSK1 |= 0x04;


       /* if motors are still disabled, re-enable them */

       if((TMSK1 & 0x70) != 0x70) {

         TMSK1 |= 0x70;
         PORTA = 0;
         TFLG1&=0x70;

         TOC2 = TOC3 = TOC4 = TCNT + 2000L;
		
         TCTL1 = 0xFC;			// set bits on successful compare for OC2, OC3, and OC4

       }

     }

   } else {				// plotter is drawing a pre-programmed picture

     /* if motors are disabled, re-enable them */

     if((TMSK1 & 0x70) != 0x70) {
		
       TMSK1 |= 0x70;
       PORTA = 0;
       TFLG1&=0x70;

       TOC2 = TOC3 = TOC4 = TCNT + 2000L;

       TCTL1 = 0xFC; 			// set bits on successful compare for OC2, OC3, and OC4
     }


     dcount++;


     /* execute current instruction when ready */

     if(dcount % INSTLEN == 0) {

       /* update servo states using program */

       state_1 += smile[3*drawinst + 1];
       state_2 += smile[3*drawinst + 2];
       state_3 = smile[3*drawinst] == 'u' ? 600: 200;


       /* update servo positions */

       t_h_1 = 1500 + state_1 * 3;
       t_l_1 = 40000L - t_h_1;

       t_h_2 = 1500 + state_2 * 3;
       t_l_2 = 40000L - t_h_2;

       t_h_3 = 1500 + state_3 * 3;
       t_l_3 = 40000L - t_h_3;


       /* move to next drawing instruction */

       drawinst++;

     }


     /* resets drawing state to normal if program is over and lifts pen from paper */

     if( (drawinst==MAXPLEN) || (smile[3*(drawinst+1)] == 'e') ) {

       drawstate = NORMAL;
       state_3 = 600;
       t_h_3 = 1500 + state_3 * 3;
       t_l_3 = 40000L - t_h_3;

     }

   }

 }


 /* bit_counter tracks number of bits in byte, byte_counter tracks number of bytes in
  * signal, and bit_c tracks the place of the current bit being inserted */

 char bit_counter;
 char byte_counter;
 char bit_c;


 /* delta_t stores change in timer to calculate total durations, stored in current_duration */

 int delta_t;
 unsigned int current_duration;


 /* array storing the decoded signal from the remote control */

 char signal[4];


 void IC1ISR() {

   /* reset input capture flag */

   TFLG1 &= 0x04;


   /* reset overflow counter */

   overflows = 0;


   /* temporarily disable output compares to avoid conflict with
    * signal decoding process
   TMSK1 =0x04;
   TCTL1 = 0x00;
   PORTA = 0x00;


   /* calculate the time between captured edges and prepare delta_t for next calculation */

   current_duration = (unsigned int)TCNT - (unsigned int)delta_t;
   delta_t = TCNT;


   /* there are 5 possibilities for silent or active duration lengths:
    * 1. the silent lasted 560 us = 1120 CPU ticks --> record a 0 bit
    * 2. the silent lasted 3 x 560 = 1680us = 3360 CPU ticks --> record a 1 bit
    * 3. the active lasted for 560 us = 1120 CPU ticks --> bit separator, do nothing
    * 4. the active lasted for 9-10 ms = 20000 CPU ticks --> wake up
    * 5. the silent lasted for 4.3ms = 8600 CPU ticks = start recording --> reset counters and signal array */

   if(REMOTE_ON) {					// decode silent duration length

     if( (current_duration > 400) && (current_duration < 1000) ) { // possibility #1: record a 0 bit

       putchar('0'); 
       bit_c++; 					// increment bit placeholder to append 0
       bit_counter++;					// increment bit counter

     }

     if( (current_duration > 1000) && (current_duration < 4000) ) { // possibility #2: record a 1 bit
		
       putchar('1');
       signal[byte_counter] |= (0x01 << bit_c++); 	// write a 1 to the appropriate bit in the signal array
       bit_counter++;					// increment bit counter

     }

     if( (current_duration > 7000) && (current_duration < 10000)) { // clear everything to record new signal

       putchar('p');
       bit_counter = 0;
       byte_counter = 0;
       bit_c = 0;
       for(i = 0; i<4; i++) signal[i]=0;

     }
		
   } else {						// decode active duration length
	
     /* no action required if bit separator detected (current_duration between 400 and 1500) */

     if( (current_duration > 10000) && (current_duration < 20000) ) { // acknowledge readiness to receive signal

       putchar('\n');
       putchar('w');

     }

   }


   /* advance byte counter once eight bits have been recorded */

   if(bit_c == 8) {

     byte_counter++;
     bit_c= 0;

   }


   /* once a 32-bit signal has been recorded, process the instruction */

   if(bit_counter > 31) {

     putchar('-'); 
     bit_counter = 0; 
     printf("\t %x ",signal[2]);


     /* if recorded signal passes error check, update servo states */

     if(signal[2] == ~signal[3]) {

       putchar('!');

       switch (signal[2]) {

         case 1: 	state_1 = state_1 + 10 ; break;				// up
         case 7: 	state_1 -= 10; break;					// down
         case 3: 	state_2 += 10; break;					// left
         case 5: 	state_2 -= 10; break;					// right
         case 4: 	state_3 = (state_3 > 400 ? 200:600); break;		// toggle pen
         case 9: 	state_1 = 500; state_2 = 500; state_3 = 600; break;	// reset all servos

         case 0x74:	drawstate = PREPROG; drawpoint = 0; drawinst = 0; dcount = 0; overflows = 0; break; // begin drawing program

       }

       /* update servo positions */

       t_h_1 = 1500 + state_1 * 3;
       t_l_1 = 40000L - t_h_1;
 
       t_h_2 = 1500 + state_2 * 3;
       t_l_2 = 40000L - t_h_2;

       t_h_3 = 1500 + state_3 * 3;
       t_l_3 = 40000L - t_h_3;
	
       printf("\nI: %5d O: %5d P: %5d\n",state_1,state_2,state_3);

       putchar('m');putchar('e');putchar('\n');


       /* re-enable motors */

       TMSK1 |= 0x70;
       PORTA = 0;
       TFLG1&=0x70;
       TOC2 = TOC3 = TOC4 = TCNT + 2000L;

       TCTL1 = 0xFC;			// set bits on successful compare for OC2, OC3, and OC4

       overflows = 0;


       /* disable input capture for ~500ms to allow servos to adjust */

       TMSK1 &= 0xF0;
       TFLG1 &= 0x04;

     }

   }

 }


 
 void main() {

   char c;


   /* intial adjustment of all positions to reset servos before beginning */

   t_h_1 = 1500 + state_1 * 3;
   t_l_1 = 40000L - t_h_1;

   t_h_2 = 1500 + state_2 * 3;
   t_l_2 = 40000L - t_h_2;

   t_h_3 = 1500 + state_3 * 3;
   t_l_3 = 40000L - t_h_3;


   /* insert JMP commands in appropriate memory locations to handle interrupts */
 
   *(unsigned char*) 0xD9  = 0x7E;
   *(void(**)()) 0xDA  = OC3ISR;
   
   *(unsigned char*) 0xDC  = 0x7E;
   *(void(**)()) 0xDD = OC2ISR;
   
   *(unsigned char*) 0xD6 = 0x7E;
   *(void(**)()) 0xD7 = OC4ISR;
   
   *(char *) 0xE8 = 0x7E;
   *(void(**)()) 0xE9 = IC1ISR;
   
   *(unsigned char*) 0xD0 = 0x7E;
   *(void(**)()) 0xD1 = TOIISR;

   
   TMSK1 = 0x74;	// locally enable IC1, OC2, OC3, and OC4 interrupts
   TMSK2 = 0x80;	// locally enable TOI interrupt
   TCTL1 = 0xFC;	// set bits on successful compare for OC2, OC3, and OC4
   TCTL2 = 0x30;	// capture on rising and falling edges for IC1


   /* set initial values for output compare timers 2, 3, and 4 */

   TOC2 = 40000L;
   TOC3 = 40000L;
   TOC4 = 40000L;


   /* globally enable interrupts */

   asm("CLI");

   while(1) {

     /* wait for command input on keyboard; remote control commands are processed
        using the IC1ISR interrupt service routine */

     c = getchar();
     putchar(c);


     /* character mapping for commands entered using keyboard number pad --> adjusts
        the values of the states of the servos to incrementally move and/or toggle
        the pen's position */

     switch(c){

       case '8': 	state_1 = state_1 + 10 ; break;				//up
       case '2': 	state_1 -= 10; break;					//down
       case '4': 	state_2 += 10; break;					//left
       case '6': 	state_2 -= 10; break;					//right
       case '5': 	state_3 = (state_3 > 400 ? 200:600); break; 		//toggle pen
       case '0': 	state_1 = 500; state_2 = 500; state_3 = 600; break;	//reset

     }

     /* update the output compare values for each of the servos */

     t_h_1 = 1500 + state_1 * 3;
     t_l_1 = 40000L - t_h_1;
 
     t_h_2 = 1500 + state_2 * 3;
     t_l_2 = 40000L - t_h_2;

     t_h_3 = 1500 + state_3 * 3;
     t_l_3 = 40000L - t_h_3;	 


     /* prints the states of each of the three servos to the BUFFALO terminal */
	 
     printf("\nI: %5d O: %5d P: %5d\n",state_1,state_2,state_3);

   } 

 }