Beispiel #1
0
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();
	}
}
Beispiel #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;
		}
	}
}
Beispiel #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);	
		
	}	
}
Beispiel #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);	
		
	}
}
Beispiel #5
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);

    }
}
Beispiel #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 (;;) 
	{

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