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