void InitApp(void) { // activation de la priorité des interruptions _NSTDIS = 0; //Init des E/S _TRISA0 = 0; _TRISA1 = 0; led = 0; led1 = 0; OpenTimer2(T2_ON & T2_GATE_OFF & T2_PS_1_256 & T2_32BIT_MODE_OFF & T2_SOURCE_INT, 50000); ConfigIntTimer2(T2_INT_PRIOR_2 & T2_INT_ON); //Init debug on UART, TX->RP8, RX->RP9 InitDebug(8,9); //Init the Servo Lib InitLibServo(); ajouterServo('B',2); //Initialise RB2, pas obligatoire ajouterServo('B',3); //Initialise RB3, pas obligatoire __delay_ms(100); modifierServoPeriod('B',2,0.0015); //1.5ms, milieu modifierServoPeriod('B',3,0.002); //2ms, droite }
/********************************************************************* * Function: * * PreCondition: * * Input: * * Output: * * Side Effects: * * Overview: * * Note: ********************************************************************/ void MSTimerInit(unsigned int pbclock) { UINT period; UINT i; period = pbclock / 1000; // number of ticks to a ms; i = 0; while((period > 0xFFFF) && (i < 7)) { period >>= 1; i++; } if(i == 7) { period = pbclock / 1000; period /= 256; } i <<= _T2CON_TCKPS_POSITION; OpenTimer2((T2_ON | i), period); ConfigIntTimer2((T2_INT_PRIOR_1)); _MSTimer = 0; _MSTimer32 = 0; }
int main ( void ) { /* 全てのA/D変換ポートをI/Oに使用できるように設定 */ AD1PCFG = 0xffff; /* クロックをPLL prescaler (1:1)で使用する */ CLKDIV = 0x0000; /* PORTB RB15を出力に設定 */ mPORTBOutputConfig(IOPORT_BIT_15); /* PORTBに0を出力 */ mPORTBWrite(0); /* タイマー割り込みを有効に */ ConfigIntTimer2(T2_INT_ON|T2_INT_PRIOR_1); /* タイマーを設定 */ OpenTimer2(T2_ON | T2_PS_1_64 ,0x9000); /* メインループ */ while (1) { /* IDLEモードに移行 */ mPWRMGNT_GotoIdleMode(); } }
extern void timerStart(timer * pTimer, const timerCalc * pTimerCalc) { pTimer->m_TimerCalc = *pTimerCalc; const uint16_t PrescalerBits = pTimer->m_TimerCalc.PrescalerBits; const uint16_t PriorityBits = pTimer->m_TimerCalc.PriorityBits; const uint16_t Ticks = pTimer->m_TimerCalc.Ticks; const timer_tCallback pCallback = pTimer->m_pCallback; pTimer->m_OverflowCount = pTimer->m_TimerCalc.OverflowCount; switch (pTimer->m_TimerNumber) { case 1: OpenTimer1(T1_ON | T1_SOURCE_INT | PrescalerBits, Ticks); ConfigIntTimer1((pCallback == NULL ? T1_INT_OFF : T1_INT_ON) | PriorityBits); break; case 2: OpenTimer2(T2_ON | T2_SOURCE_INT | PrescalerBits, Ticks); ConfigIntTimer2((pCallback == NULL ? T2_INT_OFF : T2_INT_ON) | PriorityBits); break; case 3: OpenTimer3(T3_ON | T3_SOURCE_INT | PrescalerBits, Ticks); ConfigIntTimer3((pCallback == NULL ? T3_INT_OFF : T3_INT_ON) | PriorityBits); break; case 4: OpenTimer4(T4_ON | T4_SOURCE_INT | PrescalerBits, Ticks); ConfigIntTimer4((pCallback == NULL ? T4_INT_OFF : T4_INT_ON) | PriorityBits); break; case 5: OpenTimer5(T5_ON | T5_SOURCE_INT | PrescalerBits, Ticks); ConfigIntTimer5((pCallback == NULL ? T5_INT_OFF : T5_INT_ON) | PriorityBits); break; } }
void vSetupTimerTest( unsigned short usFrequencyHz ) { /* Remember the frequency so it can be used from the ISR. */ ulFrequencyHz = ( unsigned long ) usFrequencyHz; /* T2 is used to generate interrupts above the kernel and max syscall interrupt priority. */ T2CON = 0; TMR2 = 0; /* Timer 2 is going to interrupt at usFrequencyHz Hz. */ PR2 = ( unsigned short ) ( ( configPERIPHERAL_CLOCK_HZ / ( unsigned long ) usFrequencyHz ) - 1 ); /* Setup timer 2 interrupt priority to be above the kernel priority so the timer jitter is not effected by the kernel activity. */ ConfigIntTimer2( T2_INT_ON | ( configMAX_SYSCALL_INTERRUPT_PRIORITY + 1 ) ); /* Clear the interrupt as a starting condition. */ IFS0bits.T2IF = 0; /* Enable the interrupt. */ IEC0bits.T2IE = 1; /* Start the timer. */ T2CONbits.TON = 1; }
int main(void){ mPORTBSetPinsAnalogIn(0xFFFF); //Enable all analog mPORTDClearBits(BIT_0 |BIT_1 | BIT_8); mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_8); SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); CloseADC10(); SetChanADC10(INITCH); OpenADC10(CONFIG1, CONFIG2, CONFIG3, CFGPORT, CFGSCAN); ConfigIntADC10(CFGINT); EnableADC10(); mPMPOpen(PMP_CONTROL, PMP_MODE, PMP_PORT, PMP_INT); mPMPEnable(); //mPORTDClearBits(BIT_0 |BIT_1 | BIT_8); INTEnableSystemMultiVectoredInt(); OpenTimer2(T2_ON | T2_PS_1_64, 256); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_5 | T2_INT_SUB_PRIOR_2); EnableIntT2; mPORTDToggleBits(BIT_0); while(1){ //Blink so we know we're still running noteOn(67, 100); //delayMs(1000); mPORTDToggleBits(BIT_0); //noteOff(67); DelayMs(1000); //mPORTDToggleBits(BIT_0); } return 0; }
void InitTMR2(void) { //On ouvre le Timer2 qui gère l'asservissement toutes les 10ms OpenTimer2(T2_OFF & T2_GATE_OFF & T2_IDLE_CON & T2_SOURCE_INT & T2_PS_1_64 & T2_32BIT_MODE_OFF, 6249); ConfigIntTimer2(T2_INT_PRIOR_4 & T2_INT_ON); //Interruption ON et priorité 3 T2CONbits.TON = 1; // Turn on timer 2 return; }
void setupTimer2() { //timer2 source is internal => on 80Mhz that is 12,5 for one click //multiply by 8 => timer 2 clicks on 100 nS //timer 2 counts 100 clicks and then interrupts on every 100nS * 100 = 10uS) OpenTimer2(T2_OFF | T2_SOURCE_INT | T2_PS_1_8, 100); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2); }
void setServoTimer(int value){ if(value<1) value = 1; if(value>0xfffe) value = 0xfffe; //PR2 = value; OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_256, value); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_7); }
void timer2Init(void (*timerCallbackFcn)(void), unsigned int prescalar) { OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_256, prescalar); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_3); timerCallback = timerCallbackFcn; mT2IntEnable(1); }
/**************************************************************************** Function TIMERS_Init Parameters none Returns None. Description Initializes the timer module Notes None. Author Max Dunne, 2011.11.15 ****************************************************************************/ void TIMERS_Init(void) { TimerActiveFlags = 0; TimerEventFlags = 0; FreeRunningTimer = 0; OpenTimer2(T2_ON & T2_GATE_OFF & T2_SOURCE_INT & T2_PS_1_1, PB_CLOCK / TIMER_FREQUENCY); ConfigIntTimer2(T2_INT_ON & T2_INT_PRIOR_3); EnableIntT2; }
void PWMinit() { // init OC4 & OC5 module OpenOC1( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); OpenOC2( OC_ON | OC_TIMER2_SRC | OC_PWM_FAULT_PIN_DISABLE, 0, 0); // init Timer2 mode and period (PR2) (frequency of 1 / 20 kHz = (3999 + 1) / 80MHz * 1 OpenTimer2( T2_ON | T2_PS_1_1 | T2_SOURCE_INT, 3999); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2); }
void TMR2_Init(void) { #ifdef _TARGET_440H #else // STEP 1. configure the Timer2 OpenTimer2(T2_ON | T2_SOURCE_INT | T2_IDLE_CON | T2_PS_1_256, TMR2_RELOAD); // STEP 2. set the timer interrupt to prioirty level 6 ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_6); mT2ClearIntFlag(); #endif }
void Timer_Asserv_Init(void) { // activation du Timer2 OpenTimer2(T2_ON & T2_IDLE_CON & T2_GATE_OFF & T2_PS_1_64 & T2_SOURCE_INT, 3125 ); // 3125 pour 5ms // configuration des interruptions ConfigIntTimer2(T2_INT_PRIOR_4 & T2_INT_ON); }
//Timer 2 setup function int sysServiceConfigT2(unsigned int T2conval, unsigned int T2perval, unsigned int T2intconval){ //Todo: is there any way to have a compile time semaphore here? if(T2_already_confgured){ return -1; } else{ T2_already_confgured = 1; OpenTimer2(T2conval, T2perval); ConfigIntTimer2(T2intconval); return 0; } }
/********************************************************************* * Function: * * PreCondition: * * Input: * * Output: * * Side Effects: * * Overview: * * Note: ********************************************************************/ void MSTimerInit(unsigned int pbclock) { UINT period; period = pbclock / 1000; // number of ticks to a ms; OpenTimer2((T2_ON), period); ConfigIntTimer2((T2_INT_PRIOR_1)); _MSTimer = 0; }
void initAsservissement() { initAsservVitesse(); initAsservPosition(); setConsigneVitesse(0.0,0.0); setConsignePosition(0.0,0.0); setAsservType(ASSERV_VITESSE); /* On ouvre le Timer2 qui gère l'asservissement toutes les 10ms */ OpenTimer2(T2_OFF & T2_GATE_OFF & T2_IDLE_CON & T2_SOURCE_INT & T2_PS_1_64 & T2_32BIT_MODE_OFF, 6249); ConfigIntTimer2(T2_INT_PRIOR_4 & T2_INT_ON); //Interruption ON et priorité 4 T2CONbits.TON = 1; /* Turn on timer 2 */ }
void modem_hal_start() { ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_6); // The above could also be accomplished by ... ? /* IFS0bits.T2IF = 0; // Clear the T2 interrupt flag, so that the ISR doesn't go off immediatelly IEC0bits.T2IE = 1; // Enable T2 interrupt */ // Turn PTT led on pin_write(43, HIGH); }
/* Specify Interrupt Priority Level = 2 */ void __ISR(_TIMER_2_VECTOR, IPL3) _Timer2Handler(void) { numberOfMillis--; if( 0 >= numberOfMillis) { #ifdef DEBUG sendDataBuffer("timeout reached\n"); #endif setState(STATE_ERROR); ConfigIntTimer2(T2_INT_OFF); int i; for(i=0; i < 5000;i++) {} } mT2ClearIntFlag(); }
void SetupInterrupts(void) { #if defined(__IMAGEPROC2) ConfigINT2(RISING_EDGE_INT & EXT_INT_ENABLE & EXT_INT_PRI_7); // Battery Supervisor #elif defined(__IMAGEPROC1) ConfigINT0(RISING_EDGE_INT & EXT_INT_ENABLE & EXT_INT_PRI_7); // Battery Supervisor #endif ConfigIntTimer1(T1_INT_PRIOR_4 & T1_INT_OFF); ConfigIntTimer2(T2_INT_PRIOR_4 & T2_INT_OFF); }
void TMR2_init() { /** * Initialize timer2 */ #define PB_DIV 1 #define PRESCALE 64 #define T2_TICK (SYS_CLOCK/PB_DIV/PRESCALE/TMR2_TOGGLES_PER_SEC) OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_64, T2_TICK); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_6); INTEnableSystemMultiVectoredInt(); TMR2_ticks = 0; note_count = 0; note_stop = -1; }
int TimerInit(void) { //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ //STEP 1. Configure cache, wait states and peripheral bus clock // Configure the device for maximum performance but do not change the PBDIV // Given the options, this function will change the flash wait states, RAM // wait state and enable prefetch cache but will not change the PBDIV. // The PBDIV value is already set via the pragma FPBDIV option above.. // SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~ // STEP 2. configure Timer 1 using internal clock, 1:256 prescale //OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_256, T2_TICK); OpenTimer2(T2_ON | T2_SOURCE_INT, T2_TICK); // set up the timer interrupt with a priority of 2 ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2); // enable multi-vector interrupts INTEnableSystemMultiVectoredInt(); // shake sensor debug pin TRISBbits.TRISB8 = 0; IEC0bits.INT4IE=0; // disable this interrupt // AN6/RPC0/RC0 == IR recv // config as input TRISCbits.TRISC0 = 1; SYSKEY = 0x0; SYSKEY = 0xAA996655; SYSKEY = 0x556699AA; CFGCONbits.IOLOCK = 0; // unlock configuration INT4Rbits.INT4R = 0b0110; // map RPC0 CFGCONbits.IOLOCK = 1; // relock configuration SYSKEY = 0x0; INTCONbits.INT4EP=0; // edge polarity FALLING EDGE IPC4bits.INT4IP=1; // interrupt priority IPC4bits.INT4IS=1; // interrupt sub priority IEC0bits.INT4IE=1; // enable this interrupt IEC0bits.T2IE=1; }
inline void Timer2_Setup(void) { OpenTimer2( T2_OFF & T2_IDLE_CON & T2_GATE_OFF & T2_PS_1_256 & T2_32BIT_MODE_OFF & T2_SOURCE_INT, 39063); //for 250 ms. ConfigIntTimer2( T2_INT_PRIOR_3 & T2_INT_ON ); return; }
void SetupTimer2(void) { OpenTimer2( T2_ON & T2_IDLE_CON & T2_GATE_OFF & T2_PS_1_256 & T2_32BIT_MODE_OFF & T2_SOURCE_INT, 0x3d08 ); ConfigIntTimer2( T2_INT_PRIOR_1 & T2_INT_ON ); return; }
//-------------------------------------------------------------------------------------------------------------- // Init Timers void Init_Steppers(){ STEPPERS_TRIS &= 0xFE00; // configure PORTB for output (STEPPERS) ADPCFG = 0xFFFF; // configure PORTB for digital output MAIN_SWITCH = 0; // turn on the main switch mosfet /* Enable Timer1 Interrupt and Priority to "5" */ ConfigIntTimer1(T1_INT_PRIOR_6 & T1_INT_ON); OpenTimer1(T1_ON & T1_GATE_OFF & T1_PS_1_8 & T1_SOURCE_INT, 0xFFFF); /* Enable Timer2 Interrupt and Priority to "5" */ ConfigIntTimer2(T2_INT_PRIOR_6 & T2_INT_ON); OpenTimer2(T2_ON & T2_GATE_OFF & T2_PS_1_8 & T2_SOURCE_INT, 0xFFFF); }
void InitApp(void) { // activation de la priorité des interruptions _NSTDIS = 0; //Init des E/S _TRISA0 = 0; led = 0; //Init debug on UART, TX->RP8, RX->RP9 InitDebug(8,9); //Configuration du Output Compare 1 en mode PWM OpenOC1(OC_IDLE_CON & OC_TIMER2_SRC & OC_HIGH_LOW, 20, 20); _RP15R = 18; //OC1(18) sur RP1; //Configuration du Timer 2, période 20ms OpenTimer2(T2_ON & T2_GATE_OFF & T2_PS_1_256 & T2_32BIT_MODE_OFF & T2_SOURCE_INT, 3125); ConfigIntTimer2(T2_INT_ON & T2_INT_PRIOR_4); }
void SetupTickInterrupt( void ) { /* * Configure SysTick to * interrupt at the requested rate. * and start it. */ /* * Turn ON the system clock * */ OpenTimer2(T2_CONFIG, T2_RELOAD); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_4); /* * Turn on the interrupts * */ INTEnableSystemMultiVectoredInt(); OneSecPrescaler = SYSTICKHZ; }
void timersInit() { //T4 is input from encoder. Read from TMR4 to see number of ticks counted. T4CON = 0x0; //T4 disabled T4CONSET = 0x0002; //T4 External Clock Source, 1:1 prescaler TMR4 = 0x0; //Reset T4 counter PR4 = 0xffff; //Set T4 period to max //mT4ClearIntFlag(); //clear interrupt flag //mT4SetIntPriority(7); //set Timer4 Interrupt Priority //mT4IntEnable(1); //enable timer4 interrupts T4CONSET = 0x8000; //T4 enable //T5 is input from encoder. Read from TMR5 to see number of ticks counted. T5CON = 0x0; //T5 disabled T5CONSET = 0x0002; //T5 External Clock Source, 1:1 prescaler TMR5 = 0x0; //Reset T5 PR5 = 0xffff; //Set T5 period to max //mT5ClearIntFlag(); //clear T5 interrupt flag //mT5SetIntPriority(2); //set Timer5 Interrupt Priority //mT5IntEnable(1); //enable timer5 interrupts T5CONSET = 0x8000; //T5 enable //The following code snippet opens and uses Timer1,2,3 as an interrupt. //Open Timer1 with 1:8 prescaler (80MHz -> 10MHz), with period of 1250, therefore tick = 8kHz. OpenTimer1(T1_ON | T1_PS_1_8 | T1_SOURCE_INT, 1000); ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2); OpenTimer2(T2_ON | T2_PS_1_8 | T2_SOURCE_INT, 1000); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2); OpenTimer3(T3_ON | T3_PS_1_8 | T3_SOURCE_INT, 1000); ConfigIntTimer1(T3_INT_ON | T3_INT_PRIOR_2); INTEnableSystemMultiVectoredInt(); }
int main(void) { //LOCALS unsigned int temp; unsigned int channel1, channel2; M1_stepPeriod = M2_stepPeriod = M3_stepPeriod = M4_stepPeriod = 50; // in tens of u-seconds unsigned char M1_state = 0, M2_state = 0, M3_state = 0, M4_state = 0; SYSTEMConfig(GetSystemClock(), SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); /* TIMER1 - now configured to interrupt at 10 khz (every 100us) */ OpenTimer1(T1_ON | T1_SOURCE_INT | T1_PS_1_1, T1_TICK); ConfigIntTimer1(T1_INT_ON | T1_INT_PRIOR_2); /* TIMER2 - 100 khz interrupt for distance measure*/ OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, T2_TICK); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_3); //It is off until trigger /* PORTA b2 and b3 for servo-PWM */ mPORTAClearBits(BIT_2 | BIT_3); mPORTASetPinsDigitalOut(BIT_2 | BIT_3); /* ULTRASONICS: some bits of PORTB for ultrasonic sensors */ PORTResetPins(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11 ); PORTSetPinsDigitalOut(IOPORT_B, BIT_8 | BIT_9| BIT_10 | BIT_11); //trigger /* Input Capture pins for echo signals */ //interrupt on every risging/falling edge starting with a rising edge PORTSetPinsDigitalIn(IOPORT_D, BIT_8| BIT_9| BIT_10| BIT_11); //INC1, INC2, INC3, INC4 Pin mIC1ClearIntFlag(); OpenCapture1( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//front ConfigIntCapture1(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3); OpenCapture2( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//back ConfigIntCapture2(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3); OpenCapture3( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//left ConfigIntCapture3(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3); OpenCapture4( IC_EVERY_EDGE | IC_INT_1CAPTURE | IC_TIMER2_SRC | IC_ON );//right ConfigIntCapture4(IC_INT_ON | IC_INT_PRIOR_4 | IC_INT_SUB_PRIOR_3); /* PINS used for the START (RD13) BUTTON */ PORTSetPinsDigitalIn(IOPORT_D, BIT_13); #define CONFIG (CN_ON | CN_IDLE_CON) #define INTERRUPT (CHANGE_INT_ON | CHANGE_INT_PRI_2) mCNOpen(CONFIG, CN19_ENABLE, CN19_PULLUP_ENABLE); temp = mPORTDRead(); /* PORT D and E for motors */ //motor 1 mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTD on startup. mPORTDSetPinsDigitalOut(BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTD output. //motor 2 mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Turn on PORTC on startup. mPORTCSetPinsDigitalOut(BIT_1 | BIT_2 | BIT_3 | BIT_4); // Make PORTC output. //motor 3 and 4 mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7); // Turn on PORTE on startup. mPORTESetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2 | BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7); // Make PORTE output. // UART2 to connect to the PC. // This initialization assumes 36MHz Fpb clock. If it changes, // you will have to modify baud rate initializer. UARTConfigure(UART2, UART_ENABLE_PINS_TX_RX_ONLY); UARTSetFifoMode(UART2, UART_INTERRUPT_ON_TX_NOT_FULL | UART_INTERRUPT_ON_RX_NOT_EMPTY); UARTSetLineControl(UART2, UART_DATA_SIZE_8_BITS | UART_PARITY_NONE | UART_STOP_BITS_1); UARTSetDataRate(UART2, GetPeripheralClock(), BAUD); UARTEnable(UART2, UART_ENABLE_FLAGS(UART_PERIPHERAL | UART_RX | UART_TX)); // Configure UART2 RX Interrupt INTEnable(INT_SOURCE_UART_RX(UART2), INT_ENABLED); INTSetVectorPriority(INT_VECTOR_UART(UART2), INT_PRIORITY_LEVEL_2); INTSetVectorSubPriority(INT_VECTOR_UART(UART2), INT_SUB_PRIORITY_LEVEL_0); /* PORTD for LEDs - DEBUGGING */ mPORTDClearBits(BIT_0 | BIT_1 | BIT_2); mPORTDSetPinsDigitalOut(BIT_0 | BIT_1 | BIT_2); // Congifure Change/Notice Interrupt Flag ConfigIntCN(INTERRUPT); // configure for multi-vectored mode INTConfigureSystem(INT_SYSTEM_CONFIG_MULT_VECTOR); // enable interrupts INTEnableInterrupts(); counterDistanceMeasure=600; //measure ULTRASONICS distance each 60 ms while (1) { /***************** Robot MAIN state machine *****************/ unsigned char ret = 0; switch (Robo_State) { case 0: MotorsON = 0; Robo_State = 0; InvInitialOrientation(RESET); TestDog(RESET); GoToRoom4short(RESET); BackToStart(RESET); InitialOrientation(RESET); GoToCenter(RESET); GoToRoom4long(RESET); break; case 1: ret = InvInitialOrientation(GO); if (ret == 1) { Robo_State = 2; } break; case 2: ret = TestDog(GO); if (ret == 1) { Robo_State = 3; //DOG not found } else if (ret == 2) { Robo_State = 4; //DOG found } break; case 3: ret = GoToRoom4short(GO); if (ret == 1) { Robo_State = 0; } break; case 4: ret = BackToStart(GO); if (ret == 1) { Robo_State = 5; } break; case 5: ret = GoToCenter(GO); if (ret == 1) { Robo_State = 6; } break; case 6: ret = GoToRoom4long(GO); if (ret == 1) { Robo_State = 0; } break; } if (frontDistance < 30 || backDistance < 30 || leftDistance < 30 || rightDistance < 30) mPORTDSetBits(BIT_0); else mPORTDClearBits(BIT_0); /***************************************************************/ /***************** Motors State Machine ************************/ if (MotorsON) { /**************************** MOTOR MAP M1 O-------------O M2 ON EVEN MOTORS, STEPS MUST BE INVERTED | /\ | i.e. FORWARD IS BACKWARD | / \ | | || | | || | M3 O-------------O M4 *****************************/ if (M1_counter == 0) { switch (M1_state) { case 0: // set 0011 step (0x3 , 1); if (M1forward) M1_state = 1; else M1_state = 3; break; case 1: // set 1001 step (0x9 , 1); if (M1forward) M1_state = 2; else M1_state = 0; break; case 2: // set 1100 step (0xC , 1); if (M1forward) M1_state = 3; else M1_state = 1; break; case 3: // set 0110 default: step (0x6 , 1); if (M1forward) M1_state = 0; else M1_state = 2; break; } M1_counter = M1_stepPeriod; step_counter[0]--; if (directionNow == countingDirection) step_counter[1]--; } if (M2_counter == 0) { switch (M2_state) { case 0: // set 0011 step (0x3 , 2); if (M2forward) M2_state = 1; else M2_state = 3; break; case 1: // set 0110 step (0x6 , 2); if (M2forward) M2_state = 2; else M2_state = 0; break; case 2: // set 1100 step (0xC , 2); if (M2forward) M2_state = 3; else M2_state = 1; break; case 3: // set 1001 default: step (0x9 , 2); if (M2forward) M2_state = 0; else M2_state = 2; break; } M2_counter = M2_stepPeriod; } if (M3_counter == 0) { switch (M3_state) { case 0: // set 0011 step (0x3 , 3); if (M3forward) M3_state = 1; else M3_state = 3; break; case 1: // set 1001 step (0x9 , 3); if (M3forward) M3_state = 2; else M3_state = 0; break; case 2: // set 1100 step (0xC , 3); if (M3forward) M3_state = 3; else M3_state = 1; break; case 3: // set 0110 default: step (0x6 , 3); if (M3forward) M3_state = 0; else M3_state = 2; break; } M3_counter = M3_stepPeriod; } if (M4_counter == 0) { switch (M4_state) { case 0: // set 0011 step (0x3 , 4); if (M4forward) M4_state = 1; else M4_state = 3; break; case 1: // set 0110 step (0x6 , 4); if (M4forward) M4_state = 2; else M4_state = 0; break; case 2: // set 1100 step (0xC , 4); if (M4forward) M4_state = 3; else M4_state = 1; break; case 3: // set 1001 default: step (0x9 , 4); if (M4forward) M4_state = 0; else M4_state = 2; break; } M4_counter = M4_stepPeriod; } } else { //motors off mPORTDSetBits(BIT_4 | BIT_5 | BIT_6 | BIT_7); mPORTCSetBits(BIT_1 | BIT_2 | BIT_3 | BIT_4); mPORTESetBits(BIT_0 | BIT_1 | BIT_2 | BIT_3 | BIT_4 | BIT_5 | BIT_6 | BIT_7); } /************************************************************/ /******* TEST CODE, toggles the servos (from 90 deg. to -90 deg.) every 1 s. ********/ /* if (auxcounter == 0) { servo1_angle = 0; if (servo2_angle == 90) servo2_angle = -90; else servo2_angle = 90; auxcounter = 20000; // toggle angle every 2 s. } */ servo1_angle = 0; servo2_angle = -90; /* if (frontDistance > 13 && frontDistance < 17) { servo2_angle = 90; } else servo2_angle = -90; */ /*******************************************************************/ /****************** SERVO CONTROL ******************/ /* Changing the global servoX_angle at any point in the code will move the servo to the desired angle. */ servo1_counter = (servo1_angle + 90)*(18)/180 + 6; // between 600 and 2400 us if (servo1_period == 0) { mPORTASetBits(BIT_2); servo1_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */ } servo2_counter = (servo2_angle + 90)*(18)/180 + 6; // between 600 and 2400 us if (servo2_period == 0) { mPORTASetBits(BIT_3); servo2_period = SERVOMAXPERIOD; /* 200 * 100us = 20000us period */ } /*****************************************************/ } /* end of while(1) */ return 0; }
int main(void) { BOARD_Init(); SYSTEMConfig(SYS_FREQ, SYS_CFG_WAIT_STATES | SYS_CFG_PCACHE); SERIAL_Init(); SPI_Init(SPI_SS1_CLKRATE, 0, 0); TIMERS_Init(); printf("program begins...\n"); uint16_t i, j; /* timer test: sucess SET_OUTPUT_PIN(D, 10); TIMERS_Init(); while (1) { WRITE_HIGH(D, 10); InitTimer(1, 100); while (IsTimerActive(1)); WRITE_LOW(D, 10); InitTimer(1, 100); while (IsTimerActive(1)) ; } //*/ /* SPI read test 1: need to get 0x70 to ensure SPI reading is good printf("IMU: SPI read test 1\n"); IMU_Init(); uint8_t id; while (1) { id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I ); printf( "id should be 0x70: 0x%x\n", id ); InitTimer(1, 1000); while (IsTimerActive(1)) ; } //*/ /* SPI read test 2: need to get 0x70 to ensure SPI reading is good printf("IMU: SPI read test 2\n"); IMU_Init(); uint8_t id; while (1) { MPU6500_readBytes(SPI_SS2_ID, MPU6500_RA_WHO_AM_I, 1, &id); printf( "id should be 0x70: 0x%x\n", id ); InitTimer(1, 1000); while (IsTimerActive(1)) ; } //*/ /* inv_mpu test: porting invense IMU library printf("inv_mpu test: read test\n"); uint8_t buf[3] = {0}; uint8_t flag; while (1) { flag = mpu_read_reg(MPU6500_RA_WHO_AM_I, buf); printf("flag: %d\n", flag); printf( "id should be 0x70: 0x%x\n", buf[0]); InitTimer(2, 1000); while (IsTimerActive(2)) { //printf("timer is still active\n"); } } //*/ /* porting inv_mpu test: init and read gyro printf("inv_mpu test: read test\n"); uint16_t buf[3] = {0}; uint8_t flag = mpu_init(0); mpu_reset_fifo(); mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL); mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL); printf("flag: %d\n", flag); uint16_t gyro[3], accel[3]; // flag = mpu_run_6500_self_test(gyro, accel, 0); printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]); printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]); while (1) { flag = mpu_get_gyro_reg(buf, 0); printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, buf[0], buf[1], buf[2]); mpu_get_accel_reg(buf, 0); printf(", accel: % 5d, % 5d, % 5d\n", buf[0], buf[1], buf[2]); InitTimer(2, 1000); while (IsTimerActive(2)); } //*/ /* test: reading multiple times from the same register: the internal pointer +1 when it read once int vals; uint16_t gyro[3], accel[3]; vals = mympu_open(200); printf("MPU Init: %d\n", vals); struct s_mympu mympu; uint8_t flag; while(1) { mpu_get_accel_reg(accel, 0); printf("all accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]); uint8_t tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_H); accel[0] = tmp << 8; tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_XOUT_L); accel[0] |= tmp; tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_H); accel[1] = tmp << 8; tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_YOUT_L); accel[1] |= tmp; tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_H); accel[2] = tmp << 8; tmp = MPU6500_readOneByte(MPU6500_RA_ACCEL_ZOUT_L); accel[2] |= tmp; printf("byte accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]); InitTimer(2, 100); while (IsTimerActive(2)); } //*/ /* test: reading multiple times from the same register: the internal pointer +1 when it read once int vals; uint8_t buf[20] = {0}; uint16_t gyro[3] = {0}, accel[3] = {0}; vals = mympu_open(200); printf("MPU Init: %d\n", vals); //IMU_Init(); uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I ); printf( "id should be 0x70: 0x%x\n", id); struct s_mympu mympu = {0}; int flag; //MPU6500_writeOneByte( MPU6500_RA_USER_CTRL, 0x40 | 0x10 ); while(1) { flag = mympu_update(&mympu); //printf("yaw=[%d, %d], pitch=[%d,%d], roll=[%d,%d]\n", mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]); //printf("flag=%d, yaw=% 11f, pitch=% 11f, roll=% 11f\n", flag, mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]); printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n", mympu.ypr[0], mympu.gyro[0], mympu.ypr[1], mympu.gyro[1], mympu.ypr[2], mympu.gyro[2]); //flag = dmp_read_fifo(gyro, accel, buf, NULL,&vals,&vals); //buf[0] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTH); //buf[1] = MPU6500_readOneByte(MPU6500_RA_FIFO_COUNTL); //uint16_t count = (buf[0] << 8) | buf[1]; //printf("fifo count: %d\n", count); //flag = mpu_read_fifo(gyro, accel, &vals, &vals, &vals); //flag = mpu_read_fifo_stream(10, buf, &vals); //read_fifo(buf, gyro, accel); //printf("flag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]); // printf(", accel: % 5d, % 5d, % 5", accel[0], accel[1], accel[2]); //printf(", quat: %d, %d, %d, %d\n", buf[0], buf[1], buf[2], buf[3]); InitTimer(2, 50); while (IsTimerActive(2)); } //*/ /* test: calibration by adding offset int vals = mympu_open(200); printf("MPU Init: %d\n", vals); uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I ); printf( "id should be 0x70: 0x%x\n", id); struct s_mympu mympu = {0}; int flag; printf("waiting for initial drift\n"); InitTimer(2, 20000); // wait for 20s to pass initial drift while (IsTimerActive(2)); printf("get average offset\n"); float offsetX = 0, offsetY = 0, offsetZ = 0; float offsetPrev[3] = {0}; float alpha = 0; uint8_t k; for (k=1; k <= 200; k++) { flag = mympu_update(&mympu); //printf("yaw=% 3.3f, pitch=% 3.3f, roll=% 3.3f\n", //mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]); alpha = ((float) k - 1.) / k; offsetX = alpha*offsetPrev[0] + (1. - alpha) * mympu.ypr[0]; offsetY = alpha*offsetPrev[1] + (1. - alpha) * mympu.ypr[1]; offsetZ = alpha*offsetPrev[2] + (1. - alpha) * mympu.ypr[2]; offsetPrev[0] = offsetX; offsetPrev[1] = offsetY; offsetPrev[2] = offsetZ; InitTimer(2, 50); while (IsTimerActive(2)); } printf("average: x offset= %3.3f, y offset= %3.3f, z offset= %3.3f\n", offsetX, offsetY, offsetZ); //offsetX = abs(offsetX); offsetY = abs(offsetY); //offsetZ = abs(offsetZ); while(1) { flag = mympu_update(&mympu); //printf("yaw=% 3.0f, pitch=% 3.0f, roll=% 3.0f\n", //printf("%4.1f, %4.1f, %4.1f\n", mympu.ypr[0] - offsetX, mympu.ypr[1] - offsetY, mympu.ypr[2] - offsetZ); printf("yaw=[%.3f, %.3f], pitch=[%.3f,%.3f], roll=[%.3f,%.3f]\n", mympu.ypr[0] - offsetX, mympu.gyro[0], mympu.ypr[1] - offsetY, mympu.gyro[1], mympu.ypr[2] - offsetZ, mympu.gyro[2]); InitTimer(2, 50); while (IsTimerActive(2)); } //*/ //* test: two SPI devices, OLED and IMU int vals = mympu_open(10); OLED_Init(); OpenTimer2(T2_ON | T2_SOURCE_INT | T2_PS_1_1, 0x2710); ConfigIntTimer2(T2_INT_ON | T2_INT_PRIOR_2); printf("MPU Init: %d\n", vals); uint8_t id = MPU6500_readOneByte( MPU6500_RA_WHO_AM_I ); printf( "id should be 0x70: 0x%x\n", id); UG_SetForecolor(0x0F); UG_SetBackcolor(0x00); UG_FontSelect(FONT_6X8); char str[50] = ""; while(1) { //mympu_update(&mympu); sprintf(str, "%.3f, %.3f, %.3f\n", mympu.ypr[0], mympu.ypr[1], mympu.ypr[2]); printf("%s\n",str); flag_writingScreen = 1; UG_PutString(0, 0, str); flag_writingScreen = 0; InitTimer(1, 200); while (IsTimerActive(1)); } //*/ /* flag = mpu_run_6500_self_test(gyro, accel, 0); printf("\nflag=%d, gyro: % 5d, % 5d, % 5d", flag, gyro[0], gyro[1], gyro[2]); printf(", accel: % 5d, % 5d, % 5d\n", accel[0], accel[1], accel[2]); MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_H, (accel[0] & 0x7F80) >> 8); MPU6500_writeOneByte(MPU6500_RA_XA_OFFS_L_TC, accel[0] & 0x7F); MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_H, (accel[1] & 0x7F80) >> 8); MPU6500_writeOneByte(MPU6500_RA_YA_OFFS_L_TC, accel[1] & 0x7F); MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_H, (accel[2] & 0x7F80) >> 8); MPU6500_writeOneByte(MPU6500_RA_ZA_OFFS_L_TC, accel[2] & 0x7F); MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRH, gyro[0] >> 8); MPU6500_writeOneByte(MPU6500_RA_XG_OFFS_USRL, gyro[0] & 0xFF); MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRH, gyro[1] >> 8); MPU6500_writeOneByte(MPU6500_RA_YG_OFFS_USRL, gyro[1] & 0xFF); MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRH, gyro[2] >> 8); MPU6500_writeOneByte(MPU6500_RA_ZG_OFFS_USRL, gyro[2] & 0xFF); //*/ printf("program ended...\n"); return 0; }