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
- Committer:
- JalilChavez
- Date:
- 2014-10-17
- Revision:
- 2:c878e4203b62
- Parent:
- 1:a457798861a4
- Child:
- 3:7bdacc4cf273
File content as of revision 2:c878e4203b62:
/** * 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 */ /** * Header files */ #include "camctl.h" /* Camera controller */ #include "srvctl.h" /* Servo controller */ #include "mtrctl.h" /* Motor 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(); 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"); } /* Pin alive indicator */ MAIN__xPinAlive = !MAIN__xPinAlive; wait(0.0001); } }