/*~~~~~~~ Main Code ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
int main(void)
{
initModesAndClock();
/* Disable Watchdog */
disableWatchdog();
/*Initialize LEDs on TRK-MPC560xB board */
vfnGPIO_LED_Init();
/*Initialize PUSHs on TRK-MPC560xB board */
vfnGPIO_PUSH_Init();
/*Initialize Interrupts */
INTC_InitINTCInterrupts();
/*Initialize Exception Handlers */
EXCEP_InitExceptionHandlers();
PIT_device_init();
PIT_channel_configure(PIT_CHANNEL_0 , dummy_500us);
PIT_channel_start(PIT_CHANNEL_0);
/* Enable External Interrupts*/
enableIrq();
/* Infinite loop */
dummy_endless_loop();
for (;;)
{
BackgroundSubsystemTasks();
}
}
/**
DONT_TRANSLATE
*/
BOOT_CODE void
initTimer(void)
{
int timeout;
disableWatchdog();
enableTimers();
timer->cfg = TIOCP_CFG_SOFTRESET;
for (timeout = 10000; (timer->cfg & TIOCP_CFG_SOFTRESET) && timeout > 0; timeout--)
;
if (!timeout) {
printf("init timer failed\n");
return;
}
maskInterrupt(/*disable*/ true, DMTIMER0_IRQ);
/* Set the reload value */
timer->tldr = 0xFFFFFFFFUL - TIMER_INTERVAL_TICKS;
/* Enables interrupt on overflow */
timer->tier = TIER_OVERFLOWENABLE;
/* Clear the read register */
timer->tcrr = 0xFFFFFFFFUL - TIMER_INTERVAL_TICKS;
/* Set autoreload and start the timer */
timer->tclr = TCLR_AUTORELOAD | TCLR_STARTTIMER;
}
int main(void) {
volatile int i = 0;
volatile uint32_t resultat = 0;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[16].R = 0x0100; /* Potentiomètre en Input"
/* Loop forever */
for (;;) {
reglerPotentio();
// boutonLed();
// SERVO();
}
}
void main(void) {
uint8_t ddramAddr = LCD_START_OF_FIRST_LINE;
int buttonCount = 0, charCount = 0;
const uint8_t *dr_seuss = (const uint8_t*)DR_SEUSS;
disableWatchdog();
configClocks();
turningOnLCD();
while (1) {
// Check if button pressed
if (P2IN && buttonCount < 100) {
printString((const uint8_t*)"Hello, World!", 13, ddramAddr);
}
else if (!P2IN && buttonCount < 100){
++buttonCount;
printString((const uint8_t*)"Goodbye!", 8, ddramAddr);
}
else {
printString(dr_seuss + charCount++, 32, LCD_START_OF_FIRST_LINE);
}
// Update address
ddramAddr = ddramAddr == LCD_END_OF_FIRST_LINE ?
LCD_START_OF_SECOND_LINE : ddramAddr + 0x01;
// Wrap address back around
if (ddramAddr > LCD_END_OF_SECOND_LINE)
ddramAddr = LCD_START_OF_FIRST_LINE;
// Stop the text from scrolling too fast
__delay_cycles(50000);
}
}
//-------------------------------------------------------------------
cSystem::cSystem( unsigned char disableInterrupts )
{
disableWatchdog();
if( disableInterrupts )
{
disableInterrupt();
}
}
/*~~~~~~~ Main Code ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
int main(void)
{
initModesAndClock();
/* Disable Watchdog */
disableWatchdog();
/*Initialize LEDs on TRK-MPC560xB board */
MemAllocInit(&MemAllocConfig);
vfnGPIO_LED_Init();
/*Initialize Interrupts */
INTC_InitINTCInterrupts();
/*Initialize Exception Handlers */
EXCEP_InitExceptionHandlers();
// PIT_device_init();
// PIT_channel_configure(PIT_CHANNEL_0 , Test);
// PIT_channel_start(PIT_CHANNEL_0);
/* Enable External Interrupts*/
enableIrq();
/* Infinite loop */
SchM_Init(&SchedulerConfig);
SchM_Start();
for (;;)
{
BackgroundSubsystemTasks();
}
}
/*~~~~~~~ Main Code ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~*/
int main(void)
{
initModesAndClock();
/* Disable Watchdog */
disableWatchdog();
MemAllocInit(&MemAllocConfig);
Init_LIN_SLV1();
/*Initialize Interrupts */
INTC_InitINTCInterrupts();
/*Initialize Exception Handlers */
EXCEP_InitExceptionHandlers();
/* Enable External Interrupts*/
enableIrq();
/* Infinite loop */
/*init del scheduler*/
SchM_Init(&SchConfig);
SchM_Start();
for (;;)
{
BackgroundSubsystemTasks();
}
}
void main(void) {
volatile uint32_t IdleCtr = 0;
initModesAndClks(); /* Initialize mode entries */
initPeriClkGen(); /* Initialize peripheral clock generation for LINFlex */
disableWatchdog(); /* Disable watchdog */
INTC_InitINTCInterrupts();
EXCEP_InitExceptionHandlers();
INTC.MCR.R=0;
INTC_InstallINTCInterruptHandler(&Intc_LINFLEX_Rx, 79, 10);
INTC_InstallINTCInterruptHandler(&Intc_LINFLEX_Tx, 80, 10);
INTC_InstallINTCInterruptHandler(&Intc_LINFLEX_Err, 81, 10);
INTC.CPR.R = 0;
initLINFlex_0(9600); /* Initialize FLEXCAN 0 as master */
NormalModeLINFLEX_2();
//transmitLINframe(); /* Transmit one frame from master */
while (1) {IdleCtr++;} /* Idle loop: increment counter */
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
//init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 0. DEBUGGING CODE
//option = ReadData();
//printlistall();
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_car();
// 3. Calculate Differential
for (i=0;i<127;i++) // one less than 128
{
diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx
}
// 4. Find maximum and minimum indices
max_val = diff_result[START]; // can be improved to centre + max_delta
min_val = diff_result[START];
max_I = START;
min_I = START;
for (i=START+1;i<END;i++) //0 skipped
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
// 6. MAIN STATE MACHINE ALGO
width = max_I - min_I;
if (width> LB_WIDTH && width < UB_WIDTH)
{
// Everything is assumed normal.
// IF there is a spike (very less probability)
// it should be allowed to process, it can't misguide the car.
// Find centre
if (min_I < L_BOUND)
{
SERVO (SERVO_CENTRE - SERVO_LIMIT );
continue;
}
if (max_I > R_BOUND)
{
SERVO (SERVO_CENTRE + SERVO_LIMIT );
continue;
}
centre = (max_I + min_I) /2;
}
else
{
continue;
}
if ((centre - prev_centre) > FILTER)
{
prev_centre = centre;
centre = centre * 0.20;
}
else
{
prev_centre = centre;
}
// 8. Calculate error
error = centre - 64 ;
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t option;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// set switchs as inputs. ..
SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/
SIU.PCR[65].R = 0x0100; /* Program the drive enable pin of S2 (PE1) as input*/
SIU.PCR[68].R = 0x0200; /* Program the drive enable pin of LED1 (PE4) as output*/
SIU.PCR[69].R = 0x0200; /* Program the drive enable pin of LED2 (PE5) as output*/
SIU.PCR[70].R = 0x0200; /* Program the drive enable pin of LED3 (PE6) as output*/
SIU.PCR[71].R = 0x0200; /* Program the drive enable pin of LED4 (PE7) as output*/
SIU.PGPDO[2].R |= 0x0f000000; /* Disable LEDs*/
/* Loop forever */
for (;;)
{
if((SIU.PGPDI[2].R & 0x80000000) != 0x80000000)
{
if (correction <= 1300)
{
correction = 1300;
}
else
{
correction -=10;
}
}
else if((SIU.PGPDI[2].R & 0x40000000) != 0x40000000)
{
if (correction >= 1900)
{
correction = 1900;
}
else
{
correction +=10;
}
}
option = ReadData();
if (option == '6')
{
CAMERA_simar();
}
SERVO (correction);
}
}
void initMainHardware(void)
{
disableIrq(); /* Ensure INTC current prority=0 & enable IRQ */
initBTB();
initModesAndClock();
initPeriClkGen() ;
initPads ();
disableWatchdog();
init_LinFLEX_0_UART ();
initCAN_1(); /* Initialize FLEXCAN 0*/
initSBC(); //init SBC for CAN
initADC();
initINTC(); /* Initialize INTC for software vector mode */
initPIT(); /* Initialize PIT1 for 1KHz IRQ, priority 2 */
// initSwIrq4(); /* Initialize software interrupt 4 */
SIU.PSMI[0].R = 1; //can1rxd=43
SIU.PSMI[7].R = 2; //dspi1_sscl=114
SIU.PSMI[8].R = 2; //dspi1_sscl=114
SIU.PSMI[9].R = 3; //dspi1_sscl=114
EMIOS_0.MCR.B.GPRE= 63; /* Divide 64 MHz sysclk by 63+1 = 64 for 1MHz eMIOS clk*/
EMIOS_0.MCR.B.GPREN = 1; /* Enable eMIOS clock */
EMIOS_0.MCR.B.GTBE = 1; /* Enable global time base */
EMIOS_0.MCR.B.FRZ = 1; /* Enable stopping channels when in debug mode */
EMIOS_0.CH[0].CADR.R = 14999; /* Period will be 19999+1 = 20000 clocks (20 msec)*/
EMIOS_0.CH[0].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[0].CCR.B.BSL = 0x3; /* Use internal counter */
EMIOS_0.CH[0].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[0].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[0].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
EMIOS_0.CH[23].CADR.R = 999; /* Period will be 999+1 = 1000 clocks (1 msec)*/
EMIOS_0.CH[23].CCR.B.MODE = 0x50; /* Modulus Counter Buffered (MCB) */
EMIOS_0.CH[23].CCR.B.BSL = 0x3; /* Use internal counter */
EMIOS_0.CH[23].CCR.B.UCPRE=0; /* Set channel prescaler to divide by 1 */
EMIOS_0.CH[23].CCR.B.UCPEN = 1; /* Enable prescaler; uses default divide by 1*/
EMIOS_0.CH[23].CCR.B.FREN = 1; /* Freeze channel counting when in debug mode*/
EMIOS_0.CH[4].CADR.R = 0; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[4].CBDR.R = 1500; /* Trailing edge when channel counter bus=1400 Middle, 1650 Right Max, 1150 Left Max*/
EMIOS_0.CH[4].CCR.B.BSL = 0x01; /* Use counter bus B */
EMIOS_0.CH[4].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output */
EMIOS_0.CH[4].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[28].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
EMIOS_0.CH[6].CADR.R = 000; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[6].CBDR.R = 950; /* Trailing edge when channel counter bus=500*/
EMIOS_0.CH[6].CCR.B.BSL = 0x0; /* Use counter bus A (default) */
EMIOS_0.CH[6].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output */
EMIOS_0.CH[6].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[30].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 6 to pad */
EMIOS_0.CH[7].CADR.R = 0; /* Leading edge when channel counter bus=0*/
EMIOS_0.CH[7].CBDR.R = 950; /* Trailing edge when channel's counter bus=999*/
EMIOS_0.CH[7].CCR.B.BSL = 0x0; /* Use counter bus A (default) */
EMIOS_0.CH[7].CCR.B.EDPOL = 1; /* Polarity-leading edge sets output*/
EMIOS_0.CH[7].CCR.B.MODE = 0x60; /* Mode is OPWM Buffered */
SIU.PCR[31].R = 0x0600; /* MPC56xxS: Assign EMIOS_0 ch 7 to pad */
EMIOS_0.CH[3].CADR.R = 250; /* Ch 3: Match "A" is 250 */
EMIOS_0.CH[3].CBDR.R = 500; /* Ch 3: Match "B" is 500 */
EMIOS_0.CH[3].CCR.R= 0x000000E0; /* Ch 3: Mode is OPWMB, time base = ch 23 */
EMIOS_0.CH[2].CCR.R= 0x01020082; /* Ch 2: Mode is SAIC, time base = ch 23 */
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */
asm(" wrteei 1"); /* Enable external interrupts */
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 0. DEBUGGING CODE
option = ReadData();
printlistall();
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_simar();
// 3. Calculate Differential
for (i=0; i<127; i++) // one less that 128
{
diff_result[i] = (short int) (Result[i+1] - Result [i]); // dy.dx
}
// 4. Find maximum and minimum indices
max_val = diff_result[centre]; // can be improved to centre + max_delta
min_val = diff_result[centre];
max_I = centre;
min_I = centre;
for (i=centre-1; i>2; i--) //0 skipped
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
for (i = centre+1; i<126; i++)
{
if (max_val < diff_result[i])
{
max_val = diff_result[i];
max_I = i;
}
if (min_val > diff_result[i])
{
min_val = diff_result[i];
min_I = i;
}
}
// 6. MAIN STATE MACHINE ALGO
width = max_I - min_I;
if (width> LB_WIDTH && width < UB_WIDTH)
{
// Everything is assumed normal.
// IF there is a spike (very less probability)
// it should be allowed to process, it can't misguide the car.
}
else
{
// Width is not right. Check for any psuedo max/ mins. We are assuming that
// there must be atleast one pseudo in this case. As width is not right.
diff = max_val;
if( diff > MIN_FINGER) // M is not pseudo
{
// sure (according to assumption above) m is pseudo.
// Two possibilities when one is pseudo
if (max_I < LEFT_GUARD)
{
// Guard detected
min_I = 0;
}
else
{
// 5.5 DYNAMIC INTEGRATION TIME
/* if ((max_val+ (-min_val))/2 <50 )
{
int_time = int_time*1.5;
}
else if ((max_val+ (-min_val))/2 >90 )
{
int_time = int_time*0.85;
}
*/
// Means that there had been a spike
// skip the case
// Become cautious and increase integration time.
//continue;
}
}
else
{
// M is pseudo and m can also be pseudo
// To test m is pseudo or not.
diff = min_val;
if ( diff > MIN_FINGER)
{
// m is not pseudo
// only M is pseudo
if (min_I > RIGHT_GUARD)
{
// Guard detected
max_I = 128;
}
else
{
// Means that there had been a spike
// skip the case
// Become cautious and increase integration time.
//continue;
}
}
else
{
// 5.5 DYNAMIC INTEGRATION TIME
if ((max_val+ (-min_val))/2 <50 )
{
int_time = int_time*1.5;
}
else if ((max_val+ (-min_val))/2 >90 )
{
int_time = int_time*0.85;
}
// both are pseudo
// So either a ALL_BALCK (CROSS) or ALL_WHITE
// Skip the case
// Become cautious and increase integration time.
continue;
}
}
}
// 7. Find centre
centre = (max_I + min_I) /2;
// 7.5 Filter
if (centre-prev_centre >15 || centre-prev_centre <-15)
{
prev_centre = centre;
centre = centre*0.30;
}
else
{
prev_centre = centre;
}
// 8. Calculate error
error = centre - 64 ;
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
}
}
void main (void) {
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
//init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
// Routines
DC_Motors_on();
//******************************** INFINITE LOOP ***********************************
for (;;)
{
// 1. Clean all variables
//sensor_value_left =0;
//sensor_value_right =0 ;
// 2. Sense the line
//option = ReadData();
//CAMERA();
CAMERA_simar();
// 3. Calculate SUMs
sum_left =0;
sum_right=0;
for (i=1; i<64 ; i++)
{
sum_left += Result[i];
sum_right += Result[i+64];
}
// 4. Find Difference
error = sum_left - sum_right;
// 8. Calculate pid
pid_term = (int) ( kp * error );
// 9. Calculate PID term
if (pid_term > SERVO_LIMIT)
{
pid_term = SERVO_LIMIT;
}
else if (pid_term <-SERVO_LIMIT)
{
pid_term = -SERVO_LIMIT;
}
// 10. Feed the new value to servo motor
correction = SERVO_CENTRE + pid_term;
SERVO (correction);
// 11. Debugging code
//option = ReadData();
//printserialsingned (error);
}
}
static void hardwareInit(void) {
disableWatchdog();
clock_prescale_set(clock_div_1);
}
int main()
{
cli();
disableWatchdog();
slowClock();
initBuzzer();
initSwitch();
initADC();
#ifdef DEBUG
uint16_t v = getVoltage();
uint16_t i;
for (i = 0x8000; i != 0; i >>= 1) {
if (v & i) {
dit();
} else {
dah();
}
_delay_ms(1000);
}
#endif
// Retrieve the current warning timeout from eeprom
uint8_t warn_min = eeprom_read_byte(&cfg_warn_min);
if (switchPressed()) {
number(warn_min);
// pause between increments
_delay_ms(3000);
}
// If switch is depressed (at power up), begin increasing
// warning time, in 5-minute increments, max 30 minutes
// SOS time is 2 x warning time.
//
while (switchPressed()) {
if (switchPressed()) {
// Increment warning time by 5 minutes
warn_min += 5;
// Maximum warning time is 30 minutes
if (warn_min > 30) warn_min = 5;
// Save the new warning time
eeprom_update_byte(&cfg_warn_min, warn_min);
}
number(warn_min);
// pause between increments
_delay_ms(3000);
}
// Compute the number of seconds for warning and SOS
warn_sec = warn_min * 60;
sos_sec = warn_sec * 2;
if (checkVoltage()) {
ok();
} else {
sos();
}
enableWatchdog();
sei();
while (1) {
set_sleep_mode(SLEEP_MODE_PWR_DOWN);
sleep_mode();
}
}
void main (void) {
vuint32_t i = 0;
initModesAndClock(); /* Initialize mode entries and system clock */
disableWatchdog(); /* Disable watchdog */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
initADC();
initLED();
//can stuff
initDSPI_1(); /* Initialize DSPI_1 as Slave SPI and init CTAR0 */
canSetup();
initEnergizeButton();
energizePacket.ID = ENERGIZE_ID;
energizePacket.DATA.W[0] = 0xFFFFFFFF;
energizePacket.DATA.W[1] = 0xFFFFFFFF;
deenergizePacket.ID = DEENERGIZE_ID;
deenergizePacket.DATA.W[0] = 0x00000000;
deenergizePacket.DATA.W[1] = 0x00000000;
torquePacket.ID = TORQUE_ID;
speedPacket.ID = SPEED_ID;
shutdownPacket.ID = SHUTDOWN_ID;
shutdownPacket.DATA.W[0] = 0xDEADBEEF;
shutdownPacket.DATA.W[1] = 0xDEADBEEF;
/* Loop forever */
while (1)
{
prevEnergizeButton = energizeButton;
getEnergizeButton();
switch (currentState) {
case NOT_ENERGIZED:
if (!energizeButton && prevEnergizeButton) {
currentState = ENERGIZED;
canSend(energizePacket);
}
break;
case ENERGIZED:
if (!energizeButton && prevEnergizeButton) {
currentState = NOT_ENERGIZED;
canSend(deenergizePacket);
} else {
getADC();
processAnalog();
if (imp_count < 2 || torque0 < 20) {
convertSpeed();
convertTorque();
if (MODE == 0) {
canSend(speedPacket);
} else {
canSend(torquePacket);
}
} else {
currentState = SHUTDOWN;
canSend(deenergizePacket);
}
}
break;
case SHUTDOWN:
canSend(shutdownPacket);
SHUTDOWN_CIRCUIT = TRUE;
break;
}
toLED(currentState);
//canReceive();
delay();
i++;
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t option;
initModesAndClock(); /* Initialize mode entries and system clock */
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
initPads(); /* Initialize pads used in example */
initADC(); /* Init. ADC for normal conversions but don't start yet*/
initCTU(); /* Configure desired CTU event(s) */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch3(); /* Initialize eMIOS 0 channel 3 as OPWM and channel 2 as SAIC*/
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch4(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 4 as time base */
initEMIOS_0ch6(); /* Initialize eMIOS 0 channel 0 as OPWM, ch 6 as time base */
initEMIOS_0ch7(); /* Initialize eMIOS 0 channel 1 as OPWM, ch 7 as time base */
init_LinFLEX_0_UART();
SIU.PCR[17].R = 0x0200; /* Program the drive enable pin of Right Motor as output*/
SIU.PCR[16].R = 0x0200; /* Program the drive enable pin of Left Motor as output*/
SIU.PGPDO[0].R = 0x00000000; /* Disable the motors */
/* Loop forever */
for (;;)
{
TransmitData("\n\r**The Freescale Cup**");
TransmitData("\n\r*********************");
TransmitData("\n\r1.Led\n\r");
TransmitData("2.Switch\n\r");
TransmitData("3.Servo\n\r");
TransmitData("4.Motor Left\n\r");
TransmitData("5.Motor Right\n\r");
TransmitData("6.Camera\n\r");
TransmitData("9.Camera 2");
TransmitData("7.Left Motor Current\n\r");
TransmitData("8.Right Motor Current");
TransmitData("\n\r**********************");
option = ReadData();
switch(option)
{
case '1':
LED();
break;
case '2':
SWITCH();
break;
case '3':
SERVO();
break;
case '4':
MOTOR_LEFT();
break;
case '5':
MOTOR_RIGHT();
break;
case '6':
CAMERA();
break;
case '7':
LEFT_MOTOR_CURRENT();
break;
case '8':
RIGHT_MOTOR_CURRENT();
break;
case '9':
CAMERA2();
break;
default:
break;
}
}
}
void main (void) {
volatile uint32_t i = 0; /* Dummy idle counter */
uint8_t success=0;
uint8_t byteReceived=0;
uint8_t opcode=0;
uint8_t payload=0;
char msg[32];
uint8_t msgLength=0;
clock = 0;
initModesAndClock(); /* MPC56xxP/B/S: Initialize mode entries, set sysclk = 64 MHz*/
initPeriClkGen(); /* Initialize peripheral clock generation for DSPIs */
disableWatchdog(); /* Disable watchdog */
EXCEP_InitExceptionHandlers(); /* Initialize exceptions: only need to load IVPR */
initADC();
initPIT(); /* Initialize PIT1 for 10Hz IRQ, priority 2 */
initPads(); /* Initialize software interrupt 4 */
initEMIOS_0(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_1(); /* Initialize eMIOS channels as counter, SAIC, OPWM */
initEMIOS_0ch0(); /* Initialize eMIOS 0 channel 0 as modulus counter*/
initEMIOS_0ch23(); /* Initialize eMIOS 0 channel 23 as modulus counter*/
initEMIOS_0ch8(); /* Initialize eMIOS 0 channel 8 as modulus counter*/
//just to make sure the wheels are facing straight
initDrive();
Drive();
SIU.PCR[64].R = 0x0100; /* Program the drive enable pin of S1 (PE0) as input*/
while((SIU.PGPDI[2].R & 0x80000000) == 0x80000000); /*Wait until S1 switch is pressed*/
for(i=0;i<100000;i++);
while((SIU.PGPDI[2].R & 0x80000000) != 0x80000000); /*Wait until S1 switch is released*/
INTC_InitVector();
INTC_InstallINTCInterruptHandler(&SwIrq4ISR,4,3);
INTC_InstallINTCInterruptHandler(&EOC_ISR,62,5);
INTC_InstallINTCInterruptHandler(&Pit1ISR,60,2);
INTC_InstallINTCInterruptHandler(&Pit2ISR,61,4);
INTC_InstallINTCInterruptHandler(&Pit3ISR,127,4);
initSerial();
flag_lineDone = -1;
initCamera();
initSteeringController();
INTC_InitINTCInterrupts();
INTC.CPR.B.PRI = 0; /* Single Core: Lower INTC's current priority */
initDrive();
// setPWMRw(48);
// setPWMLw(48);
setDirection(FORWARD);
in = getInBuffer();
out = getOutBuffer();
MESSAGE("I'm running\n\r");
fifo_write(&out->fifo,msg,msgLength);
while(isCameraReady()!=STATE_READY);
for(;;)
{
if(!(flag_lineDone==1))
{
Drive();
success=0;
success = fifo_read(&in->fifo,&byteReceived,1);
if(success==1)
{
if(opcode==0)
{
opcode = byteReceived;
MESSAGE("Command Received: ");
fifo_write(&out->fifo,msg,msgLength);
}
else
{
payload = byteReceived;
switch(opcode)
{
case DRIVE:
// TransmitData("Drive Command\n\r");
MESSAGE("Drive Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setDirection(payload);
break;
case SET_SPEED:
// TransmitData("Set Speed Command\n\r");
MESSAGE("Set Speed Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setPWMRw(payload);
setPWMLw(payload);
break;
case STEERING:
// TransmitData("Set Steering Command\n\r");
MESSAGE("Set Steering Command\n\r");
fifo_write(&out->fifo,msg,msgLength);
setAngle(payload);
break;
default:
// TransmitData("Bad command\n\r");
MESSAGE("Bad command\n\r");
fifo_write(&out->fifo,msg,msgLength);
break;
}
opcode = 0;
}
}
}
if(TRANSMISSION_DONE()&&(out->fifo.length>0))
Tx();
if(RECEPTION_DONE()&&(in->fifo.length<IN_BUFFER_SIZE))
Rx();
}
}