Timerservo

/* Servo Pulse Reader and Generator                     */
/* Uses Timer3 for pulse gen, timer 4 for pulse read    */
/* Coded by N. Playle                                   */
/* January 20 2012                                      */


#define TIMER_MONITOR_CHANNELS (4)

HardwareTimer timer(3);
HardwareTimer timer4(4);

// Define the values to hold the input data
int SER_CH_IN_1 = 0;
int SER_CH_IN_2 = 0;
int SER_CH_IN_3 = 0;
int SER_CH_IN_4 = 0;


int timesTrigd = 0; // holds how many timers have been read
int polarity = 0;   // capture rising or falling edge of pulse on CC channels
int temp = 0;       // dummy value to clear interrupt flag



void setup() {


/* TIMER 3 IS USED TO OUTPUT PULSES EVERY 20MSEC */
    // Set all servo outputs as PWM pins
    pinMode(12, PWM);
    pinMode(11, PWM);
    pinMode(27, PWM);
    pinMode(28, PWM);
    pinMode(7, OUTPUT);
    //Pause the timer while we're configuring it
    timer.pause();

    // // Set up period
    timer.setPeriod(20000); // in microseconds
    timer_set_prescaler(TIMER3, 21); // A prescaler of 21 gives a period of ~20msec
    // // Refresh the timer's count, prescale, and overflow
    timer.refresh();

    // Start the timer counting
    timer.resume();
    // Generate some pulses. One count ~= 0.3uSec
    pwmWrite(12,2638);
    pwmWrite(11,5000);
    pwmWrite(27,3500);
    pwmWrite(28,4000);


/*TIMER 4 WILL BE USED TO MEASURE THE PULSES FROM TIMER3 */

    // Configure timer4[4..1] as inputs
    pinMode(5, INPUT_PULLDOWN);
    pinMode(9, INPUT_PULLDOWN);
    pinMode(24, INPUT_PULLDOWN);
    pinMode(14, INPUT_PULLDOWN);
    TIMER4_BASE->CCMR1 = 0xF1F1;    //channel 2,1 configure as input, IC2,IC1 mapped on TI2,TI1
    TIMER4_BASE->CCMR2 = 0xF1F1;    //channel 4,3 configure as input, IC4,IC3 mapped on TI4,TI3
    TIMER4_BASE->CCER = 0x1111;     //configure CC channels [4..1] as an input
    TIMER4_BASE->SMCR = 0x0000;     //all slave modes disabled, CC transitions on clock
    TIMER4_BASE->CR1 = 0x0001;      //enable the counter
    TIMER4_BASE->CR2 = 0x0000;      //connect TIM4_CH1 pin to TI1 input
    TIMER4_BASE->PSC = 0x0015;      //set PRESCALER = 21 (0x15) FOR SERVO, 50HZ

    // Attach the interrupts to the servos. Maybe combine in future?
    timer_attach_interrupt(TIMER4, TIMER_CC1_INTERRUPT, __measurePulse_irq);
    timer_attach_interrupt(TIMER4, TIMER_CC2_INTERRUPT, __measurePulse_irq2);
    timer_attach_interrupt(TIMER4, TIMER_CC3_INTERRUPT, __measurePulse_irq3);
    timer_attach_interrupt(TIMER4, TIMER_CC4_INTERRUPT, __measurePulse_irq4);
    // Restart timer4
    timer_resume(TIMER4);


}

void loop() {
    delay(100);

    //prints the resulting pulse width
    SerialUSB.print("ax");
    SerialUSB.println(SER_CH_IN_1);
    SerialUSB.print("ay");
    SerialUSB.println(SER_CH_IN_2);
    SerialUSB.print("az");
    SerialUSB.println(SER_CH_IN_3);
    SerialUSB.print("gx");
    SerialUSB.println(SER_CH_IN_4);

    pwmWrite(12,SER_CH_IN_1);
    pwmWrite(11,SER_CH_IN_2);
    pwmWrite(27,SER_CH_IN_3);
    pwmWrite(28,SER_CH_IN_4);
}

void __measurePulse_irq()
{
    //get the bit to see if this is an up or down pulse
    polarity = TIMER4_BASE->CCER & 0x0002;

    if(polarity == 0)   //this is a rising edge capture
    {
        timer_set_count(TIMER4, 0x0000);        //zero the counter
        temp = TIMER4_BASE->CCR1;       //clear the interrupt by reading CCR1 SER_CH_IN_
        TIMER4_BASE->CCER |= 0x0002;        //change polarity to capture falling edge
    }
    else    //this is a falling edge capture
    {
        SER_CH_IN_1 = TIMER4_BASE->CCR1;        //read the captured SER_CH_IN_

        TIMER4_BASE->CCER &= ~0x0002;       //change polarity to capture rising edge
        if (TIMER_MONITOR_CHANNELS <= timesTrigd + 1){
            timer_set_count(TIMER4, 0x0000);    //zero the counter
            timesTrigd =0;
        }
        else{
            timesTrigd++;
        }
    }
}

void __measurePulse_irq2()
{
    //get the bit to see if this is an up or down pulse
    polarity = TIMER4_BASE->CCER & 0x0020;

    if(polarity == 0)   //this is a rising edge capture
    {
        timer_set_count(TIMER4, 0x0000);        //zero the counter
        //temp = TIMER4_BASE->CCR1;     //clear the interrupt by reading CCR1 SER_CH_IN_
        temp = TIMER4_BASE->CCR2;       // clear ccr2 trigger if it was
        TIMER4_BASE->CCER |= 0x0020;        //change polarity to capture falling edge
    }
    else    //this is a falling edge capture
    {
        SER_CH_IN_2 = TIMER4_BASE->CCR2;        //read the captured SER_CH_IN_
        TIMER4_BASE->CCER &= ~0x0020;       //change polarity to capture rising edge
        if (TIMER_MONITOR_CHANNELS <= timesTrigd + 1){
            timer_set_count(TIMER4, 0x0000);    //zero the counter
            timesTrigd =0;
        }
        else{
            timesTrigd++;
        }
    }
}

void __measurePulse_irq3()
{
    //get the bit to see if this is an up or down pulse
    polarity = TIMER4_BASE->CCER & 0x0200;

    if(polarity == 0)   //this is a rising edge capture
    {
        timer_set_count(TIMER4, 0x0000);        //zero the counter
        temp = TIMER4_BASE->CCR3;       // clear ccr2 trigger if it was
        TIMER4_BASE->CCER |= 0x0200;        //change polarity to capture falling edge
    }
    else    //this is a falling edge capture
    {
        SER_CH_IN_3 = TIMER4_BASE->CCR3;        //read the captured SER_CH_IN_
        TIMER4_BASE->CCER &= ~0x0200;       //change polarity to capture rising edge
        if (TIMER_MONITOR_CHANNELS <= timesTrigd + 1){
            timer_set_count(TIMER4, 0x0000);    //zero the counter
            timesTrigd =0;
        }
        else{
            timesTrigd++;
        }
    }
}

void __measurePulse_irq4()
{
    //get the bit to see if this is an up or down pulse
    polarity = TIMER4_BASE->CCER & 0x2000;

    if(polarity == 0)   //this is a rising edge capture
    {
        timer_set_count(TIMER4, 0x0000);        //zero the counter
        //temp = TIMER4_BASE->CCR1;     //clear the interrupt by reading CCR1 SER_CH_IN_
        temp = TIMER4_BASE->CCR4;       // clear ccr2 trigger if it was
        TIMER4_BASE->CCER |= 0x2000;        //change polarity to capture falling edge
    }
    else    //this is a falling edge capture
    {
        SER_CH_IN_4 = TIMER4_BASE->CCR4;        //read the captured SER_CH_IN_
        TIMER4_BASE->CCER &= ~0x2000;       //change polarity to capture rising edge
        if (TIMER_MONITOR_CHANNELS <= timesTrigd + 1){
            timer_set_count(TIMER4, 0x0000);    //zero the counter
            timesTrigd =0;
        }
        else{
            timesTrigd++;
        }
    }
}