/*************************************************************************** */
/* ej1-servos2.c  (c) Juan Gonzalez. Abril 2009                              */
/*---------------------------------------------------------------------------*/
/* Ejemplo de oscilacion de los servos 1 y 2. Probado en una tarjeta Skypic  */
/*---------------------------------------------------------------------------*/
/*  LICENCIA GPL                                                             */
/*****************************************************************************/

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

#include "timer2.h"
#include "servos.h"
#include "sin.h"


/******************************************/
/*  PARAMETROS DE LAS OSCILACIONES        */
/*  DEFINIDOS POR EL USUARIO              */
/******************************************/
//-- Amplitud (en grados) (0-90)
#define A 45

//--Fase inicial (en grados) (0-360)
#define FASE0 0

//-- Diferencia de fase (en grados) (-180 a 180)
#define DF -45

//-- Offset (en grados) (-90 y 90)
#define OFFSET 0

//-- Periodo (en ms)
#define T 1000

//-- Incremento de fase (en grados) (1-180)
#define INC 1

/***********************************************************/
/* Posicionar un servo segun los parametros del oscilador  */
/* ENTRADAS:                                               */
/*  -servo: Numero de servo (0-7)                          */
/*  -amp: Amplitud (0-90)                                  */
/*  -fase: La fase (0-360)                                 */
/***********************************************************/
void servos_osc(int servo, int amp, int fase, int offset)
{
  int pos;

  pos=sin(amp,fase)+offset;
  servos_set(servo,pos);
}

/***************************************/
/* Programa principal                  */
/***************************************/
void main(void)
{
  int fase;
  unsigned int pausa;
  float pausaf;

  //-- Inicializar timer2
  timer2_init();

  //-- Inicializacion
  servos_init();

  //-- Activar los servos
  servos_enable(SERVO0 | SERVO1);

  //-- Servos a posiciones iniciales
  servos_osc(0,A,FASE0,OFFSET);
  servos_osc(1,A,FASE0+DF,OFFSET);
  timer2_delay(1000);

  //-- Calcular la pausa a realizar entre dos muestras
  //-- consecutivas
  pausaf = ((float)INC*(float)T)/360.0;
  pausa = (unsigned int) pausaf;

  //-- Hacer las oscilaciones!!!
  while(1) {
    for (fase=0; fase<360; fase+=INC) {
      servos_osc(0,A,fase+FASE0,OFFSET);
      servos_osc(1,A,fase+FASE0+DF,OFFSET);
      timer2_delay(pausa);
    }
  }
}

