Diff: main.cpp

Revision:

2:f501e506d62b

Parent:

1:2e895e5272ab

Child:

3:7b0c215eb6a7

--- a/main.cpp	Fri Mar 31 19:22:14 2017 +0000
+++ b/main.cpp	Fri Mar 31 22:24:46 2017 +0000
@@ -5,12 +5,26 @@
 
 I2C camera1(p9, p10);
 
+//initial camera data
 int IRsensorAddress = 0xB0;
 int slaveAddress;
 char data_buf[16];
+char s;
 int i;
 
 
+//point variables
+int point1x = 0;
+int point1y = 0;
+int point2x = 0;
+int point2y = 0;
+int point3x = 0;
+int point3y = 0;
+int point4x = 0;
+int point4y = 0;
+
+
+
 //sensitivity
 //Level 5: p0 = 0x96, p1 = 0xFE, p2 = 0xFE, p3 = 0x05
 int sen0 = 0x96;
@@ -18,10 +32,10 @@
 int sen2 = 0xFE;
 int sen3 = 0x00;
 
+//matrices of x and y coordinates
+int onex[4];
+int oney[4];
 
-int Ix[4];
-int Iy[4];
-int s;
 
 void write2bytes(char data1, char data2){
     char out[2];
@@ -47,6 +61,113 @@
 
 }
 
+void readCameraData(void){
+        
+    //request data from camera 
+    char out[1];
+    out[0] = 0x36;   
+    camera1.write(slaveAddress, out, 1);
+    //wait(0.2); //do we need this?
+    
+    //get data from camera
+    camera1.read(slaveAddress, data_buf, 16);
+    
+    //print content of data buffer
+    for(int i = 0; i < 16; i++){
+        pc.printf(" data buf %d --> %c <--    \t as int --> %d <--\n", i, data_buf[i], (float) data_buf[i]);
+        
+    }
+   
+    
+    
+    
+    //POINT 1
+    //get data
+    point1x = data_buf[1];
+    point1y = data_buf[2];
+    s = data_buf[3];
+    //load x,y
+    point1x += (s & 0x30) << 4;
+    point1y += (s & 0xC0) << 2;
+    
+    pc.printf("point1x %d <-- here\n", point1x);
+    
+    onex[0] = point1x;
+    oney[0] = point1y;
+    
+    //onex[0] = point1x + ((s & 0x30) << 4);
+    //oney[0] = point1y + ((s & 0xC0) << 2);
+    
+    
+
+    
+    
+    //POINT 2
+    //get data
+    point2x = data_buf[4];
+    point2y = data_buf[5];
+    s = data_buf[6];
+    //load x,y
+    onex[1] = point2x + ((s & 0x30) << 4);
+    oney[1] = point2y + ((s & 0xC0) << 2);
+    
+    
+    //POINT 3
+    //get data
+    point3x = data_buf[7];
+    point3y = data_buf[8];
+    s = data_buf[9];
+    //load x,y
+    onex[2] = point3x + ((s & 0x30) << 4);
+    oney[2] = point3y + ((s & 0xC0) << 2);
+    
+    
+    //POINT 4
+    //get data
+    point4x = data_buf[10];
+    point4y = data_buf[11];
+    s = data_buf[12];
+    //load x,y
+    onex[3] = point4x + ((s & 0x30) << 4);
+    oney[3] = point4y + ((s & 0xC0) << 2);
+    
+}
+
+void printCamData(int xcor[4], int ycor[4]){
+    for(int i = 0; i<4; i++){
+        int x = xcor[i];
+        int y = ycor[i];
+        //determine what to print
+        //x coordinate
+        if(x < 1000){
+            pc.printf("");
+        }
+        if(x < 100){
+            pc.printf("");
+        }
+        if(x < 10){
+            pc.printf(" %d,", x);
+        }
+        
+        //y coordinate
+        if(y < 1000){
+            pc.printf("");
+        }
+        if(y < 100){
+            pc.printf("");
+        }
+        if(y < 10){
+            pc.printf(" %d", y);
+        }
+        
+        pc.printf("\t");        
+    }
+    
+    //new line and delay
+    pc.printf("\n");      
+    wait(0.1);
+}
+
 int main() {
     myled = 0;
     slaveAddress = IRsensorAddress >> 1;
@@ -54,13 +175,19 @@
     
     
     while(1) {
-        myled = myled - 1;
-        char out[1];
-        out[0] = 0x36;
+        myled = 1 - myled;
+        
+        //get the camera data
+        readCameraData();
         
-        camera1.write(slaveAddress, out, 1);
+        //print points
+        printCamData(onex, oney);
+        
         
-        wait(0.2);
+        
+        
+
+        
         
     }
 }