Diff: communications.cpp

Revision:

3:4c0f2f3de33e

Parent:

2:0a739218ab11

Child:

4:52b3e4c5a425

--- a/communications.cpp	Sun Feb 02 20:37:48 2014 +0000
+++ b/communications.cpp	Sun Feb 02 21:18:05 2014 +0000
@@ -410,15 +410,47 @@
             response[1] = sgyro % 256;
             break;
         }
-        case 10: // Request background IR reading
+        case 10: { // Request background IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                unsigned short m_val = piswarm.read_adc_value(sensor);
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
             break;
-        case 11: // Request illuminated IR reading
+        }
+        case 11: { // Request illuminated IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                unsigned short m_val = piswarm.read_illuminated_raw_ir_value(sensor);
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
             break;
-        case 12: // Request line-tracking IR reading
+        }
+
+        case 12: { // Request distance IR reading
+            response_length = 16;
+            response = new char[16];
+            for(int sensor = 0; sensor < 8; sensor ++){
+                int offset = sensor * 2;
+                float f_val = piswarm.read_reflected_ir_distance(sensor);
+                //f_val ranges from 0 to 100.0;
+                f_val *= 655;
+                unsigned short m_val = (unsigned short) f_val;
+                response[offset]=m_val >> 8;
+                response[offset+1]=m_val % 256;
+            }
             break;
-        case 13: // Request uptime
+        }
+            
+        case 13: // Request line-tracking IR reading
             break;
-        case 14: //
+        case 14: // Request uptime
             break;
         case 15: //
             break;
@@ -983,6 +1015,11 @@
         swarm[i].battery = 0.0;
         swarm[i].light_sensor = 0.0;
         swarm[i].gyro = 0.0;
+        for(int k=0; k<8; k++) {
+          swarm[i].background_ir[k] = 0;
+          swarm[i].reflected_ir[k] = 0;
+          swarm[i].distance_ir[k] = 0;
+        }
     }
 }
 
@@ -1214,6 +1251,12 @@
                     if(success == 1) {
                         if(length == 16) {
                             swarm[sender].status_rf_request_background_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                swarm[sender].background_ir[i]=value;   
+                            }
                             outcome = 1;
                         } else {
                             swarm[sender].status_rf_request_background_ir = 3;
@@ -1232,6 +1275,12 @@
                     if(success == 1) {
                         if(length == 16) {
                             swarm[sender].status_rf_request_reflected_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                swarm[sender].reflected_ir[i]=value;   
+                            }
                             outcome = 1;
                         } else {
                             swarm[sender].status_rf_request_reflected_ir = 3;
@@ -1250,6 +1299,13 @@
                     if(success == 1) {
                         if(length == 16) {
                             swarm[sender].status_rf_request_distance_ir = 2;
+                            for(int i=0;i<8;i++){
+                                int offset = i * 2;
+                                unsigned short value = (unsigned short) data[offset] << 8;
+                                value += data[offset+1];
+                                float adjusted = (float) value / 655.0;
+                                swarm[sender].distance_ir[i]=adjusted;   
+                            }
                             outcome = 1;
                         } else {
                             swarm[sender].status_rf_request_distance_ir = 3;