/**********************************************************************
 * 			MEKATRONIX Copyright 1998 		      *
 * Title        servome.c  (Servo Drivers for TJ PRO)                 *
 * Programmer   Keith L. Doty			                      *
 * Date         May 07, 1999                                          *
 * Version      1                                                     *
 *                                                                    *
  * Description: Servo drivers.					      *
	servo 0 on PA3 
	servo 1 on PA4 
	servo 2 on PA5 
	servo 3 on PA6 
	servo 4 on PA7

	All pulse widths created in sequence, i.e., the five servos
	are phase shifted relative to one another by the pulse width 
	of the servo that preceeds it in the sequence.


 * History:     Total revision					      *
 **********************************************************************/


/**************************** Includes **********************************/
#include <hc11.h>
#include <mil.h>
/************************************************************************/


/**************************** Constants *********************************/
#define HALFPERIOD 16000     	// Refresh every 20 ms for each servo
#define servo_hand  TOC4_isr
#define NSERVO 5				//Number of servos
#define NO_PULSE 1000			//Duration of a "zero pulse"
/************************************************************************/


/*************************** Prototypes *********************************/
#pragma interrupt_handler TOC4_isr    /*Servo Handler*/
void init_servome(void);

/*
   servo() Arguments:
   Servo# (0,1,2,3,4), Pulse Widths dictated by servo_limits[][]
   For the ME11 board servos 2 and 3 are the left and right motors.
*/
void servo(int, int);

void servo_hand(void);
/************************************************************************/


/***************************** Globals **********************************/
/* servo_width[] is the on-time for each servo*/
extern unsigned int previous_pwidth;
extern unsigned int servo_width[NSERVO]; 
extern unsigned char servo_signal_state, servomask[NSERVO];
extern int period_end;

extern unsigned int servo_limits[NSERVO][2];
extern char srv_lim_default;
/************************************************************************/



void init_servome(void)
/************************************************************************
 * Function: Initializes all necessary variables for the servos.        *
 * Returns: None                                                        *
 *                                                                      *
 * Inputs                                                               *
 *   Parameters: None                                                   *
 *   Globals:    None                                                   *
 *   Registers:  None                                                   *
 * Outputs                                                              *
 *   Parameters: None                                                   *
 *   Globals:   servo_width[], servo_signal_state, servomask[],		*
 *              current_servo_width       					*
 *   Registers: TOC1, PORTB, TMSK1, TCTL1, PACTL                               *
 * Functions called: None                                               *
 * Notes: None                                                          *
 ************************************************************************/
{
  int i;
  
  INTR_OFF();
  
  /*install interrupt handler on OC4*/
  *((void (**)())0xffe2)  = servo_hand;
  
  for(i=0; i<NSERVO; i++)
  {
  //Motors start turned off
 	servo_width[i] = 0;
 	previous_pwidth = 0;
 	
  //Establish bit maps to control PortA outputs
	servomask[i] = 0x08<<i;

  }
     
  TOC4 = 0;                       
  servo_signal_state = 0;

  SET_BIT(PACTL, 0x88);		   /* DDRA7 & DDRA3 set for output */
  SET_BIT(TMSK1,0x10);         /* Enable OC4 interrupt */
  INTR_ON();
}

/************************ end init_servome ******************************/


void servo(int index, int newwidth)
/************************************************************************
 * Function: Sets one of the servos to the speed defined by newwidth    *
			 Servo limits are checked. Default is no limit check.
 * Returns: None                                                        *
 *                                                                      *
 * Inputs                                                               *
 *   Parameters: index, newwidth                                        *
 *   Globals:    None                                                   *
 *   Registers:  None                                                   *
 * Outputs                                                              *
 *   Parameters: None                                                   *
 *   Globals: servo_width[index]                                              *
 *   Registers:  None                                                   *
 * Functions called: None                                               *
 * Notes: None                                                          *
 ************************************************************************/
{
 unsigned int x;
   
  if(srv_lim_default == 1) //Check newwidth against default settings
  {
	x = servo_limits[index][0];
  //Check lower servo limit. Zero special case, turns servo off. 
	if((newwidth<= x) && (newwidth != 0)) newwidth = x;
	
	x = servo_limits[index][1];
  //Check upper servo limit
	if(newwidth >= x) newwidth = x;
  }
  servo_width[index] = newwidth;

}

/*********************** End servo ***********************************/





void servo_hand(void)
/************************************************************************
 * Function: Interrupt handler for servo signals.                       *
 *	     Interrupt is OC4. Servo_0 is on PA3 and Servo_1 on PA4 and 	*
 *           Servo_2 is on PA7.							*
 * Returns: None                                                        *
 *                                                                      *
 * Inputs                                                               *
 *   Parameters: None                                                   *
 *   Globals:    servo_width[], servo_signal_state, servomask[], 		*
 *               current_servo_width      					*
 *   Registers:  None                                                   *
 * Outputs                                                              *
 *   Parameters: None                                                   *
 *   Globals:    current_servo_width                                    *
 *   Registers:  TOC4, PORTA, TFLG1                                     *
 * Functions called: None                                               *
 * Notes: None                                                          *
 ************************************************************************/
{
  int i;
  unsigned int pwidth;

/*
   k=0,1,2,3,4: servo_signal_state = k -> Turn on servok, 
   Increment TOC4 by servo_width[k]. Begin sequence again
   a full servo period after start of servo 0 period.
   
   Assumption: Sum of the servo pulse widths < full servo period.

*/

//Turn off previous servo, if exist and if on!
  if((servo_signal_state != 0) && (previous_pwidth != 0))
    PORTA ^= servomask[servo_signal_state-1];
    
//Mark the end of zero servo's period 	
  if(servo_signal_state == 0) period_end = TOC4 + HALFPERIOD + HALFPERIOD;

  
//Get pulse width of this servo
  if(servo_signal_state < NSERVO)
  { 
	pwidth = servo_width[servo_signal_state];
	previous_pwidth = pwidth; //Save for next pass through
	
	if(pwidth == 0)
	  TOC4 += NO_PULSE;   //No-pulse for 1ms!
    else
      TOC4 += pwidth;
  		
  //Turn on this servo, if not zero pulse width
    if(pwidth != 0)
	  PORTA ^= servomask[servo_signal_state];
  }
  
  if(servo_signal_state == NSERVO)
  {
   	TOC4 = period_end;
   	servo_signal_state = 0;
  }
  else
  //Prepare for next servo access 
    servo_signal_state++;
  
  CLEAR_FLAG(TFLG1,0x10);          /* Clear OC4I flag */
}

/*********************** End servo_hand ***********************************/