Diff: main.cpp

Revision:

2:4b6f6fc84793

Parent:

1:a4883d9c75ec

Child:

3:87a5122682fa

--- a/main.cpp	Fri Oct 14 15:55:16 2016 +0000
+++ b/main.cpp	Fri Oct 28 10:19:09 2016 +0000
@@ -1,22 +1,34 @@
 #include "mbed.h"
 #include "TFC.h"
-//#define CAM_THRESHOLD 3000
+#define CAM_THRESHOLD 3000
  
- 
+Serial pc(USBTX,USBRX); 
 DigitalOut myled(LED1);
  
 int main() {
+    //Serial config
+    pc.baud(57600);
+    
+    
     TFC_Init();
     
     uint32_t i,t = 0;
     //uint32_t width = 0;
     char curr_line[129];
+    int leftSide, rightSide;
+    float centreOffset;
     
     /* Ensure string is null terminated */
     curr_line[128] = 0; 
+    
+    
+    char thres_line[16];
+    for(i = 0; i < 16; i++) {
+        thres_line[i] = 0;
+    }
  
     
-    printf("Starting camera test program\n");
+    pc.printf("Starting camera test program\n");
     
     while(1) {
         //If we have an image ready
@@ -24,8 +36,8 @@
             {
                 //Reset image ready flag
                 TFC_LineScanImageReady=0;
-                printf("\r\n");
-                //printf("L:");
+                pc.printf("\r\n");
+                //pc.printf("L:");
                 
                 //Strange thing that cycles the battery indicator
                 //Leaving in as this is a good indicator that the camera test program is running
@@ -34,36 +46,74 @@
                 else
                     t++;
                 TFC_SetBatteryLED_Level(t);
-                   
-                for(i = 0; i < 128; i++) {
-                    curr_line[i] = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
-                    printf("%02x", curr_line[i]);
+                
+                //Byte sending
+                //for(i = 0; i < 128; i++) {
+                    //curr_line[i] = (int8_t)(TFC_LineScanImage0[i] >> 4) & 0xFF;
+                    //pc.printf("%02x", curr_line[i]);
+                //}
+                pc.printf("\n");
+                
+                
+                                    //NEW CENTRE DETECTION CODE
+                //Centre of track detection
+                //First, start at the centre of the 128 bit array
+                //Moving left, start at 63 and move 63 steps left (to get to 0)
+                //Moving right, start at 64 and move 63 steps right (to get to 127)
+                
+                //Move leftwards, stopping where we reach the edge of the track, and record the index where this occurred
+                for (i = 63; i > 0; i--) {
+                    if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
+                        leftSide = i;
+                        break;
+                        }
                 }
-                printf("\n");
+                
+                //Then, move rightwards, stopping where we reach the edge of the track, record the index where this occurred
+                for (i = 64; i < 127; i++) {
+                    if(TFC_LineScanImage0[i] < CAM_THRESHOLD) {
+                        rightSide = i;
+                        break;
+                        }
+                }
+                
+                //centreOffset = (leftSide + rightSide)/2;
+                centreOffset = (leftSide - rightSide) / 127;
+                
+                //Send offset over serial to hosted program
+                pc.putc('E');
+                pc.printf("%f", centreOffset);
+                //pc.printf("centre: %d\n", centreOffset);
+                                ///END OF NEW CODE
+                
                 
                 //Old version below, we don't threshold on the FRDM board anymore
-                //printf("0x%x\n", curr_line);
-               /*
+                //pc.printf("0x%x\n", curr_line);
+               
+               
                //Loop through camera pixels
                for(i=0;i<128;i++)
                {
                    //If the pixel value exceeds our threshold, print a 1
                    if (TFC_LineScanImage0[i] > CAM_THRESHOLD) 
                    {
-                       curr_line[i] = '1';
-                       width++; //Increment the width value
+                       //curr_line[i] = '1';
+                       thres_line[i/8] |= 1 << (i%8);
                    }
                    //Else, print a 0
                    else
                    {
-                      curr_line[i] = '0';  
+                      //curr_line[i] = '0';  
                    }
                }
-               printf("%s\n",curr_line);
+               pc.putc('I');
+                for(i = 0; i < 16; i++) {
+                    pc.putc(thres_line[i]);
+                }
                 
-                printf("Width: %d\n", width);
-                width = 0;
-                */
+                //pc.printf("Width: %d\n", width);
+                //width = 0;
+                
                    
                }
     }