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;
}
