// // Date init 14.12.2004 // // Revision date $Date:: 14-11-07 12:40 $ // // Filename $Workfile:: d_output.r $ // // Version $Revision:: 1 $ // // Archive $Archive:: /LMS2006/Sys01/Main_V02/Firmware/Source/d_outp $ // // Platform C // #ifdef SAM7S256 #if defined (PROTOTYPE_PCB_3) || (PROTOTYPE_PCB_4) #define MOTOR_A_DIR AT91C_PIO_PA1 #define MOTOR_A_INT AT91C_PIO_PA15 #define MOTOR_B_DIR AT91C_PIO_PA9 #define MOTOR_B_INT AT91C_PIO_PA26 #define MOTOR_C_DIR AT91C_PIO_PA8 #define MOTOR_C_INT AT91C_PIO_PA0 #else #define MOTOR_A_DIR AT91C_PIO_PA1 #define MOTOR_A_INT AT91C_PIO_PA15 #define MOTOR_B_DIR AT91C_PIO_PA9 #define MOTOR_B_INT AT91C_PIO_PA26 #define MOTOR_C_DIR AT91C_PIO_PA8 #define MOTOR_C_INT AT91C_PIO_PA27 #endif #define FORWARD 0x01 #define REVERSE -0x01 #define TIMER_0_ID12 (1L << AT91C_ID_TC0) #define TIMER_1_ID13 (1L << AT91C_ID_TC1) #define TIMER_2_ID14 (1L << AT91C_ID_TC2) typedef struct { SLONG TachoCountTable; SLONG TachoCountTableOld; SBYTE MotorDirection; }TACHOPARAMETERS; static TACHOPARAMETERS MotorTachoValue[3]; #define OUTPUTInit {\ UBYTE Tmp;\ for (Tmp = 0; Tmp < NOS_OF_AVR_OUTPUTS; Tmp++)\ {\ IoToAvr.PwmValue[Tmp] = 0;\ }\ IoToAvr.OutputMode = 0x00;\ IoToAvr.PwmFreq = 8;\ } #define INSERTSpeed(Motor, Speed) IoToAvr.PwmValue[Motor] = Speed #define INSERTMode(Motor, Mode) if (Mode & 0x02)\ {\ IoToAvr.OutputMode |= (0x01 << Motor);\ }\ else\ {\ IoToAvr.OutputMode &= ~(0x01 << Motor);\ } #define ENABLEDebugOutput {\ *AT91C_PIOA_PER = 0x20000000; /* Enable PIO on PA029 */\ *AT91C_PIOA_OER = 0x20000000; /* PA029 set to Output */\ } #define SETDebugOutputHigh *AT91C_PIOA_SODR = 0x20000000 #define SETDebugOutputLow *AT91C_PIOA_CODR = 0x20000000 #define ENABLECaptureMotorA {\ *AT91C_PIOA_PDR = MOTOR_A_INT; /* Disable PIO on PA15 */\ *AT91C_PIOA_BSR = MOTOR_A_INT; /* Enable Peripheral B on PA15 */\ *AT91C_PIOA_PPUDR = MOTOR_A_INT | MOTOR_A_DIR; /* Disable Pull Up resistor on PA15 & PA1 */\ *AT91C_PIOA_PER = MOTOR_A_DIR; /* Enable PIO on PA1 */\ *AT91C_PIOA_ODR = MOTOR_A_DIR; /* PA1 set to input */\ *AT91C_PIOA_IFER = MOTOR_A_INT | MOTOR_A_DIR; /* Enable filter on PA15 & PA1 */\ *AT91C_PMC_PCER = TIMER_1_ID13; /* Enable clock for TC1*/\ *AT91C_TCB_BMR = AT91C_TCB_TC1XC1S_NONE; /* No external clock signal XC2 */\ *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ *AT91C_TC1_CMR = *AT91C_TC1_CMR & 0xFFFF7FFF; /* Enable capture mode */\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_ABETRG; /* Use external trigger for TIO1*/\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ *AT91C_TC1_CMR = *AT91C_TC1_CMR | AT91C_TC_LDRA_RISING; /* RA loading register set */\ *AT91C_AIC_IDCR = TIMER_1_ID13; /* Irq controller setup */\ AT91C_AIC_SVR[13] = (unsigned int)CaptureAInt; \ AT91C_AIC_SMR[13] = 0x05; /* Enable trigger on level */\ *AT91C_AIC_ICCR = TIMER_1_ID13; /* Clear interrupt register PID13*/\ *AT91C_TC1_IDR = 0xFF; /* Disable all interrupt from TC1 */\ *AT91C_TC1_IER = 0x80; /* Enable interrupt from external trigger */\ *AT91C_AIC_IECR = TIMER_1_ID13; /* Enable interrupt from TC1 */\ *AT91C_TC1_CCR = 0x00; /* Clear registers before setting */\ *AT91C_TC1_CCR = AT91C_TC_CLKEN; /* Enable clock */\ } #define ENABLECaptureMotorB {\ *AT91C_PIOA_PDR = MOTOR_B_INT; /* Disable PIO on PA26 */\ *AT91C_PIOA_BSR = MOTOR_B_INT; /* Enable Peripheral B on PA26 */\ *AT91C_PIOA_PER = MOTOR_B_DIR; /* Enable PIO on PA09 */\ *AT91C_PIOA_PPUDR = MOTOR_B_INT | MOTOR_B_DIR; /* Disable Pull Up resistor on PA26 & PA09 */\ *AT91C_PIOA_ODR = MOTOR_B_DIR; /* PA09 set to input */\ *AT91C_PIOA_IFER = MOTOR_B_INT | MOTOR_B_DIR; /* Enable filter on PA26 & PA09 */\ *AT91C_PMC_PCER = TIMER_2_ID14; /* Enable clock for TC2*/\ *AT91C_TCB_BMR = AT91C_TCB_TC2XC2S_NONE; /* No external clock signal */\ *AT91C_TCB_BCR = 0x0; /* Clear SYNC */\ *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0X00000000; /* Clear all bits in TC1_CMR */\ *AT91C_TC2_CMR = *AT91C_TC2_CMR & 0xFFFF7FFF; /* Enable capture mode */\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_ABETRG; /* Use external trigger for TIO2*/\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ *AT91C_TC2_CMR = *AT91C_TC2_CMR | AT91C_TC_LDRA_RISING; /* RA loading register set */\ *AT91C_AIC_IDCR = TIMER_2_ID14; /* Irq controller setup */\ AT91C_AIC_SVR[14] = (unsigned int)CaptureBInt; \ AT91C_AIC_SMR[14] = 0x05; /* Enable trigger on level */\ *AT91C_AIC_ICCR = TIMER_2_ID14; /* Clear interrupt register PID14*/\ *AT91C_TC2_IDR = 0xFF; /* Disable all interrupt from TC2 */\ *AT91C_TC2_IER = 0x80; /* Enable interrupts from external trigger */\ *AT91C_AIC_IECR = TIMER_2_ID14; /* Enable interrupt from TC2 */\ *AT91C_TC2_CCR = 0x00; /* Clear registers before setting */\ *AT91C_TC2_CCR = AT91C_TC_CLKEN; /* Enable clock */\ } #define ENABLECaptureMotorC {\ *AT91C_PIOA_PDR = MOTOR_C_INT; /* Disable PIO on PA0 */\ *AT91C_PIOA_BSR = MOTOR_C_INT; /* Enable Peripheral B on PA0 */\ *AT91C_PIOA_PER = MOTOR_C_DIR; /* Enable PIO on PA08 */\ *AT91C_PIOA_PPUDR = MOTOR_C_INT | MOTOR_C_DIR; /* Disable Pull Up resistor on PA0 & PA08 */\ *AT91C_PIOA_ODR = MOTOR_C_DIR; /* PA08 set to input */\ *AT91C_PIOA_IFER = MOTOR_C_INT | MOTOR_C_DIR; /* Enable filter on PA26 & PA09 */\ *AT91C_PMC_PCER = TIMER_0_ID12; /* Enable clock for TC0*/\ *AT91C_TCB_BMR = AT91C_TCB_TC0XC0S_NONE; /* No external clock signal */\ *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0X00000000; /* Clear all bits in TC0_CMR */\ *AT91C_TC0_CMR = *AT91C_TC0_CMR & 0xFFFF7FFF; /* Enable capture mode */\ *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_CLKS_TIMER_DIV5_CLOCK; /* Set clock for timer to Clock5 = div 1024*/\ *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_ABETRG; /* Use external trigger for TI0*/\ *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_EEVTEDG_BOTH; /* Trigger on both edges */\ *AT91C_TC0_CMR = *AT91C_TC0_CMR | AT91C_TC_LDRA_RISING; /* RA loading register set */\ *AT91C_AIC_IDCR = TIMER_0_ID12; /* Disable interrupt */\ AT91C_AIC_SVR[12] = (unsigned int)CaptureCInt; \ AT91C_AIC_SMR[12] = 0x05; /* Enable trigger on level */\ *AT91C_AIC_ICCR = TIMER_0_ID12; /* Clear interrupt register PID12*/\ *AT91C_TC0_IDR = 0xFF; /* Disable all interrupt from TC0 */\ *AT91C_TC0_IER = 0x80; /* Enable interrupts from external trigger */\ *AT91C_AIC_IECR = TIMER_0_ID12; /* Enable interrupt from TC0 */\ *AT91C_TC0_CCR = 0x00; /* Clear registers before setting */\ *AT91C_TC0_CCR = AT91C_TC_CLKEN; /* Enable clock */\ } __ramfunc void CaptureAInt(void) { if (*AT91C_TC1_SR & AT91C_TC_MTIOA) { if (*AT91C_PIOA_PDSR & MOTOR_A_DIR) { MotorTachoValue[0].MotorDirection = REVERSE; //Motor is running reverse MotorTachoValue[0].TachoCountTable--; } else { MotorTachoValue[0].MotorDirection = FORWARD; //Motor is running forward MotorTachoValue[0].TachoCountTable++; } } else { if (*AT91C_PIOA_PDSR & MOTOR_A_DIR) { MotorTachoValue[0].MotorDirection = FORWARD; MotorTachoValue[0].TachoCountTable++; } else { MotorTachoValue[0].MotorDirection = REVERSE; MotorTachoValue[0].TachoCountTable--; } } } __ramfunc void CaptureBInt(void) { if (*AT91C_TC2_SR & AT91C_TC_MTIOA) { if (*AT91C_PIOA_PDSR & MOTOR_B_DIR) { MotorTachoValue[1].MotorDirection = REVERSE; //Motor is running reverse MotorTachoValue[1].TachoCountTable--; } else { MotorTachoValue[1].MotorDirection = FORWARD; //Motor is running forward MotorTachoValue[1].TachoCountTable++; } } else { if (*AT91C_PIOA_PDSR & MOTOR_B_DIR) { MotorTachoValue[1].MotorDirection = FORWARD; MotorTachoValue[1].TachoCountTable++; } else { MotorTachoValue[1].MotorDirection = REVERSE; MotorTachoValue[1].TachoCountTable--; } } } //__ramfunc void CaptureBInt(void) //{ // if (((bool)(*AT91C_TC2_SR & AT91C_TC_MTIOA))==((bool)(*AT91C_PIOA_PDSR & MOTOR_B_DIR))) // { // MotorTachoValue[1].MotorDirection = REVERSE; //Motor is running reverse // MotorTachoValue[1].TachoCountTable--; // } // else // { // MotorTachoValue[1].MotorDirection = FORWARD; //Motor is running reverse // MotorTachoValue[1].TachoCountTable++; // } //} __ramfunc void CaptureCInt(void) { if (*AT91C_TC0_SR & AT91C_TC_MTIOA) { if (*AT91C_PIOA_PDSR & MOTOR_C_DIR) { MotorTachoValue[2].MotorDirection = REVERSE; //Motor is running reverse MotorTachoValue[2].TachoCountTable--; } else { MotorTachoValue[2].MotorDirection = FORWARD; //Motor is running forward MotorTachoValue[2].TachoCountTable++; } } else { if (*AT91C_PIOA_PDSR & MOTOR_C_DIR) { MotorTachoValue[2].MotorDirection = FORWARD; MotorTachoValue[2].TachoCountTable++; } else { MotorTachoValue[2].MotorDirection = REVERSE; MotorTachoValue[2].TachoCountTable--; } } } #define OUTPUTExit {\ *AT91C_AIC_IDCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable interrupts for the timers */\ *AT91C_AIC_ICCR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Clear penting interrupt register for timers*/\ *AT91C_PMC_PCDR = TIMER_0_ID12 | TIMER_1_ID13 | TIMER_2_ID14; /* Disable the clock for each of the timers*/\ *AT91C_PIOA_PER = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable PIO on PA15, PA11, PA26, PA09, PA27 & PA08 */\ *AT91C_PIOA_ODR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Set to input PA15, PA11, PA26, PA09, PA27 & PA08 */\ *AT91C_PIOA_PPUDR = MOTOR_A_DIR | MOTOR_A_INT | MOTOR_B_DIR | MOTOR_B_INT | MOTOR_C_DIR | MOTOR_C_INT; /* Enable Pullup on PA15, PA11, PA26, PA09, PA27 & PA08 */\ } #define TACHOCountReset(MotorNr) {\ MotorTachoValue[MotorNr].TachoCountTable = 0;\ MotorTachoValue[MotorNr].TachoCountTableOld = 0;\ } #define TACHOCaptureReadResetAll(MotorDataA,MotorDataB,MotorDataC){\ MotorDataA = (MotorTachoValue[MOTOR_A].TachoCountTable - MotorTachoValue[MOTOR_A].TachoCountTableOld);\ MotorTachoValue[MOTOR_A].TachoCountTableOld = MotorTachoValue[MOTOR_A].TachoCountTable;\ MotorDataB = (MotorTachoValue[MOTOR_B].TachoCountTable - MotorTachoValue[MOTOR_B].TachoCountTableOld);\ MotorTachoValue[MOTOR_B].TachoCountTableOld = MotorTachoValue[MOTOR_B].TachoCountTable;\ MotorDataC = (MotorTachoValue[MOTOR_C].TachoCountTable - MotorTachoValue[MOTOR_C].TachoCountTableOld);\ MotorTachoValue[MOTOR_C].TachoCountTableOld = MotorTachoValue[MOTOR_C].TachoCountTable;\ } #define GetMotorDirection(MotorNr) MotorTachoValue[MotorNr].MotorDirection #endif #ifdef PCWIN #endif