This is the source code used by master vehicle. Modules: CAMCTL SRVCTL MTRCTL still missing i2c readings from accelerometer and rf communication
Dependencies: CAMCTL MTRCTL SRVCTL mbed FXOS8700CQ XBEE
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
Generated on Wed Aug 10 2022 14:12:58 by 1.7.2