/******************************************************************
*                                                                 *
*           Librería de control para Sabertooth                   *
*                                                                 *
*           Autor:      Ángel Hernández Mejías (Mif)              *
*           Contacto:   angel@tupperbot.es                        *
*           Blog:       www.tupperbot.es                          *
*           Fecha:      10/02/2009                                *
*           Versión:    0.1                                       *
*                                                                 *
*******************************************************************
*                                                                 *
*                        Licencia                                 *
*  Esta librería está publicada bajo la licencia Creative         *
* Commons en la versión de Attribution and Share Alike. Es decir, *
* se permite copiar, modificar y distribuir este material         *
* siempre que se reconozca al autor y se mantenga la misma        *
* licencia.                                                       *
*                                                                 *
*******************************************************************
*                                                                 *
*  Esta librería sirve para manejar de un modo sencillo y limpio  *
* el driver de motores Sabertooth en modo analógico               *
*                                                                 *
*  Es recomendable usar el sistema Skypic para evitar problemas   *
* en la electrónica al hacer montajes básicos.                    *
*                                                                 *
*  Requisitos:                                                    *
*  -setup_timer_2(T2_DIV_BY_4, 127, 1);                           *
*  -setup_ccp1(CCP_PWM);                                          *
*  -setup_ccp2(CCP_PWM);                                          *
*                                                                 *
******************************************************************/
   
#define EnaIzq PIN_C2   //Enable Motor Izquierdo Enable_A 6
#define EnaDch PIN_C1   //Enable Motor Derecho Enable_B 11

const int Numero_Velocidades = 11;

const unsigned int16 PWM_Centro = 512; //461
const unsigned int16 PWM_Max  =  1023;
const unsigned int16 PWM_Off  =  0;
const unsigned int16 PWM_0    =  0;
const unsigned int16 PWM_1    =  51;
const unsigned int16 PWM_2    =  102;
const unsigned int16 PWM_3    =  153;
const unsigned int16 PWM_4    =  205;
const unsigned int16 PWM_5    =  256;
const unsigned int16 PWM_6    =  307;
const unsigned int16 PWM_7    =  358;
const unsigned int16 PWM_8    =  409;
const unsigned int16 PWM_9    =  461;
const unsigned int16 PWM_10   =  511;

int Limite_Velocidad = 7;

unsigned int16 PWM_Seleccion[Numero_Velocidades] = {PWM_0, PWM_1, PWM_2, PWM_3, PWM_4, PWM_5, PWM_6, PWM_7, PWM_8, PWM_9, PWM_10};

int PWM_Index = 5;
unsigned int16 Velocidad = PWM_5;

int Marcha;
char Marcha_Txt[7];

void InicializarMotores()
{
   setup_ccp1(CCP_PWM);
   set_pwm1_duty(PWM_Centro);
   setup_ccp2(CCP_PWM);
   set_pwm2_duty(PWM_Centro);
}

void MotorDch(int Direccion,  int Velocidad)
{
   if (Direccion == 0){set_pwm1_duty(PWM_Centro-PWM_Seleccion[Velocidad]);}
   else {set_pwm1_duty(PWM_Centro+PWM_Seleccion[Velocidad]);}
}

void MotorIzq(int Direccion,  int Velocidad)
{
   if (Direccion == 0){set_pwm2_duty(PWM_Centro-PWM_Seleccion[Velocidad]);}
   else {set_pwm2_duty(PWM_Centro+PWM_Seleccion[Velocidad]);}
}

void Delante (int Velocidad)
{
   set_pwm1_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "Delante");
   Marcha = 1;
}

void Detras (int Velocidad)
{
   set_pwm1_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "Detras");
   Marcha = 2;
}

void Izq (int Velocidad)
{
   set_pwm1_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro);
   sprintf(Marcha_Txt, "Izq");
   Marcha = 3;
}

void DetrasIzq (int Velocidad)
{
   set_pwm1_duty(PWM_Centro);
   set_pwm2_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "Dtr.Izq");
   Marcha = 3;
}

void Dch (int Velocidad)
{
   set_pwm1_duty(PWM_Centro);
   set_pwm2_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "Dch");
   Marcha = 4;
}

void DetrasDch (int Velocidad)
{
   set_pwm1_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro);
   sprintf(Marcha_Txt, "Dtr.Dch");
   Marcha = 4;
}

void GiroHorario (int Velocidad)
{
   set_pwm1_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "G. Dch");
   Marcha = 5;
}

void GiroAntiHorario (int Velocidad)
{
   set_pwm1_duty(PWM_Centro-PWM_Seleccion[Velocidad]);
   set_pwm2_duty(PWM_Centro+PWM_Seleccion[Velocidad]);
   sprintf(Marcha_Txt, "G. Izq");
   Marcha = 6;
}

void Paro()
{
   set_pwm1_duty(PWM_Centro);
   set_pwm2_duty(PWM_Centro);
   sprintf(Marcha_Txt, "Paro");
   Marcha = 0;
}

void Freno()
{
   Paro();
}