/*************************************************************************** */
/* ej-PP1.c  (c) Juan Gonzalez. Abril 2009                                   */
/*---------------------------------------------------------------------------*/
/* Ejemplo de movimiento de la configuracion PP (minicube-I)                 */
/* Al apretar el pulsador S1 de la Skypic, el robot cambia el sentido        */
/* del movimiento                                                            */
/*---------------------------------------------------------------------------*/
/* Se emplean los servos 1 y 2 (segun numeracion 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"

#define PULSADOR RA0
#define ON       0
#define OFF      1

/******************************************/
/*  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 -125

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

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

//-- 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;
  int inc;

  //-- Inicializar timer2
  timer2_init();

  //-- Inicializacion
  servos_init();

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

  //-- Configurar RA0 como entrada digital
  //-- Todos los pines como E/S Digitales
  ADCON1=0x06;

  //-- Configurar el bit 0 como entrada
  TRISA0 = 1;

  //-- 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;

  //-- Inicializar las variables
  inc=INC;
  fase=0;

  //-- Bucle principal
  while(1) {
    //-- Establecer las posiciones de esta fase
    servos_osc(0,A,fase+FASE0,OFFSET);
    servos_osc(1,A,fase+FASE0+DF,OFFSET);
    timer2_delay(pausa);

    //-- Si se aprieta el boton cambiar el signo del incremento
    //-- de la fase (para cambiar el sentido del movimiento)
    if (PULSADOR==ON) {
      inc=-inc;
      //-- Esperar hasta que el boton se suelte
      while(PULSADOR==ON);
    }

    //-- Incrementar la fase
    fase=fase + inc;

    //-- Las fases negativas se convierten a positivas
    if (fase<0) fase=359;

    //-- Mantener la fase en el rango 0 - 360 grados
    fase = fase % 360;
  }
}

