File content as of revision 4:0c97edd98157:

 /** 
  * File:    frdm_MasterVehicle/main.cpp
  * 
  * Author1:  Carla Amaya
  * Author2:  Gerardo Cordero
  * Author3:  José Pesado
  * Author3:  Jalil Chávez
  *
  * Date:     October 2014  
  * Course:   Instrumentation
  * 
  * Summary of File: 
  * 
  *   This file contains code which controlls a line follower.
  *   This device takes as input camera readings and process this
  *   information in order to control direction and speed.
  * Rev 0.1 14/10/2014
  * - Added support of internal accelerometer.
  * - Code commented.
  * - Included Rx and Tx Buffer in order to allow MSGHDL integration.
  * Rev 0.1.1 14/10/2014
  * - Corrected placing o data into Tx buffer
  * Rev 0.2 18/10/2014
  * - Added XBee services.
  */ 
  
/**
* Header files
*/
#include "camctl.h"         /* Camera controller */
#include "srvctl.h"         /* Servo controller */
#include "mtrctl.h"         /* Motor controller */
#include "xbee.h"           /* XBee controller */
#include "FXOS8700CQ.h"     /* Accelerometer controller */

/**
* Object initialization
*/
DigitalOut MAIN__xPinAlive( LED_BLUE );
PwmOut MAIN__xLEDIllumination( D3 );
FXOS8700CQ IMU( PTE25 /*SDA*/, PTE24 /*SCL*/, FXOS8700CQ_SLAVE_ADDR1/*ADDRESS*/);
Serial xComPort1(USBTX,USBRX);

/**
* Global variable declaration
*/
uint16_t au16ThresholdBuffer[BUFFER_LENGTH] = {0};
uint16_t *pu16DataBuffer;
uint8_t u8Index = 0;
uint8_t u8FrameCtr = 0;
/* Variables to get the line center */
uint32_t u32Sum = 0;
uint8_t u8LinePos = 0;
uint32_t u3SampleCtr = 0;
bool boCalibrationDone = false;
/* Accelerometer variables */
uint8_t u8IMUStatus = 0;
SRAWDATA accData;
SRAWDATA magData;
/* TxBuffer and RxBuffer */
uint8_t au8TxBuffer[9] = {0};
uint8_t au8RxBuffer[9] = {0};
/*
*   Element0 --> u8LinePos
*   Element1 --> u8AccData.x
*   Element3 --> u8AccData.y
*/


/**
* Main program
*/
int main()
{   
    /* Initialization of modules */
    CAMCTL_vInit();
    MTRCTL_vInit();
    SRVCTL_vInit();
    XBee_vInit();
    IMU.enable();
    MAIN__xLEDIllumination.period_us(8333);
    xComPort1.baud(115200);
    
    /**
    * Main loop
    */
    while(true)
    {
        /* Wait until camera is calibrated */
        if(boCalibrationDone == false)
        {
            boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 );
        }
        else
        {
            MAIN__xLEDIllumination = 1.0;
            /* Get frame */          
            CAMCTL_vTriggerOneShotCapture();
            pu16DataBuffer = CAMCTL_pu16GetData();
            while( pu16DataBuffer == NULL )
            {
                pu16DataBuffer = CAMCTL_pu16GetData();
            }
            /* Detect path center */
            for( u8Index = 0; u8Index < (BUFFER_LENGTH); u8Index++ )
            {
                if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] )
                {
                    u32Sum = u32Sum + u8Index;
                    u3SampleCtr++;
                    
                }
                else
                {
                    
                }
            }
            /*Move servo according to camer readings*/
            u8LinePos = (uint8_t)( u32Sum / u3SampleCtr );
            if( u3SampleCtr > 10 )
            {
                SRVCTL_vSetPosition( (uint16_t)(BUFFER_LENGTH-u8LinePos), BUFFER_LENGTH );
                MTRCTL_vSetSystemSpeed( 1000, (uint8_t)(BUFFER_LENGTH-u8LinePos) );    
                /* Report data through COM port */
                xComPort1.printf("CAM: Line position = %u\r\n",u8LinePos);
                /* Place data in Tx buffer */
                au8TxBuffer[0] = u8LinePos;
                
            }
            else
            {
                xComPort1.printf("CAM: Readings not valid\r\n");
            }
            /* Reinitialize variables */
            u32Sum = 0;
            u3SampleCtr = 0;
        }
        u8IMUStatus = IMU.get_data(&accData,&magData);
        if( u8IMUStatus == 0 )
        {
            /* Place data in Tx Buffer */
            au8TxBuffer[1] = (uint8_t)((accData.x)&0x00FF);
            au8TxBuffer[2] = (uint8_t)(((accData.x)&0xFF00)>>8);
            au8TxBuffer[3] = (uint8_t)((accData.y)&0x00FF);
            au8TxBuffer[4] = (uint8_t)(((accData.y)&0xFF00)>>8);
            xComPort1.printf("ACCEL: %i,%i \r\n",accData.x,accData.y);
        }
        else
        {
            xComPort1.printf("ACCEL: Data invalid\r\n");
        }
        
        memcpy(CommandTx,au8TxBuffer,9);
        XBee_u8TxRq = 1;
        
        XBee_vMain();
        
        /* Pin alive indicator */
        MAIN__xPinAlive = !MAIN__xPinAlive;
        wait(0.0001);
    }
}