/*************************************************************************** */
/* ej-PYP.c  (c) Juan Gonzalez. Noviembre 2009                               */
/*---------------------------------------------------------------------------*/
/* Ejemplo de movimiento de la configura minima PYP (minicube-II)            */
/* El robot ejecuta diferentes secuencias de movimiento. Se pasa de una      */
/* a otra al apretar el pulsador                                             */
/* Cada movimiento tiene sus propios parametros que pueden ser cambiados     */
/* en las tablas.                                                            */
/*---------------------------------------------------------------------------*/
/*  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 tipico (en ms)
#define TIP 1000

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

//-- Servos virtuales:            1   2   3
__code unsigned char v[]  = {0,   1,  2,  4};

//-- Amplitudes para los diferentes estados:
//-- STOP, ADELANTE, ATRAS, ARCO-AD, ARCO-AT, LAT1, LAT2, ROT1, ROT2, ROLL1  
__code int Av[]  = {0,   45,  45,    45,    45, 30,   30,  45,  45,  0,    70};
__code int Ah[]  = {0,    0,   0,    0,      0, 30,   30,  45,  45,  0,    70};
__code int Oh[]  = {0,    0,   0,    45,    45,  0,    0,   0,   0,  70,    0};
__code int DFv[] = {0, -125, -125, -125,  -125,  0,    0,  180, 180,  0,    0};
__code int DFvh[]= {0,    0,    0,    0,     0, 90,   90,   90,  90,  0,   90};
__code int inc[] = {0,  INC, -INC,  INC,  -INC, INC,-INC,  INC,-INC,  0,  INC};
__code int T[] =   {0,  TIP,  TIP,  TIP,  TIP, TIP, TIP,  TIP, TIP, TIP, 7000};

/***********************************************************/
/* Posicionar un servo segun los parametros del oscilador  */
/* ENTRADAS:                                               */
/*  -servo: Numero de servo (1-8)                          */
/*  -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-1,pos); 
}

/***************************************/
/* Programa principal                  */
/***************************************/
void main(void)
{
  int fase;
  unsigned int pausa;
  float pausaf;
  unsigned int mov;
  unsigned char TAM=sizeof(Av)/sizeof(int);  //-- Numero de movimientos diferentes

  //-- Inicializar timer2
  timer2_init();

  //-- Inicializacion
  servos_init();

  //-- Activar todos los servos
  servos_enable(0xFF);

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

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

  //-- Inicializar las variables
  fase=0;
  mov=0;
    
  //-- Servos a posiciones iniciales
  servos_osc(v[1],Av[mov],FASE0,0);
  servos_osc(v[2],Ah[mov],FASE0+DFvh[mov],Oh[mov]);
  servos_osc(v[3],Av[mov],FASE0+DFv[mov],0);
  timer2_delay(1000);

    
  //-- Bucle principal
  while(1) {

    //-- Calcular la pausa a realizar entre dos muestras
    //-- consecutivas
    pausaf = ((float)INC*(float)T[mov])/360.0;
    pausa = (unsigned int) pausaf;
      
    //-- Establecer las posiciones de esta fase
    servos_osc(v[1],Av[mov],fase+FASE0,0);
    servos_osc(v[2],Ah[mov],fase+FASE0+DFvh[mov],Oh[mov]);
    servos_osc(v[3],Av[mov],fase+FASE0+DFv[mov],0);
    timer2_delay(pausa);

    //-- Incrementar la fase
    fase=fase + inc[mov];

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

    //-- Mantener la fase en el rango 0 - 360 grados
    fase = fase % 360;
      
    //-- Si se aprieta el boton se pasa al siguiente modo de caminar
    if (PULSADOR==ON) {
      //-- Pasar al siguiente modo   
      mov++;

      //-- Si se llega al final, volver a empezar
      mov = mov % TAM;

      //-- Iniciar la fase para el movimiento de rolling
      if (mov==9) {
        fase=0;
      }
     
      //-- Esperar hasta que el boton se suelte
      while(PULSADOR==ON);
    }

   
  }
}

