Diff: main.cpp

Revision:

0:2a825db40e1b

Child:

1:a457798861a4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Oct 14 04:00:40 2014 +0000
@@ -0,0 +1,75 @@
+#include "camctl.h"
+#include "srvctl.h"
+#include "mtrctl.h"
+
+DigitalOut MAIN__xPinAlive( LED_BLUE );
+Serial xComPort1(USBTX,USBRX);
+
+
+/**********************************************************************************
+* CAMCTL-Module
+***********************************************************************************/
+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;
+uint32_t u3SampleCtr = 0;
+bool boCalibrationDone = false;
+
+int main()
+{   
+    /* Initialization of modules */
+    CAMCTL_vInit();
+    MTRCTL_vInit();
+    SRVCTL_vInit();
+    xComPort1.baud(115200);
+    /* Main loop */
+    while(true)
+    {
+        /* Wait until camera is calibrated */
+        if(boCalibrationDone == false)
+        {
+            boCalibrationDone = CAMCTL_boCalibrateCamera( au16ThresholdBuffer, 4 );
+        }
+        else
+        {
+            /* 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*/
+            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));
+            /* Reinitialize variables */
+            u32Sum = 0;
+            u3SampleCtr = 0;
+        }
+        /* Pin alive message */
+        MAIN__xPinAlive = !MAIN__xPinAlive;
+        wait(0.0001);
+    }
+}
+