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
Diff: main.cpp
- Revision:
- 1:a457798861a4
- Parent:
- 0:2a825db40e1b
- Child:
- 2:c878e4203b62
--- a/main.cpp Tue Oct 14 04:00:40 2014 +0000 +++ b/main.cpp Fri Oct 17 21:50:24 2014 +0000 @@ -1,32 +1,83 @@ -#include "camctl.h" -#include "srvctl.h" -#include "mtrctl.h" + /** + * 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. + */ + +/** +* 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); - -/********************************************************************************** -* CAMCTL-Module -***********************************************************************************/ +/** +* 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; -uint32_t u32Avg = 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 */ + + /** + * Main loop + */ while(true) { /* Wait until camera is calibrated */ @@ -36,6 +87,7 @@ } else { + MAIN__xLEDIllumination = 1.0; /* Get frame */ CAMCTL_vTriggerOneShotCapture(); pu16DataBuffer = CAMCTL_pu16GetData(); @@ -44,7 +96,7 @@ pu16DataBuffer = CAMCTL_pu16GetData(); } /* Detect path center */ - for( u8Index = 0; u8Index < BUFFER_LENGTH; u8Index++ ) + for( u8Index = 0; u8Index < (BUFFER_LENGTH); u8Index++ ) { if( pu16DataBuffer[u8Index] < au16ThresholdBuffer[u8Index] ) { @@ -58,16 +110,38 @@ } } /*Move servo according to camer readings*/ - u32Avg = (uint32_t)( u32Sum / u3SampleCtr ); - SRVCTL_vSetPosition( u32Avg, BUFFER_LENGTH ); - MTRCTL_vSetSystemSpeed( 400, (uint8_t)(u32Avg) ); - /* Report data through COM port */ - xComPort1.printf("u8CenterPos = %f\r\n",(float)(u32Avg/128.0)); + 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; } - /* Pin alive message */ + u8IMUStatus = IMU.get_data(&accData,&magData); + if( u8IMUStatus == 0 ) + { + /* Place data in Tx Buffer */ + au8TxBuffer[1] = accData.x; + au8TxBuffer[3] = accData.y; + 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); }