Exemplo n.º 1
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);

    }
}
Exemplo n.º 2
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);	
		
	}	
}
Exemplo n.º 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 (;;) 
	{

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