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);
     }