From: asterix Date: Wed, 30 Apr 2008 15:00:29 +0000 (+0000) Subject: Add timer module for stepper driver. X-Git-Tag: 1.0.0~15 X-Git-Url: https://codewiz.org/gitweb?a=commitdiff_plain;h=99c9bbc62b1b6003eefd78bf1e1b83beae4b9f38;hp=211f1718bc7a3553323733eea821a477decf0cc7;p=bertos.git Add timer module for stepper driver. git-svn-id: https://src.develer.com/svnoss/bertos/trunk@1237 38d2e660-2303-0410-9eaa-f027e97ec537 --- diff --git a/bertos/cpu/arm/drv/stepper_at91.c b/bertos/cpu/arm/drv/stepper_at91.c new file mode 100644 index 00000000..eef7793e --- /dev/null +++ b/bertos/cpu/arm/drv/stepper_at91.c @@ -0,0 +1,426 @@ +/** + * \file + * + * + * \brief Stepper driver interface implementation. + * + * This module use the three timer on the at91 family, to generate a + * six periodic variable pwm waveform. The pulse width is fix, and could + * change by setting the STEPPER_DELAY_ON_COMPARE_C define, but you make + * an attention to do this, becouse the pulse width is not exactly + * STEPPER_DELAY_ON_COMPARE_C. The pulse width depend also to latency + * time of cpu to serve an interrupt, this generate an pwm waveform affect + * to noise. This noise not effect the period but only the pulse width, + * becouse the raising edge is generate by hardware comply with the our + * period settings. + * + * Note: is most important to set STEPPER_DELAY_ON_COMPARE_C value minor + * than a interrupt time service, becouse the falling edge must be happen + * inside to inerrupt service to guarantee a correct functionaly of pwm + * generator. + * + * + * \version $Id$ + * + * \author Daniele Basile + */ + +#include "stepper_at91.h" + +#include +#include + +#include +#include + +#include + +#include "appconfig.h" + +/* + * Delay to set C compare to clear output + * on select TIO output + */ +#define STEPPER_DELAY_ON_COMPARE_C 20 + +/* + * Forward declaration for interrupt handler + */ +static void stepper_tc0_irq(void); +static void stepper_tc1_irq(void); +static void stepper_tc2_irq(void); + +///< Static array of timer counter struct for stepper. +static struct TimerCounter stepper_timers[CONFIG_TC_STEPPER_MAX_NUM] = +{ + { //Timer Counter settings for TIOA0 output pin + .timer_id = TC0_ID, + .blk_ctrl_set = TC_NONEXC0, + .chl_mode_reg = &TC0_CMR, + .chl_ctrl_reg = &TC0_CCR, + .comp_effect_mask = TC_ACPA_MASK, + .comp_effect_set = TC_ACPA_SET_OUTPUT, + .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_ACPC_MASK, + .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC0, + .comp_reg = &TC0_RA, + .comp_c_reg = &TC0_RC, + .count_val_reg = &TC0_CV, + .irq_enable_reg = &TC0_IER, + .irq_disable_reg = &TC0_IDR, + .irq_set_mask = BV(TC_CPAS), + .irq_mask_reg = &TC0_IMR, + .isr = stepper_tc0_irq, + .status_reg = &TC0_SR, + .tio_pin = TIOA0, + .callback = NULL, + .motor = NULL, + }, + { //Timer Counter settings for TIOB0 output pin + .timer_id = TC0_ID, + .blk_ctrl_set = TC_NONEXC0, + .chl_mode_reg = &TC0_CMR, + .chl_ctrl_reg = &TC0_CCR, + .comp_reg = &TC0_RB, + .comp_c_reg = &TC0_RC, + .count_val_reg = &TC0_CV, + .comp_effect_mask = TC_BCPB_MASK, + .comp_effect_set = TC_BCPB_SET_OUTPUT, + .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_BCPC_MASK, + .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC0, + .irq_enable_reg = &TC0_IER, + .irq_disable_reg = &TC0_IDR, + .irq_set_mask = BV(TC_CPBS), + .irq_mask_reg = &TC0_IMR, + .isr = stepper_tc0_irq, + .status_reg = &TC0_SR, + .tio_pin = TIOB0, + .callback = NULL, + .motor = NULL, + }, + { //Timer Counter settings for TIOA1 output pin + .timer_id = TC1_ID, + .blk_ctrl_set = TC_NONEXC1, + .chl_mode_reg = &TC1_CMR, + .chl_ctrl_reg = &TC1_CCR, + .comp_reg = &TC1_RA, + .comp_c_reg = &TC1_RC, + .count_val_reg = &TC1_CV, + .comp_effect_mask = TC_ACPA_MASK, + .comp_effect_set = TC_ACPA_SET_OUTPUT, + .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_ACPC_MASK, + .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC1, + .irq_enable_reg = &TC1_IER, + .irq_disable_reg = &TC1_IDR, + .irq_set_mask = BV(TC_CPAS), + .irq_mask_reg = &TC1_IMR, + .isr = stepper_tc1_irq, + .status_reg = &TC1_SR, + .tio_pin = TIOA1, + .callback = NULL, + .motor = NULL, + }, + { //Timer Counter settings for TIOB1 output pin + .timer_id = TC1_ID, + .blk_ctrl_set = TC_NONEXC1, + .chl_mode_reg = &TC1_CMR, + .chl_ctrl_reg = &TC1_CCR, + .comp_reg = &TC1_RB, + .comp_c_reg = &TC1_RC, + .count_val_reg = &TC1_CV, + .comp_effect_mask = TC_BCPB_MASK, + .comp_effect_set = TC_BCPB_SET_OUTPUT, + .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_BCPC_MASK, + .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC1, + .irq_enable_reg = &TC1_IER, + .irq_disable_reg = &TC1_IDR, + .irq_set_mask = BV(TC_CPBS), + .irq_mask_reg = &TC1_IMR, + .isr = stepper_tc1_irq, + .status_reg = &TC1_SR, + .tio_pin = TIOB1, + .callback = NULL, + .motor = NULL, + }, + { //Timer Counter settings for TIOA2 output pin + .timer_id = TC2_ID, + .blk_ctrl_set = TC_NONEXC2, + .chl_mode_reg = &TC2_CMR, + .chl_ctrl_reg = &TC2_CCR, + .comp_reg = &TC2_RA, + .comp_c_reg = &TC2_RC, + .count_val_reg = &TC2_CV, + .comp_effect_mask = TC_ACPA_MASK, + .comp_effect_set = TC_ACPA_SET_OUTPUT, + .comp_effect_clear = TC_ACPA_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_ACPC_MASK, + .comp_effect_c_clear = TC_ACPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC2, + .irq_enable_reg = &TC2_IER, + .irq_disable_reg = &TC2_IDR, + .irq_set_mask = BV(TC_CPAS), + .irq_mask_reg = &TC2_IMR, + .isr = stepper_tc2_irq, + .status_reg = &TC2_SR, + .tio_pin = TIOA2, + .callback = NULL, + .motor = NULL, + }, + { //Timer Counter settings for TIOB2 output pin + .timer_id = TC2_ID, + .blk_ctrl_set = TC_NONEXC2, + .chl_mode_reg = &TC2_CMR, + .chl_ctrl_reg = &TC2_CCR, + .comp_reg = &TC2_RB, + .comp_c_reg = &TC2_RC, + .count_val_reg = &TC2_CV, + .comp_effect_mask = TC_BCPB_MASK, + .comp_effect_set = TC_BCPB_SET_OUTPUT, + .comp_effect_clear = TC_BCPB_CLEAR_OUTPUT, + .comp_effect_c_mask = TC_BCPC_MASK, + .comp_effect_c_clear = TC_BCPC_CLEAR_OUTPUT, + .ext_event_set = TC_EEVT_XC2, + .irq_enable_reg = &TC2_IER, + .irq_disable_reg = &TC2_IDR, + .irq_set_mask = BV(TC_CPBS), + .irq_mask_reg = &TC2_IMR, + .isr = stepper_tc2_irq, + .status_reg = &TC2_SR, + .tio_pin = TIOB2, + .callback = NULL, + .motor = NULL, + } +}; + +/** + * Generic TIO interrupt handler. + */ +INLINE void stepper_tc_tio_irq(struct TimerCounter * t) +{ + // + *t->chl_mode_reg &= ~t->comp_effect_c_mask; + *t->chl_mode_reg |= t->comp_effect_c_clear; + + /* + * Cleat TIO output on c register compare. + * This generate an pulse with variable lenght, this + * depend to delay that interrupt is realy service. + */ + *t->comp_c_reg = *t->count_val_reg + STEPPER_DELAY_ON_COMPARE_C; + + //Call the associate callback + t->callback(t->motor); + + *t->chl_mode_reg &= ~t->comp_effect_c_mask; +} + + +/* + * Interrupt handler for timer counter TCKL0 + */ +static void ISR_FUNC stepper_tc0_irq(void) +{ + /* + * Warning: when we read the status_reg register, we reset it. + * That mean if is occur an interrupt event we can read only + * the last that has been occur. To not miss an interrupt event + * we save the status_reg register and then we read it. + */ + uint32_t status_reg = TC0_SR & TC0_IMR; + + if ((status_reg & BV(TC_CPBS)) && (status_reg & BV(TC_CPAS))) + STEPPER_STROBE_ON; + + if (status_reg & BV(TC_CPAS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOA0]); + + if (status_reg & BV(TC_CPBS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOB0]); + + STEPPER_STROBE_OFF; + /* Inform hw that we have served the IRQ */ + AIC_EOICR = 0; + +} + +/* + * Interrupt handler for timer counter TCKL1 + */ +static void ISR_FUNC stepper_tc1_irq(void) +{ + STEPPER_STROBE_ON_1; + /* + * Warning: when we read the status_reg register, we reset it. + * That mean if is occur an interrupt event we can read only + * the last that has been occur. To not miss an interrupt event + * we save the status_reg register and then we read it. + */ + uint32_t status_reg = TC1_SR & TC1_IMR; + + if (status_reg & BV(TC_CPAS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOA1]); + + if (status_reg & BV(TC_CPBS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOB1]); + + + /* Inform hw that we have served the IRQ */ + AIC_EOICR = 0; + STEPPER_STROBE_OFF_1; +} + + +/* + * Interrupt handler for timer counter TCKL2 + */ +static void ISR_FUNC stepper_tc2_irq(void) +{ + + /* + * Warning: when we read the status_reg register, we reset it. + * That mean if is occur an interrupt event we can read only + * the last that has been occur. To not miss an interrupt event + * we save the status_reg register and then we read it. + */ + uint32_t status_reg = TC2_SR & TC2_IMR; + + STEPPER_STROBE_ON_2; + if (status_reg & BV(TC_CPAS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOA2]); + + if (status_reg & BV(TC_CPBS)) + stepper_tc_tio_irq(&stepper_timers[TC_TIOB2]); + + STEPPER_STROBE_OFF_2; + /* Inform hw that we have served the IRQ */ + AIC_EOICR = 0; + +} + +/** + * Timer couter setup. + * + * This function apply to select timer couter all needed settings. + * Every settings are stored in stepper_timers[]. + */ +void stepper_tc_setup(int index, stepper_isr_t callback, struct Stepper *motor) +{ + ASSERT(index < CONFIG_TC_STEPPER_MAX_NUM); + + motor->timer = &stepper_timers[index]; + + //Disable PIO controller and enable TIO function + TIO_PIO_PDR = BV(motor->timer->tio_pin); + TIO_PIO_ABSR = BV(motor->timer->tio_pin); + + /* + * Sets timer counter in waveform mode. + * We set as default: + * - Waveform mode 00 (see datasheet for more detail.) + * - Master clock prescaler to STEPPER_MCK_PRESCALER + * - Set none external event + * - Clear pin output on comp_reg + * - None effect on reg C compare + */ + *motor->timer->chl_mode_reg = BV(TC_WAVE); + *motor->timer->chl_mode_reg |= motor->timer->ext_event_set; + *motor->timer->chl_mode_reg &= ~TC_WAVSEL_MASK; + *motor->timer->chl_mode_reg |= TC_WAVSEL_UP; + *motor->timer->chl_mode_reg |= STEPPER_MCK_PRESCALER; + *motor->timer->chl_mode_reg |= motor->timer->comp_effect_clear; + *motor->timer->chl_mode_reg &= ~motor->timer->comp_effect_c_mask; + + //Reset comp_reg and C compare register + *motor->timer->comp_reg = 0; + *motor->timer->comp_c_reg = 0; + + //Register interrupt vector + cpuflags_t flags; + IRQ_SAVE_DISABLE(flags); + + /* + * Warning: To guarantee a correct management of interrupt event, we must + * trig the interrupt on level sensitive. This becouse, we have only a common + * line for interrupt request, and if we have at the same time two interrupt + * request could be that the is service normaly but the second will never + * been detected and interrupt will stay active but never serviced. + */ + AIC_SVR(motor->timer->timer_id) = motor->timer->isr; + AIC_SMR(motor->timer->timer_id) = AIC_SRCTYPE_INT_LEVEL_SENSITIVE; + AIC_IECR = BV(motor->timer->timer_id); + + // Disable interrupt on select timer counter + stepper_tc_irq_disable(motor->timer); + + IRQ_RESTORE(flags); + + //Register callback + motor->timer->callback = callback; + motor->timer->motor = motor; +} + +/** + * Timer counter init. + */ +void stepper_tc_init(void) +{ + STEPPER_STROBE_INIT; + + ASSERT(CONFIG_NUM_STEPPER_MOTORS <= CONFIG_TC_STEPPER_MAX_NUM); + + /* + * Enable timer counter: + * - power on all timer counter + * - disable all interrupt + * - disable all external event/timer source + */ + for (int i = 0; i < CONFIG_TC_STEPPER_MAX_NUM; i++) + { + PMC_PCER = BV(stepper_timers[i].timer_id); + *stepper_timers[i].irq_disable_reg = 0xFFFFFFFF; + TC_BMR = stepper_timers[i].blk_ctrl_set; + } + + /* + * Enable timer counter and start it. + */ + for (int i = 0; i < CONFIG_TC_STEPPER_MAX_NUM; i++) + *stepper_timers[i].chl_ctrl_reg = (BV(TC_CLKEN) | BV(TC_SWTRG)); + +} + diff --git a/bertos/cpu/arm/drv/stepper_at91.h b/bertos/cpu/arm/drv/stepper_at91.h new file mode 100644 index 00000000..7f1d4d41 --- /dev/null +++ b/bertos/cpu/arm/drv/stepper_at91.h @@ -0,0 +1,186 @@ +/** + * \file + * + * + * \brief Stepper hardware-specific definitions + * + * \version $Id$ + * + * \author Daniele Basile + */ + + +#include +#include +#include +#include + +/** + * Setting master clock prescaler for all timer couter + * + * You could choise one of these: + * - TC_CLKS_MCK2: Selects MCK / 2 + * - TC_CLKS_MCK8: Selects MCK / 8 + * - TC_CLKS_MCK32: Selects MCK / 32 + * - TC_CLKS_MCK128: Selects MCK / 128 + * - TC_CLKS_MCK1024: Selects MCK / 1024 + */ +#if STEPPER_PRESCALER_LOG2 == 1 + #define STEPPER_MCK_PRESCALER TC_CLKS_MCK2 +#elif STEPPER_PRESCALER_LOG2 == 3 + #define STEPPER_MCK_PRESCALER TC_CLKS_MCK8 +#elif STEPPER_PRESCALER_LOG2 == 5 + #define STEPPER_MCK_PRESCALER TC_CLKS_MCK32 +#elif STEPPER_PRESCALER_LOG2 == 7 + #define STEPPER_MCK_PRESCALER TC_CLKS_MCK128 +#elif STEPPER_PRESCALER_LOG2 == 10 + #define STEPPER_MCK_PRESCALER TC_CLKS_MCK1024 +#else + #error Unsupported stepper prescaler value. +#endif + +/** + * Timer counter hw enumeration. + */ +enum +{ + TC_TIOA0 = 0, + TC_TIOB0, + TC_TIOA1, + TC_TIOB1, + TC_TIOA2, + TC_TIOB2, + + TC_CNT +}; + +/** + * IRQ callback function type definition. + */ +typedef void (*irq_t)(void); + +/** + * Timer contex structure. + */ +typedef struct TimerCounter +{ + int timer_id; ///< Timer counter ID + uint32_t blk_ctrl_set; ///< Control block setting for this timer + reg32_t *chl_mode_reg; ///< Channel mode register + reg32_t *chl_ctrl_reg; ///< Channel control register + reg32_t *comp_reg; ///< Compare register + reg32_t *comp_c_reg; ///< C compare register + reg32_t *count_val_reg; ///< Current timer counter value + uint32_t comp_effect_mask; ///< Bit mask for TIO register compare effects + uint32_t comp_effect_set; ///< Set TIO on register compare event + uint32_t comp_effect_clear; ///< Clear TIO on register compare event + uint32_t comp_effect_c_mask; ///< Bit mask for TIO on C register compare effects + uint32_t comp_effect_c_clear; ///< Clear TIO on C register compare event + uint32_t ext_event_set; ///< Setting for extern event trigger for TIOB + reg32_t *irq_enable_reg; ///< Enable interrupt register + reg32_t *irq_disable_reg; ///< Disable interrupt register + uint32_t irq_set_mask; ///< IRQ flag bit for select TIO + reg32_t *irq_mask_reg; ///< IRQ mask register + irq_t isr; ///< IRQ handler + reg32_t *status_reg; ///< Timer status register + int tio_pin; ///< Timer I/O pin + stepper_isr_t callback; ///< Interrupt callback pointer + struct Stepper *motor; ///< Stepper context structure + +} TimerCounter; + +/** + * Enable interrupt for timer counter compare event. + */ +INLINE void stepper_tc_irq_enable(struct TimerCounter *timer) +{ + *timer->irq_enable_reg = timer->irq_set_mask; +} + +/** + * Disable interrupt for timer counter compare event. + */ +INLINE void stepper_tc_irq_disable(struct TimerCounter *timer) +{ + *timer->irq_disable_reg = timer->irq_set_mask; +} + +/** + * Set delay for next interrupt compare event. + */ +INLINE void stepper_tc_setDelay(struct TimerCounter *timer, stepper_time_t delay) +{ + *timer->comp_reg += delay; +} + + +/** + * Set delay for next interrupt compare event. + */ +INLINE void stepper_tc_resetTimer(struct TimerCounter *timer) +{ + *timer->comp_reg = 0; +} + +/** + * Programm timer counter to generate a pulse on select TIO output. + */ +INLINE void FAST_FUNC stepper_tc_doPulse(struct TimerCounter *timer) +{ + *timer->chl_mode_reg &= ~timer->comp_effect_mask; + *timer->chl_mode_reg |= timer->comp_effect_set; +} + +/** + * Programm timer counter to not generate a pulse on select TIO output. + */ +INLINE void FAST_FUNC stepper_tc_skipPulse(struct TimerCounter *timer) +{ + *timer->chl_mode_reg &= ~timer->comp_effect_mask; +} + +void stepper_tc_setup(int index, stepper_isr_t callback, struct Stepper *motor); +void stepper_tc_init(void); + +/* + * Test the hardware timer counter on board. + * This test generate a square waveform through irq, setting + * the timer register. + */ +void stepper_timer_test_brute(void); +/* + * Test the timer driver structure. + * This test generate a square waveform through irq. + * The irq callback is programmable, and all timer setting + * are save in one data structure. Every pulse is generate through + * a call of this irq callback. + */ +void stepper_timer_test_prestepper(struct Stepper *local_motor, struct StepperConfig *local_cfg, int index); diff --git a/bertos/cpu/arm/drv/stepper_at91_hw_test.c b/bertos/cpu/arm/drv/stepper_at91_hw_test.c new file mode 100644 index 00000000..b50ea2fe --- /dev/null +++ b/bertos/cpu/arm/drv/stepper_at91_hw_test.c @@ -0,0 +1,174 @@ +/** + * \file + * + * + * \brief Low level test for stepper driver interface implementation. + * + * \version $Id$ + * + * \author Daniele Basile + */ + +#include "stepper_at91.h" + +#include +#include + +#include +#include + +#include + +#include "appconfig.h" + +#warning This test are incomplete.. you MUST review.. + + +static void stepper_test_irq_schedule(struct Stepper *motor, stepper_time_t delay) +{ + stepper_tc_doPulse(motor->timer); + stepper_tc_setDelay(motor->timer, delay); +} + +static void stepper_test_irq(struct Stepper *motor) +{ + + stepper_test_irq_schedule(motor, 300); +} +/* + * Test a timer couter driver + */ +void stepper_timer_test_prestepper(struct Stepper *local_motor, struct StepperConfig *local_cfg, int index) +{ + local_cfg->pulse = 300; + + local_motor->cfg = local_cfg; + stepper_tc_init(index, &stepper_test_irq, local_motor); + stepper_tc_irq_enable(local_motor->timer); +} + + +bool su = true; +bool sub = true; +uint16_t periodo_st0 = 100; +uint16_t periodo_st1 = 233; + +static void tc_irq(void) __attribute__ ((interrupt)); +static void tc_irq(void) +{ + uint32_t status_reg = TC2_SR & TC2_IMR; + + if (status_reg & BV(TC_CPAS)) + { + TC2_CMR &= ~TC_ACPA_MASK; + if (su) + { + TC2_CMR |= TC_ACPA_CLEAR_OUTPUT; + TC2_RA += periodo_st0; + } + else + { + TC2_CMR |= TC_ACPA_SET_OUTPUT; + TC2_RA += periodo_st1; + } + su = !su; + } + if (status_reg & BV(TC_CPBS)) + { + TC2_CMR &= ~TC_BCPB_MASK ; + if (sub) + { + TC2_CMR |= TC_BCPB_CLEAR_OUTPUT; + TC2_RB += periodo_st0; + } + else + { + TC2_CMR |= TC_BCPB_SET_OUTPUT; + TC2_RB += periodo_st1; + } + sub = !sub; + } + /* Inform hw that we have served the IRQ */ + AIC_EOICR = 0; +} + +/* + * Test a timer couter hardware + */ +void stepper_timer_test_brute(void) +{ + PIOA_PDR |= BV(26) | BV(27); + PIOA_BSR |= BV(26) | BV(27); + + // Power on TCLK0 + PMC_PCER |= BV(TC2_ID);// | BV(TC1_ID) | BV(TC2_ID); + + TC_BCR = 1; + TC_BMR |= TC_NONEXC2; + + // Select waveform mode + TC2_CMR = BV(TC_WAVE); + + TC2_CMR |= TC_EEVT_XC2; + TC2_CMR |= TC_WAVSEL_UP; + TC2_CMR |= TC_CLKS_MCK8; + + //Set waveform on TIOA and TIOB + TC2_CMR |= TC_ACPA_SET_OUTPUT; + TC2_CMR |= TC_BCPB_SET_OUTPUT; + + + //Reset all comp_reg register + TC2_RA = 0; + TC2_RB = 0; + + cpuflags_t flags; + IRQ_SAVE_DISABLE(flags); + + /* Set the vector. */ + AIC_SVR(TC2_ID) = tc_irq; + /* Initialize to edge triggered with defined priority. */ + AIC_SMR(TC2_ID) = AIC_SRCTYPE_INT_EDGE_TRIGGERED; + /* Enable the USART IRQ */ + AIC_IECR = BV(TC2_ID); + + IRQ_RESTORE(flags); + + // Disable all interrupt + TC2_IDR = 0xFFFFFFFF; + + //Enable interrupt on RA, RB + TC2_IER = BV(TC_CPAS) | BV(TC_CPBS); + + //Enable timer and trig it + TC2_CCR = BV(TC_CLKEN) | BV(TC_SWTRG); +} +