/*      Servo Control program for SPOT                               */
/*      written for ICC and compiled for single chip board           */
/*      NOTE:  6811E2 chip running in BOOTSTRAP mode                 */
/*      program receives data through serial port and generates PWM's*/
/*      Written by:  Rodrigo J. Osorio for EEL5666                   */
/*      Last revision:  December 10, 1997                            */
/*                                                                   */


#include<hc11.h>
#include<mil.h>

#define TOC2_ISR servo_driver
#define SCI_ISR serial_driver
#pragma interrupt_handler servo_driver
#pragma interrupt_handler serial_driver

void servo_driver();
void serial_driver();
void init_serial();
void init_servos();
void init_board();

static int state,i;

int mask[8]={0x01,0x02,0x04,0x08,0x10,0x20,0x40,0x80};
int data[8];

main(void){
  init_board();
  INTR_OFF();
  init_servos();                  // Initialize servos & start servo interrupts
  init_serial();                  // Initialize serial ports & start interrupts
  INTR_ON();

  while(1);
  
}

void servo_driver()
{
      if(state)               // Turn-off port
      {
        PORTC=0x00;
        PORTB=0x00;
        TOC2=TCNT+5000;       // Progran next rising-edge
        state=0;
      }
      else
      {
        PORTC=mask[i];        //
        PORTB=mask[i];        //  Turn-on port
        TOC2=TCNT+data[i];    //  Progran pulse width
        state=1;
        i++;                //  i indexes servo widths and port masks
        i%=8;               //  i must be between zero and seven
      }
    CLEAR_FLAG(TFLG1,0x40);   //  Clear TOC2 flag
}

void serial_driver(){
   int com_data;
   com_data=SCDR;
   if(com_data<32) data[0]=-com_data*63+3874;          //4000
   else if(com_data<64) data[4]=(com_data-32)*63+2000;

   else if(com_data<96) data[1]=(com_data-64)*63+2126;  //2000
   else if(com_data<128) data[5]=-(com_data-96)*63+4000;

   else if(com_data<160) data[2]=-(com_data-128)*63+4000;
   else if(com_data<192) data[6]=-(com_data-160)*63+4000;

   else if(com_data<224) data[3]=(com_data-192)*63+2000;
   else  data[7]=(com_data-224)*63+2000;
   CLEAR_FLAG(SCSR,0x20);       // clear receive-data-register-full flag
}

void init_serial(){
	BAUD=0xb0;                   // Baud=9600
   SCCR2=0x2c;                  // Enable Transmit and Receive
                                // Enable interrupts on receive-full
}

void init_board(){
    CLEAR_BIT(HPRIO,0x40);          //Remap interrupts to 0xffd6
    DDRC=0xff;                      //  Configure PORTC for output
}

void init_servos(){
  TOC2 = 0;                       // First OC2 int will occur at TCNT=0 */
  SET_BIT(TMSK1,0x40);            // Enable OC2 interrupt
  data[0]=3000;data[1]=3000;data[2]=3000;data[3]=3000;
  data[4]=3000;data[5]=3000;data[6]=3000;data[7]=3000;
  state=0;

}