/*************************************************************************** */
/* Andres Prieto-Moreno                                                      */
/* Juan gonzalez                                                             */
/*---------------------------------------------------------------------------*/
/*  LICENCIA GPL                                                             */
/*---------------------------------------------------------------------------*/
/* Posicionamiento de servos del tipo Futaba 3003 o compatibles              */
/* Se controlan hasta 8 servos                                               */
/* La senal PWM se genera mediante las interrupciones de overflow del        */
/* temporizador 0                                                            */
/* Las señales se envian por los pines RB0 - RB7                             */
/* En la tarjeta Skypic se pueden conectar los servos directamente           */
/*****************************************************************************/

//-- Especificar el pic a emplear
#include <pic16f876a.h>

//------------- RELACIONADAS CON LOS SERVOS -----------------
#define DELAY_0_3ms 0xEA	  // Tiempo 0'3ms, con prescaler a =64
#define DELAY_1_0ms 0xB0	  // Posicion central: 1ms, pres a = 64
#define DELAY_2_2ms 0x52    // Tiempo 2,2ms, con prescaler a =64

//-- Para llevar los servos a la posicion central
#define CENTRO	    0xB7

/**************************/
/* VARIABLES DEL MODULO   */
/**************************/

// --Posicion de los servos, en unidades de tiempo
static volatile unsigned char pos_servos[8]; 

//-- Numero de servo actual
static unsigned char indice;

//-- Servo actual en binario. Solo tiene un bit activo que indica el servo
//-- actual
static unsigned char servo_actual; 

//-- Mascara que indica los servos activos
//-- Los bits a '0' indican servos desactivados (no reciben senal PWM)
//-- Los bits a '1' reciben el PWM
static unsigned char salidas;

//-- Estado del automata que controla la generacion del PWM de los 8 servos
static unsigned char estado;	

//-- Calculos intermedios
static unsigned char pos; 

/*******************************************************************/
/* Rutina de atención a la interrupción                            */
/* Se supone que la unica interrupcion activa es la del timer0     */
/* Cada servo se controla mediante una ventana de 2.5ms. Para      */
/* cada ventana hay 3 estados:                                     */
/*  -> Estado 0: PWM del servo actual se pone a 1. Este estado     */
/*       dura 0.3ms, que es la anchura minima del PWM              */
/*  -> Estado 1: Se establece el tiempo que debe estar la senal    */
/*               PWM a '1'. Esto depende de la posicion a la que   */
/*               se situe el servo                                 */
/*  -> Estado 2: Se ponen todas las senales PWM a 0 y se calcula   */
/*        el valor del timer0 para que en total hayan transcurrido */
/*        2.5ms, que es el ancho de la ventana temporal            */
/*-----------------------------------------------------------------*/
/* Se puede encontrar mas informacion sobre como manejar 8 servos  */
/* usando un unico temporizador en este enlace:                    */
//  http://www.iearobotics.com/personal/juan/doctorado/tea/html/node79.html 
// o en la pagina 87 de este PDF:                                            
// http://www.iearobotics.com/personal/juan/doctorado/cube-reloaded/download/tea.pdf
/*******************************************************************/

void intr(void) interrupt 0 {
	
  // Borra flag de interrupcion TIMER0 
  T0IF=0;

  //-- Estado inicial. Pulso de anchura 0.3ms
  if (estado==0) {
    //-- Poner la senal PWM del servo actual a '1', si el bit correspondiente
    //-- de la masacara de habilitacion (salidas) esta a '1'
    PORTB=salidas & servo_actual;  

    //-- Lanzar temporizador para calcular anchura fija de 0.3ms
    TMR0=DELAY_0_3ms;

    //-- Pasar al siguiente estado cuando llegue la siguiente interrupcion
    estado=1;
  }
  //-- Estado 1: Anchura del pulso
  else if (estado==1) {

    //-- Anchura para situar el servo
    TMR0=pos_servos[indice];

    //-- Pasar al siguiente estado cuando llegue la siguiente interrupcion
    estado=2;
  }
  //-- Estado final: Poner la senal PWM a '0' y calcular el tiempo que resta
  //-- para completar la ventana de 2.5ms
  else if (estado==2) {
    //-- Todas las senales PWM deben estar a cero
    PORTB=0;

    //-- Calcular timer 0 para que se produzca una interrupcion al completarse
    //-- los 2.5ms (en el estado 0 se han perdido 0.3ms)
    TMR0=DELAY_2_2ms-pos_servos[indice];

    //-- Pasar el estado inicial. Continuamos con el siguiente servo
    estado=0;
		
    //-- Pasar al siguiente servo
    servo_actual=servo_actual<<1;
    indice++;

    //-- Si ya se han generado las 8 senales PWM, volver a empezar desde
    //-- el servo 0
    if (servo_actual==0x00) { 
      servo_actual=1;
      indice=0;
    }	
  }	 
  else {
    // Aqui nunca deberia llegar
    // Pasar al estado inicial
    estado=0;
  }	
}

/***************************************/
/*  Configuracion del Timer 0          */
/***************************************/
void timer0_configure() {
  // TOCS=Modo timer bit5=0
  // PS2:PS9 prescaler a 32
  // PSA=0 para asignar el prescaler al timer0
  OPTION_REG=0x05;   
	
  // Borra flag de interrupcion TIMER0 TOIF
  T0IF=0;  //-- Borrar flag de interrupcion
  T0IE=1;  //-- Activar interrupcion del timer0
  GIE=1;   //-- Activar las interrupciones globales
  TMR0=0;               // Inicializa el registro del timer
}

/*----------------------------------------------------*/
/*    F U N C I O N E S    D E    I N T E R F A Z     */
/*----------------------------------------------------*/


/*****************************************************/
/* Establecer la posicion del servo en grados        */
/* ENTRADAS:                                         */
/*    -servo: Numero de servo. Entre 0 y 7           */
/*    -grados: Posicion en grados. Entre -90 y 90    */
/*****************************************************/
void servos_set(int servo, int grados)
{
  int result;
  volatile int g;
  volatile float temp;
  volatile int t2;

  //-- La formula para calcular el valor temporal a aplicar al timer0 en 
  //-- funcion de los grados es: tiempo = grados*0.8 + 72;
  
  //-- En el SDCC 2.8.0 y 2.9.0 hay un bug que impide realizar operaciones
  //-- con los numeros flotantes negativos. Por eso se divide el calculo en
  //-- dos partes, dependiendo del signo de grados

  if (grados>=0)
    result = (int) (grados*0.8 + 72);
  else {
    //-- calculamos el valor pos_grados*0.8 como un numero entero positivo
    g = -grados;
    temp = g*0.8;
    t2 = (int)temp;

    //-- Y luego hacemos la resta
    result = 72 - t2;
   }
  
  //-- Posicionar el servo
  pos_servos[servo]=0xFF-result;
}

/************************************************************************/
/* Establecer la mascara de funcionamiento de los servos                */
/* ENTRADAS:                                                            */
/*   -mascara: un byte que se usara de mascara. Los bits a '1' indican  */
/*             que el servo correspondiente esta activo. Los bits '0'   */
/*             que el servo esta inactivo (y no recibe senal PWM)       */
/************************************************************************/
void servos_enable(unsigned char mascara)
{
  salidas = mascara;
}

/*****************************************************************/
/* Configurar el modulo servos para funcionar                    */
/* Esta funcion tiene que ser invocada antes de                  */
/* poder posicionar los servos                                   */
/* Se inicializan las variables y se activan las interrupciones  */
/*****************************************************************/
void servos_init(void)
{
  unsigned char i;

  //-- Configurar puerto B como salida
  TRISB=0x00;

  //--inicializacion de variables
  indice=0;
  servo_actual=1; 
  estado=0;

  //-- Servos en su posicion central
  for (i=0; i<8; i++) {
    pos_servos[i]=CENTRO; 
  }

  //-- Por defecto las senales PWM estan desactivadas
  salidas=0x00;
  
  //-- Configurar el Timer0 para controlar los servos
  //-- y activar su interrupcion
  timer0_configure();
}

