main.cpp

00001  /** 
00002   * File:    frdm_MasterVehicle/main.cpp
00003   * 
00004   * Author1:  Carla Amaya
00005   * Author2:  Gerardo Cordero
00006   * Author3:  José Pesado
00007   * Author3:  Jalil Chávez
00008   *
00009   * Date:     October 2014  
00010   * Course:   Instrumentation
00011   * 
00012   * Summary of File: 
00013   * 
00014   *   This file contains code which controlls a line follower.
00015   *   This device takes as input camera readings and process this
00016   *   information in order to control direction and speed.
00017   * Rev 0.1 14/10/2014
00018   * - Added support of internal accelerometer.
00019   * - Code commented.
00020   * - Included Rx and Tx Buffer in order to allow MSGHDL integration.
00021   * Rev 0.1.1 14/10/2014
00022   * - Corrected placing o data into Tx buffer
00023   * Rev 0.2 18/10/2014
00024   * - Added XBee services.
00025   */ 
00026   
00027 /**
00028 * Header files
00029 */
00030 #include "camctl.h"         /* Camera controller */
00031 #include "srvctl.h"         /* Servo controller */
00032 #include "mtrctl.h"         /* Motor controller */
00033 #include "xbee.h"           /* XBee controller */
00034 #include "FXOS8700CQ.h"     /* Accelerometer controller */
00035 
00036 /**
00037 * Object initialization
00038 */
00039 DigitalOut MAIN__xPinAlive( LED_BLUE );
00040 PwmOut MAIN__xLEDIllumination( D3 );
00041 FXOS8700CQ IMU( PTE25 /*SDA*/, PTE24 /*SCL*/, FXOS8700CQ_SLAVE_ADDR1/*ADDRESS*/);
00042 Serial xComPort1(USBTX,USBRX);
00043 
00044 /**
00045 * Global variable declaration
00046 */
00047 uint16_t au16ThresholdBuffer[BUFFER_LENGTH] = {0};
00048 uint16_t *pu16DataBuffer;
00049 uint8_t u8Index = 0;
00050 uint8_t u8FrameCtr = 0;
00051 /* Variables to get the line center */
00052 uint32_t u32Sum = 0;
00053 uint8_t u8LinePos = 0;
00054 uint32_t u3SampleCtr = 0;
00055 bool boCalibrationDone = false;
00056 /* Accelerometer variables */
00057 uint8_t u8IMUStatus = 0;
00058 SRAWDATA accData;
00059 SRAWDATA magData;
00060 /* TxBuffer and RxBuffer */
00061 uint8_t au8TxBuffer[9] = {0};
00062 uint8_t au8RxBuffer[9] = {0};
00063 /*
00064 *   Element0 --> u8LinePos
00065 *   Element1 --> u8AccData.x
00066 *   Element3 --> u8AccData.y
00067 */
00068 
00069 
00070 /**
00071 * Main program
00072 */
00073 int main()
00074 {   
00075     /* Initialization of modules */
00076     CAMCTL_vInit();
00077     MTRCTL_vInit();
00078     SRVCTL_vInit();
00079     XBee_vInit();
00080     IMU.enable();
00081     MAIN__xLEDIllumination.period_us(8333);
00082     xComPort1.baud(115200);
00083     
00084     /**
00085     * Main loop
00086     */
00087     while(true)
00088     {
00089         /* Wait until camera is calibrated */
00090         if(boCalibrationDone == false)
00091         {
00092             boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 );
00093         }
00094         else
00095         {
00096             MAIN__xLEDIllumination = 1.0;
00097             /* Get frame */          
00098             CAMCTL_vTriggerOneShotCapture();
00099             pu16DataBuffer = CAMCTL_pu16GetData();
00100             while( pu16DataBuffer == NULL )
00101             {
00102                 pu16DataBuffer = CAMCTL_pu16GetData();
00103             }
00104             /* Detect path center */
00105             for( u8Index = 0; u8Index < (BUFFER_LENGTH); u8Index++ )
00106             {
00107                 if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] )
00108                 {
00109                     u32Sum = u32Sum + u8Index;
00110                     u3SampleCtr++;
00111                     
00112                 }
00113                 else
00114                 {
00115                     
00116                 }
00117             }
00118             /*Move servo according to camer readings*/
00119             u8LinePos = (uint8_t)( u32Sum / u3SampleCtr );
00120             if( u3SampleCtr > 10 )
00121             {
00122                 SRVCTL_vSetPosition( (uint16_t)(BUFFER_LENGTH-u8LinePos), BUFFER_LENGTH );
00123                 MTRCTL_vSetSystemSpeed( 1000, (uint8_t)(BUFFER_LENGTH-u8LinePos) );    
00124                 /* Report data through COM port */
00125                 xComPort1.printf("CAM: Line position = %u\r\n",u8LinePos);
00126                 /* Place data in Tx buffer */
00127                 au8TxBuffer[0] = u8LinePos;
00128                 
00129             }
00130             else
00131             {
00132                 xComPort1.printf("CAM: Readings not valid\r\n");
00133             }
00134             /* Reinitialize variables */
00135             u32Sum = 0;
00136             u3SampleCtr = 0;
00137         }
00138         u8IMUStatus = IMU.get_data(&accData,&magData);
00139         if( u8IMUStatus == 0 )
00140         {
00141             /* Place data in Tx Buffer */
00142             au8TxBuffer[1] = (uint8_t)((accData.x)&0x00FF);
00143             au8TxBuffer[2] = (uint8_t)(((accData.x)&0xFF00)>>8);
00144             au8TxBuffer[3] = (uint8_t)((accData.y)&0x00FF);
00145             au8TxBuffer[4] = (uint8_t)(((accData.y)&0xFF00)>>8);
00146             xComPort1.printf("ACCEL: %i,%i \r\n",accData.x,accData.y);
00147         }
00148         else
00149         {
00150             xComPort1.printf("ACCEL: Data invalid\r\n");
00151         }
00152         
00153         memcpy(CommandTx,au8TxBuffer,9);
00154         XBee_u8TxRq = 1;
00155         
00156         XBee_vMain();
00157         
00158         /* Pin alive indicator */
00159         MAIN__xPinAlive = !MAIN__xPinAlive;
00160         wait(0.0001);
00161     }
00162 }
00163