/*
 * File:   Ruedas.c
 * Author: Christian
 *
 * Created on 30 de marzo de 2018, 22:51
 */


#include "main.h"


#if  PWM_Period_Select == 1
    int MinPowerDC1 = 0;
    int MinPowerDC2 = 0;
    int MinPowerDC3 = 0;
    int MinPowerDC4 = 0;
#elif PWM_Period_Select == 2
    int MinPowerDC1 = 0;
    int MinPowerDC2 = 0;
    int MinPowerDC3 = 0;
    int MinPowerDC4 = 0;
#elif PWM_Period_Select == 3
    int MinPowerDC1 = 0;
    int MinPowerDC2 = 0;
    int MinPowerDC3 = 0;
    int MinPowerDC4 = 0;
#elif PWM_Period_Select == 4
    int MinPowerDC1 = 50;         //Alante izquierda
    int MinPowerDC2 = 41;         //Alante derecha
    int MinPowerDC3 = 55;         //Atras  izquierda
    int MinPowerDC4 = 56;         //Atras  derecha
    int MaxPowerDC1 = PWM_Period-56;
    int MaxPowerDC2 = PWM_Period+1;
    int MaxPowerDC3 = PWM_Period+23;
    int MaxPowerDC4 = PWM_Period+1;
//    int MinPowerDC1 = 0;
//    int MinPowerDC2 = 0;
//    int MinPowerDC3 = 0;
//    int MinPowerDC4 = 0;
#else
    int MinPowerDC1 = 0;
    int MinPowerDC2 = 0;
    int MinPowerDC3 = 0;
    int MinPowerDC4 = 0;
#endif

const double PWM_Period_divBy100  = (PWM_Period+1)/100;
const double PWM_Period_divBy1000 = (PWM_Period+1)/1000;

double PWM1_Period_D1000_wMaxMin;
double PWM2_Period_D1000_wMaxMin;
double PWM3_Period_D1000_wMaxMin;
double PWM4_Period_D1000_wMaxMin;

void RUEDAS_update_correction_coeficients(void){
    PWM1_Period_D1000_wMaxMin = ((MaxPowerDC1)-MinPowerDC1)/1000.0;
    PWM2_Period_D1000_wMaxMin = ((MaxPowerDC2)-MinPowerDC2)/1000.0;
    PWM3_Period_D1000_wMaxMin = ((MaxPowerDC3)-MinPowerDC3)/1000.0;
    PWM4_Period_D1000_wMaxMin = ((MaxPowerDC4)-MinPowerDC4)/1000.0;
}

volatile char RUEDAS_ENABLED;
volatile int Right_p1000;
volatile int Left_p1000;


void Inic_Ruedas(){
    _TRISB10=0;
    _TRISB11=0;
    _LATB10=0;
    _LATB11=0;
    
    RUEDAS_ENABLED = FALSE;
    
    Right_p1000 = 0;
    Left_p1000  = 0;
    
    RUEDAS_update_correction_coeficients();
}

void enable_RUEDAS()
{
    _LATB10=1;
    _LATB11=1;
    RUEDAS_ENABLED = TRUE;
}
void disable_RUEDAS()
{
    _LATB10=0;
    _LATB11=0;
    RUEDAS_ENABLED = FALSE;
}
static void RUEDAS_setSpeed_right(int percent);
static void RUEDAS_setSpeed_left(int percent);
void RUEDAS_Forward()
{
    if(Right_p1000 < 100){
        RUEDAS_setSpeed_right(Right_p1000+=1);
    }
    if(Left_p1000 < 100){
        RUEDAS_setSpeed_left(Left_p1000+=1);
    }
    ruedas_cf.P100 = 1;
}
void RUEDAS_Backward()
{
    if(Right_p1000 > -100){
        RUEDAS_setSpeed_right(Right_p1000-=1);
    }
    if(Left_p1000 > -100){
        RUEDAS_setSpeed_left(Left_p1000-=1);
    }
    ruedas_cf.P100 = 1;
}
static void RUEDAS_setSpeed_right(int p1000)
{
    if(p1000 == 0){
        zero_dutyCicle_4();
        zero_dutyCicle_2();
    }else if(p1000 >0){
        set_dutyCicle_4(PWM_Period_divBy1000* (unsigned int)p1000);
        zero_dutyCicle_2();
        _IC2_Direction = 0;
    }else{
        zero_dutyCicle_4();
        set_dutyCicle_2(PWM_Period_divBy1000* (unsigned int)(-p1000));
        _IC2_Direction = 1;
    }
}
static void RUEDAS_setSpeed_left(int p1000)
{
    if(p1000 == 0){
        zero_dutyCicle_3();
        zero_dutyCicle_1();
    }else if(p1000 >0){
        set_dutyCicle_3(PWM_Period_divBy1000* (unsigned int)p1000);
        zero_dutyCicle_1();
        _IC1_Direction = 0;
    }else{
        zero_dutyCicle_3();
        set_dutyCicle_1(PWM_Period_divBy1000* (unsigned int)(-p1000));
        _IC1_Direction = 1;
    }
}

void RUEDAS_setSpeed_right_p100(int percent)
{
    
    if(percent == 0){
        zero_dutyCicle_4();
        zero_dutyCicle_2();
    }else if(percent >0){
        set_dutyCicle_4(PWM_Period_divBy100* (unsigned int)percent);
        zero_dutyCicle_2();
        _IC2_Direction = 0;
    }else{
        zero_dutyCicle_4();
        set_dutyCicle_2(PWM_Period_divBy100* (unsigned int)(-percent));
        _IC2_Direction = 1;
    }
    Right_p1000=percent*10; //Convertir el porciento a mil
    ruedas_cf.P100 = 1;
}
void RUEDAS_setSpeed_left_p100(int percent)
{
    if(percent == 0){
        zero_dutyCicle_3();
        zero_dutyCicle_1();
    }else if(percent >0){
        set_dutyCicle_3(PWM_Period_divBy100* (unsigned int)percent);
        zero_dutyCicle_1();
        _IC1_Direction = 0;
    }else{
        zero_dutyCicle_3();
        set_dutyCicle_1(PWM_Period_divBy100* (unsigned int)(-percent));
        _IC1_Direction = 1;
    }
    Left_p1000=percent*10; //Convertir el porciento a mil
    ruedas_cf.P100 = 1;
}
void RUEDAS_setSpeed_right_p1000(int perthousand)
{
    
    if(perthousand == 0){
        zero_dutyCicle_4();
        zero_dutyCicle_2();
    }else if(perthousand >0){
        set_dutyCicle_4(MinPowerDC4+(PWM4_Period_D1000_wMaxMin* perthousand));
        zero_dutyCicle_2();
        _IC2_Direction = 0;
    }else{
        zero_dutyCicle_4();
        set_dutyCicle_2(MinPowerDC2+(PWM2_Period_D1000_wMaxMin* -perthousand));
        _IC2_Direction = 1;
    }
    Right_p1000=perthousand;
    ruedas_cf.P100 = 1;
}
void RUEDAS_setSpeed_left_p1000(int perthousand)
{
    if(perthousand == 0){
        zero_dutyCicle_3();
        zero_dutyCicle_1();
    }else if(perthousand >0){
        set_dutyCicle_3(MinPowerDC3+(PWM3_Period_D1000_wMaxMin* perthousand));
        zero_dutyCicle_1();
        _IC1_Direction = 0;
    }else{
        zero_dutyCicle_3();
        set_dutyCicle_1(MinPowerDC1+(PWM1_Period_D1000_wMaxMin* -perthousand));
        _IC1_Direction = 1;
    }
    Left_p1000=perthousand;
    ruedas_cf.P100 = 1;
}