/*
 * File:   PWMfunctions.c
 * Author: Jay Burns
 *
 * Created on April 3, 2012, 9:47 PM
 */

/*
 * README
 *
 * There are four functions you need to call to effectively use the PWM code.
 * First, call initializePWMcapture() and initializePWMoutput().  This will
 * setup the control registers and timers so input and output is ready to go.  Then, in your while loop,
 * call getPWM();  This will read the PWM values.  They are stored in:
 *
 * CH1value
 * CH2value
 * CH3value
 * CH4value
 * These are all values of 0 to 100.
 * 
 * CH5status
 * CH5status is either 1 or 0.  I suggest your main looking something like this:
 *
 * int main(void)
{
    initializePWMcapture();
    initializePWMoutput();

    while(1)
    {
        getPWM();
        if (CH5status == 1)
        {
            setPower(75, 74, 76, 72);   //This is just an example output to the motors.  Remember, it's
        }                               //setPower(motor1, motor2, motor3, motor4)
        else if (CH5status == 0)
        {
            setPower(0, 0, 0, 0);
        }
    }
}.
 */

//	Function Prototypes
void getPWM(void);
void initializePWMcapture(void);
void initializePWMoutput(void);
void setPower(int, int, int, int);
float readCH1(int);
float readCH2(int);
float readCH3(int);
float readCH4(int);
void readCH5(void);

//Declare Global Variables

int IC1Capture1, IC1Capture2, IC2Capture1, IC2Capture2, IC3Capture1, IC3Capture2, IC4Capture1, IC4Capture2, IC5Capture1, IC5Capture2;
int CH1count,CH2count, CH3count, CH4count, CH5count, CH1value, CH2value, CH3value, CH4value, CH5value, CH5status;

void initializePWMoutput(void)
{

    int pulseperiod = 4629;
    int pulsemin = 280;

    // Configure Output Functions (Table 10-4)
    RPOR5bits.RP11R  = 18;              // Assign OC1 to Pin RP11 (RD0)
    RPOR12bits.RP24R = 19;              // Assign OC2 to Pin RP24 (RD1)
    RPOR11bits.RP23R = 20;              // Assign OC3 to Pin RP23 (RD2)
    RPOR11bits.RP22R = 21;              // Assign OC4 to Pin RP22 (RD3)

    //Set up OC1
    OC1CON1             = 0x0000;       //Clear control registers
    OC1CON2             = 0x0000;       //Clear control registers
    OC1RS               = pulsemin;     //Load secondary register with initial pulse value
    OC1R                = pulsemin;     //Load primary register with initial pulse value
    OC1CON1bits.OCTSEL  = 0b000;        //Select Timer 2 as source
    OC1CON2bits.SYNCSEL = 0b01100;      //Select Timer 2 as sync
    OC1CON1bits.OCM     = 0b110;        //Select Edge aligned PWM
    OC1CON2bits.OCTRIG  = 0;


    //Set up OC2
    OC2CON1             = 0x0000;       //Clear control registers
    OC2CON2             = 0x0000;       //Clear control registers
    OC2RS               = pulsemin;     //Load secondary register with initial pulse value
    OC2R                = pulsemin;     //Load primary register with initial pulse value
    OC2CON1bits.OCTSEL  = 0b000;        //Select Timer 2 as source
    OC2CON2bits.SYNCSEL = 0b01100;      //Select Timer 2 as sync
    OC2CON1bits.OCM     = 0b110;        //Select Edge aligned PWM
    OC2CON2bits.OCTRIG  = 0;

    //Set up OC3
    OC3CON1             = 0x0000;       //Clear control registers
    OC3CON2             = 0x0000;       //Clear control registers
    OC3RS               = pulsemin;     //Load secondary register with initial pulse value
    OC3R                = pulsemin;     //Load primary register with initial pulse value
    OC3CON1bits.OCTSEL  = 0b000;        //Select Timer 2 as source
    OC3CON2bits.SYNCSEL = 0b01100;      //Select Timer 2 as sync
    OC3CON1bits.OCM     = 0b110;        //Select Edge aligned PWM
    OC3CON2bits.OCTRIG  = 0;

    //Set up OC4
    OC4CON1             = 0x0000;       //Clear control registers
    OC4CON2             = 0x0000;       //Clear control registers
    OC4RS               = pulsemin;     //Load secondary register with initial pulse value
    OC4R                = pulsemin;     //Load primary register with initial pulse value
    OC4CON1bits.OCTSEL  = 0b000;        //Select Timer 2 as source
    OC4CON2bits.SYNCSEL = 0b01100;      //Select Timer 2 as sync
    OC4CON1bits.OCM     = 0b110;        //Select Edge aligned PWM
    OC4CON2bits.OCTRIG  = 0;

    //Set up Timer2, our timer for output PWM
    TMR2                = 0;            //Set Timer2 to 0
    PR2                 = pulseperiod;  //Load number of increments in period
    T2CONbits.TCKPS     = 0b10;         //Prescale value is 64
    IFS0bits.T2IF       = 0;            //Clear Timer2 interrupt flag
    IEC0bits.T2IE       = 1;		//Enable Timer2 interrupts
    T2CONbits.TON       = 1;            //Turn Timer2 on
}

void initializePWMcapture(void)
{
    // Configure Output Functions (Table 10-4)

    RPINR7bits.IC1R     = 2;        //Assign Input Capture 1 to RP2 (RD8)
    RPINR7bits.IC2R     = 4;        //Assign Input Capture 2 to RP4 (RD9)
    RPINR8bits.IC3R     = 3;        //Assign Input Capture 3 to RP3 (RD10)
    RPINR8bits.IC4R     = 12;       //Assign Input Capture 4 to RP12 (RD11)
    RPINR9bits.IC5R     = 42;       //Assign Input Capture 5 to RPI42 (RD12)

    //Initalize IC1
    TRISDbits.TRISD8    = 1;        //Set RD8 to input
    IC1CON1bits.ICM     = 0b000;    //This should reset the overflow condition flag, reset the FIFO to empty, and reset the prescale count
    IC1CON1bits.ICTSEL  = 0b010;    //Select Timer4 as timer
    IC1CON1bits.ICM     = 0b01;     //Capture timer value on every edge
    IC1CON1bits.ICI     = 0b00;     //Interrupt on every capture event
    IC1CON2bits.SYNCSEL = 0b01110;  //Sync with Timer4
    IC1CON2bits.ICTRIG  = 0;        //Clear trigger bit for synchronous mode.
    IEC0bits.IC1IE      = 1;        //Enable IC1 interrupt
    IFS0bits.IC1IF      = 0;        //Set interrupt flag status to 0
    
    //Initalize IC2
    TRISDbits.TRISD9    = 1;        //Set RD9 to input
    IC2CON1bits.ICM     = 0b000;    //This should reset the overflow condition flag, reset the FIFO to empty, and reset the prescale count
    IC2CON1bits.ICTSEL  = 0b010;    //Select Timer4 as timer
    IC2CON1bits.ICM     = 0b01;     //Capture timer value on every edge
    IC2CON1bits.ICI     = 0b00;     //Interrupt on every capture event
    IC2CON2bits.SYNCSEL = 0b01110;  //Sync with Timer4
    IC2CON2bits.ICTRIG  = 0;        //Clear trigger bit for synchronous mode.
    IEC0bits.IC2IE      = 1;        //Enable IC2 interrupt
    IFS0bits.IC2IF      = 0;        //Set interrupt flag status to 0
    
    //Initialize IC3
    TRISDbits.TRISD10    = 1;        //Set RD10 to input
    IC3CON1bits.ICM     = 0b000;    //This should reset the overflow condition flag, reset the FIFO to empty, and reset the prescale count
    IC3CON1bits.ICTSEL  = 0b010;    //Select Timer4 as timer
    IC3CON1bits.ICM     = 0b01;     //Capture timer value on every edge
    IC3CON1bits.ICI     = 0b00;     //Interrupt on every capture event
    IC3CON2bits.SYNCSEL = 0b01110;  //Sync with Timer4
    IC3CON2bits.ICTRIG  = 0;        //Clear trigger bit for synchronous mode.
    IEC2bits.IC3IE      = 1;        //Enable IC3 interrupt
    IFS2bits.IC3IF      = 0;        //Set interrupt flag status to 0

    //Initialize IC4
    TRISDbits.TRISD11   = 1;        //Set RD11 to input
    IC4CON1bits.ICM     = 0b000;    //This should reset the overflow condition flag, reset the FIFO to empty, and reset the prescale count
    IC4CON1bits.ICTSEL  = 0b010;    //Select Timer4 as timer
    IC4CON1bits.ICM     = 0b01;     //Capture timer value on every edge
    IC4CON1bits.ICI     = 0b00;     //Interrupt on every capture event
    IC4CON2bits.SYNCSEL = 0b01110;  //Sync with Timer4
    IC4CON2bits.ICTRIG  = 0;        //Clear trigger bit for synchronous mode.
    IEC2bits.IC4IE      = 1;        //Enable IC5 interrupt
    IFS2bits.IC4IF      = 0;        //Set interrupt flag status to 0

    //Initialize IC5
    TRISDbits.TRISD12   = 1;        //Set RD12 to input
    IC5CON1bits.ICM     = 0b000;    //This should reset the overflow condition flag, reset the FIFO to empty, and reset the prescaler count
    IC5CON1bits.ICTSEL  = 0b010;    //Select Timer4 as timer
    IC5CON1bits.ICM     = 0b01;     //Capture timer value on every edge
    IC5CON1bits.ICI     = 0b00;     //Interrupt on every capture event
    IC5CON2bits.SYNCSEL = 0b01110;  //Sync with Timer4
    IC5CON2bits.ICTRIG  = 0;        //Clear trigger bit for synchronous mode.
    IEC2bits.IC5IE      = 1;        //Enable IC5 interrupt
    IFS2bits.IC5IF      = 0;        //Set interrupt flag status to 0

    //Set up Timer4, our clock for PWM
    //IFS1bits.T4IF       = 0;        // Commented out because I couldn't think of a reason to need the interrupt
    //IEC1bits.T4IE       = 1;        // Enable Timer4 Compare interrupts
    T4CONbits.TON       = 1;        // Start Timer2 with assumed settings
    T4CONbits.TCKPS     = 0b10;     // Timer4 Prescale value = 64
    TMR4                = 0;        // Set value of Timer4 to 0
}

void __attribute__((interrupt, shadow, no_auto_psv)) _IC1Interrupt()
{
    IFS0bits.IC1IF = 0; //Reset respective interrupt flag
    
    if (PORTDbits.RD8 == 1)     //If the input is high, start count
    {
        IC1Capture1 = IC1BUF;   //Load the start value from the capture buffer
    }
    
    else if (PORTDbits.RD8 == 0)    //If the input is low, end count
    {
        IC1Capture2 = IC1BUF;   //Load the end value from the capture buffer
    }

    CH1count = (IC1Capture2 - IC1Capture1); //Running time is ent time minus start time

        if (CH1count < 0)               //Verify integrity of count
        {CH1count = CH1count + 4619;}
}

void __attribute__((interrupt, shadow, no_auto_psv)) _IC2Interrupt()
{
    IFS0bits.IC2IF = 0; //Reset respective interrupt flag

    if (PORTDbits.RD9 == 1)
    {
        IC2Capture1 = IC2BUF;
    }

    else if (PORTDbits.RD9 == 0)
    {
        IC2Capture2 = IC2BUF;
    }

    CH2count = (IC2Capture2 - IC2Capture1);

    if (CH2count < 0)               //Verify integrity of count
    CH2count = CH2count;
}

void __attribute__((interrupt, shadow, no_auto_psv)) _IC3Interrupt()
{
    IFS2bits.IC3IF = 0; //Reset respective interrupt flag

    if (PORTDbits.RD10 == 1)
    {
        IC3Capture1 = IC3BUF;
    }

    else if (PORTDbits.RD10 == 0)
    {
        IC3Capture2 = IC3BUF;
    }

    CH3count = (IC3Capture2 - IC3Capture1);

    if (CH3count < 0)               //Verify integrity of count
    CH3count = CH3count + 4619;
}

void __attribute__((interrupt, shadow, no_auto_psv)) _IC4Interrupt()
{
    IFS2bits.IC4IF = 0; //Reset respective interrupt flag

    if (PORTDbits.RD11 == 1)
    {
        IC4Capture1 = IC4BUF;
    }

    else if (PORTDbits.RD11 == 0)
    {
        IC4Capture2 = IC4BUF;
    }

    CH4count = (IC4Capture2 - IC4Capture1);

    if (CH4count < 0)               //Verify integrity of count
    CH4count = CH4count + 4619;
}

void __attribute__((interrupt, shadow, no_auto_psv)) _IC5Interrupt()
{
    IFS2bits.IC5IF = 0; //Reset respective interrupt flag

    if (PORTDbits.RD12 == 1)
    {
        IC5Capture1 = IC5BUF;
    }

    else if (PORTDbits.RD12 == 0)
    {
        IC5Capture2 = IC5BUF;
    }

    CH5count = (IC5Capture2 - IC5Capture1);

    if (CH5count < 0)               //Verify integrity of count
    CH5count = CH5count + 4619;
}

void __attribute__((interrupt, shadow, no_auto_psv)) _T2Interrupt()
{
    IFS0bits.T2IF = 0;
}

void __attribute__((interrupt, shadow, no_auto_psv)) _T4Interrupt()
{
    IFS1bits.T4IF = 0;
}

void setPower(int one, int two, int three, int four)  //Function takes motor thrust commands from 0 to 100% and outputs OCxR register value
{
    OC1R = (one) + 280;      //Scale up the percentage to register value
    OC2R = (two) + 280;
    OC3R = (three) + 280;
    OC4R = (four) + 280;
}

float readCH1(int count)            //Takes captured value and translates it to percentage 0 to 100
{
    float value = 0;                  //Placeholder variable
    if (count > 500 || count < 250) //Check intergrity of value, if out of 6-10.5% range, just write it as low value, 0%
    {
        value = 0;
    }
    else                            //If it's good, compute its percentage range
    {
        value = 0.510126*count - 145.38591;
    }
    return value;                   //Return that the percentage
}

float readCH2(int count)            //Takes captured value and translates it to percentage 0 to 100
{
    float value = 0;                  //Placeholder variable
    if (count > 500 || count < 250) //Check intergrity of value, if out of 6-10.5% range, just write it as low value, 0%
    {
        value = 0;
    }
    else                            //If it's good, compute its percentage range
    {
        value = 0.50505051*count - 139.39394;
    }
    return value;                   //Return that the percentage
}

float readCH3(int count)            //Takes captured value and translates it to percentage 0 to 100
{
    float value = 0;                  //Placeholder variable
    if (count > 500 || count < 250) //Check intergrity of value, if out of 6-10.5% range, just write it as low value, 0%
    {
        value = 0;
    }
    else                            //If it's good, compute its percentage range
    {
        value = 0.50761421*count - 141.62437;
    }
    return value;                   //Return that the percentage
}

float readCH4(int count)            //Takes captured value and translates it to percentage 0 to 100
{
    float value = 0;                  //Placeholder variable
    if (count > 500 || count < 250) //Check intergrity of value, if out of 6-10.5% range, just write it as low value, 0%
    {
        value = 0;
    }
    else                            //If it's good, compute its percentage range
    {
        value = 0.51282051*count - 144.61538;
    }
    return value;                   //Return that the percentage
}
void readCH5(void)
{
    if (CH5count > 500 && CH5count < 530)
        {
            CH5status = 1;
        }
    else if (CH5count < 400 && CH5count > 230)
        {
            CH5status = 0;
        }
}

void getPWM(void)
{

    CH1value = readCH1(CH1count);                //Read in the input channels
    CH2value = readCH1(CH2count);
    CH3value = readCH1(CH3count);
    CH4value = readCH1(CH4count);
    readCH5();
}