示例#1
0
文件: main.c 项目: JRMN7/Freescale
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();
	}
}
示例#2
0
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;
		}
	}
}
示例#3
0
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);	
		
	}	
}
示例#4
0
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);	
		
	}
}
示例#5
0
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 */	
}
示例#6
0
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);

    }
}
示例#7
0
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);		
	}	
}
示例#8
0
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();

        }       
}