File content as of revision 4:61edb416e860:

#include "mbed.h"
#include "TFC.h"
#include <cmath>

 
//This macro is to maintain compatibility with Codewarrior version of the sample.   This version uses the MBED libraries for serial port access
Serial PC(USBTX,USBRX);


 
 //This ticker code is used to maintain compability with the Codewarrior version of the sample.   This code uses an MBED Ticker for background timing.
 
#define NUM_TFC_TICKERS 4

Ticker TFC_TickerObj;
 
volatile uint32_t TFC_Ticker[NUM_TFC_TICKERS];
 
void TFC_TickerUpdate()
{
    int i;
 
    for(i=0; i<NUM_TFC_TICKERS; i++)
     {
        if(TFC_Ticker[i]<0xFFFFFFFF) 
        {
            TFC_Ticker[i]++;
        }
    }
}
 
//finds light intensity values of lines.
 
int FindBlack();

// finds and sets by reference the values of the left and right bounds
void Bounds(int &, int &, int);

// finds distance between bounds 
int distance(int &, int &, int);

// returns interger for turn direction 0 for left 1 for right

int turnDir( int &, int &, int, int);

//void turnLeft(int &, int &, int, int);  needs to be finished

 
int main()
{
    uint32_t i,t,time = 0;
    
    int a = 200, b = 200, d = 200;
    
    int data[128];
    
    int black = 0;
  
    PC.baud(9600);
    TFC_TickerObj.attach_us(&TFC_TickerUpdate,2000);
   
    TFC_Init();
    
    bool didInit  = false  ; // flag for track calibration 
    
    for(;;)
    {      
        //TFC_Task must be called in your main loop.  This keeps certain processing happy (I.E. Serial port queue check)
         //   TFC_Task();

            //This Demo program will look at the middle 2 switch to select one of 4 demo modes.
            //Let's look at the middle 2 switches
            switch((TFC_GetDIP_Switch()>>1)&0x03)
            {
            default:
            case 0 ://initilazation case
                if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
                {
                    TFC_Ticker[0] = 0;
                    TFC_LineScanImageReady=0;
                    if(TFC_PUSH_BUTTON_0_PRESSED&& didInit == false)
                    {
                        black = FindBlack();
                        d=distance(a,b,black);
                        
                        PC.printf("%d \n" , a  );
                        PC.printf("%d \n", b );
                        PC.printf("%d \n", black);
                        PC.printf("%d \n" , d );                    
                        
                        didInit = true; // sets initialzation to true
                    }
                }
                TFC_SetServo(0,0.0);
                
                // this is test code to have the car print if a turn case happened.
                
                if(didInit) // check to see if initalized 
                {
                    if(d < distance(a,b,black))
                        {
                            int whichTurn = 2; // impossible for function to return
                            
                            whichTurn = turnDir(a,b,black,d); // sets which turn to 0 for left and 1 for right
                            
                            PC.printf("%d \n", whichTurn);
                        }
                }        
                break;                   
                    
            case 1: // moves servo and prints pot value.
                
                
                if(TFC_Ticker[0]>=20)
                {
                    TFC_Ticker[0] = 0; //reset the Ticker
                    //Every 20 mSeconds, update the Servos
                    TFC_SetServo(0,TFC_ReadPot(0));
                    TFC_SetServo(1,TFC_ReadPot(1));
                    
                  
                }
                
                 if(TFC_PUSH_BUTTON_1_PRESSED)
                   { 
                   
                    PC.printf("%f", TFC_ReadPot(0));
                    }
                
                
                //Let's put a pattern on the LEDs
                if(TFC_Ticker[1] >= 125)
                {
                    TFC_Ticker[1] = 0;
                    t++;
                    if(t>4)
                    {
                        t=0;
                    }           
                    TFC_SetBatteryLED_Level(t);
                }
                
              //  TFC_SetMotorPWM(0,0); //Make sure motors are off
              //  TFC_HBRIDGE_DISABLE;
            

                break;
                
            case 2 :
              
         
                //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
                //Labview Application
                
                
                if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
                    {
                     TFC_Ticker[0] = 0;
                     TFC_LineScanImageReady=0;
                    // PC.printf("\r\n");
                    // PC.printf("L:");
                     
                        if(t==0)
                            t=4;
                        else
                            t--;
                        
                         TFC_SetBatteryLED_Level(t);
                        
                         // camera 1
                         
                         if(TFC_PUSH_BUTTON_0_PRESSED)
                    {          
                  
                         
                         black = FindBlack();
                         
                         PC.printf("%i", black);
                
                
               
                break;
            
            case 3 :
            
         
                //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
                //Labview Application
                
                
                if(TFC_Ticker[0]>50 && TFC_LineScanImageReady>0)
                    {
                     TFC_Ticker[0] = 0;
                     TFC_LineScanImageReady=0;
                    // PC.printf("\r\n");
                    // PC.printf("L:");
                     
                        if(t==0)
                            t=4;
                        else
                            t--;
                        
                         TFC_SetBatteryLED_Level(t);
                        
                         // camera 1
                         
                         if(TFC_PUSH_BUTTON_0_PRESSED)
                    {          
                               //Demo Mode 3 will be in Freescale Garage Mode.  It will beam data from the Camera to the 
                //Labview Application

                        
                         // camera 1
                         for(i=0;i<128;i++)
                         {
                               if(TFC_LineScanImage0[i]<=black)
                               {
                                   // zero is black
                                   
                                   data[i]=0;
                                }
                                else
                                {
                                    // one is white
                                    data[i]=1;
                                }
                                PC.printf("%d", data[i]);
                         }
                         PC.printf("\r");
                        
                        // camera 2
                      /*   for(i=0;i<128;i++)
                         {
                                 if(i==127)
                                     PC.printf("%X\r\n",TFC_LineScanImage1[i]);
                                 else
                                     PC.printf("%X,",TFC_LineScanImage1[i]);
                           
                         } */
                         
                      }   
                    }
                 
                break;
            }
    }
    
 
}
}
}

// finds and sets by reference the values of the left and right bounds

void bounds(int &a,int &b,int black)
{
 
     for(int i=0; i<128; i++)
     {
         if(i==0)
         {
             a = i;
         }
     
        else if(TFC_LineScanImage0[i] <= black)
         {
            //if there are to black values next to eachother it sets a to newer inside value.
            if ( a == 0 && i!=0)
                a = i;
                
            else if( (i-a) == 1 )
            {
                a = i;   
            }    
            
            // if there is a space between black values the next black value is the right bound.
            else
            {
              // sets the right inside bound and ends the loop.
              
              b = i;
              break;   
            }
         }
       } 
}         
 
    
  

int FindBlack()
{
    int low1=2000,low2=2000;
    for(int i=2;i<64;i++)//first half of line scan
    {
        int ave=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
        if(i==2)
        {
            low1=ave;//first loop sets lowest average
        }
        if(low1>ave)
        {
            low1=ave;
        }
    }
    for(int i=66;i<128;i++)//second half of line scan
    {
        int ave2=(TFC_LineScanImage0[i-2]+TFC_LineScanImage0[i-1]+TFC_LineScanImage0[i])/3;
        if(i==66)
        {
            low2=ave2;
        }
        if(low2>ave2)
        {
            low2=ave2;
        }
        if(low1==low2)
        {
            return low1; //confirms bothe lines and breaks loop returns black
        }   
    }
    if(low1>low2)
    {
        return low1;
    }
    else
    {
        return low2;
    }
}
int distance(int &a, int &b, int black)
{
    bounds(a,b,black);
    return (b-a);
}

int turnDir( int &a, int &b, int black, int d)
    {
        int l = 200, r = 200;
        
        bounds(a,b,black); // resets bounds for start of program
        
        while(d != distance(l,r,black)) // keeps checking rate of change while approaching turn
            {
                if( abs(a-l) > abs(b-r) ) // if the change in the left direction is greater than the right
                    {
                        return 0; // this returns left
                    }
                
                if( abs(a-l) < abs(b-r)) // if the change in the right direction is greater than the left
                    {
                        return 1; // this returns right
                    }
                    
             }
     } 
     
/*void turnLeft(int &a, int &b, black, d)
    {
        // waits for the amount of time it takes to reach the turn
        wait(.146);
        
         
              //   turns the wheels to the angle they need to be 
              //   while the distance between the lines
              //   indicate the car is in a turn
                
        while(d > distance(a, b, black) )
            {
                setservo( turn value);
            }
            
              // returns the wheels back to straight
            
         setservo(0,0.0);
     }           
                                      
  */