Revision 15:be92991621b4, committed 2017-07-14

Comitter:

JessicaGao

Date:

Fri Jul 14 12:34:22 2017 +0000

Parent:

14:d32d948f7a76

Commit message:

detect beacon;

Changed in this revision

diff -r d32d948f7a76 -r be92991621b4 psiswarm.cpp
--- a/psiswarm.cpp	Tue Jun 27 17:01:58 2017 +0000
+++ b/psiswarm.cpp	Fri Jul 14 12:34:22 2017 +0000
@@ -125,7 +125,7 @@
 float line_position = 0;
 char line_found = 0;
 
-unsigned short background_ir_values [8];
+char background_ir_values [8];
 unsigned short illuminated_ir_values [8];
 float reflected_ir_distances [8];
 char ir_values_stored = 0;

diff -r d32d948f7a76 -r be92991621b4 psiswarm.h
--- a/psiswarm.h	Tue Jun 27 17:01:58 2017 +0000
+++ b/psiswarm.h	Fri Jul 14 12:34:22 2017 +0000
@@ -187,7 +187,7 @@
 extern Timeout ultrasonic_timeout;
 extern Ticker ultrasonic_ticker;
 
-extern unsigned short background_ir_values [];
+extern char background_ir_values [];
 extern unsigned short illuminated_ir_values [];
 extern float reflected_ir_distances [];
 extern char ir_values_stored;

diff -r d32d948f7a76 -r be92991621b4 sensors.cpp
--- a/sensors.cpp	Tue Jun 27 17:01:58 2017 +0000
+++ b/sensors.cpp	Fri Jul 14 12:34:22 2017 +0000
@@ -172,7 +172,7 @@
 }
 
 // Returns the stored value of the non-illuminated sensor based on last call of store_background_raw_ir_values
-unsigned short Sensors::get_background_raw_ir_value ( char index )
+char Sensors::get_background_raw_ir_value ( char index )
 {
     // Sanity check: check range of index and that values have been stored
     if (index>7 || ir_values_stored == 0) return 0.0;

diff -r d32d948f7a76 -r be92991621b4 sensors.h
--- a/sensors.h	Tue Jun 27 17:01:58 2017 +0000
+++ b/sensors.h	Fri Jul 14 12:34:22 2017 +0000
@@ -91,7 +91,7 @@
     * @param index - The index of the sensor to read (range 0 to 7, clockwise around robot from front-right)
     * @return Unsigned short of background IR reading (range 0 to 4095)
     */
-    unsigned short get_background_raw_ir_value ( char index );
+    char get_background_raw_ir_value ( char index );
     
     /** Returns the stored value of the illuminated side-facing IR sensor value based on last call of store_illuminated_raw_ir_values
     * Call either store_ir_values() or store_illuminated_raw_ir_values() before using this function to update reading

diff -r d32d948f7a76 -r be92991621b4 serial.cpp
--- a/serial.cpp	Tue Jun 27 17:01:58 2017 +0000
+++ b/serial.cpp	Fri Jul 14 12:34:22 2017 +0000
@@ -1012,40 +1012,53 @@
             send_message = 1;
             break;
         case 125:
+            send_message = 2;
             strcpy(command,"GET BEACON DATA");
-            if(allow_commands) {
-                command_status = 1;
-                beacon_data_needing = 1;
-            } else command_status = 2;
+            unsigned short beacon_data[8];
+            sensors.store_background_raw_ir_values();
+            for(int sensor=0; sensor<8; sensor++) {
+                beacon_data[sensor] = sensors.get_background_raw_ir_value(sensor);
+            }
+            // beacon_data holds the array of IR values at the middle of the beacon flash
+            
+            // As an example - we will now wait for 20% of the beacon period and check the background value
+            // background_data holds the true background values (no beacon)
+            int maximum1 = 0, maximum2 = 0, max1_sensor = 8, max2_sensor = 8; 
+            for(int i = 0; i < 8; i++){
+                if(beacon_data[i] > maximum1){
+                    maximum2 = maximum1;
+                    max2_sensor = max1_sensor;
+                    max1_sensor = i;
+                    maximum1 = beacon_data[i];
+                }
+                else if(beacon_data[i] > maximum2){
+                    max2_sensor = i;
+                    maximum2 = beacon_data[i];
+                }
+            }
+            pc.printf("%c%1d,%4d,%1d,%4d,",RESPONSE_MESSAGE_BYTE,max1_sensor,maximum1,max2_sensor,maximum2);
             break;
-        case 126:
-            strcpy(command,"GET BACKGROUND  DATA");
-            if(allow_commands) {
-                command_status = 1;
-                beacon_data_needing = 2;
-            } else command_status = 2;
+        case 255:
+            send_message = 2;
+            pc.printf("swarm");
             break;
-        case 127:
-            strcpy(command,"GET BOTH BEACON AND BACKGROUND  DATA");
-            if(allow_commands) {
-                command_status = 1;
-                beacon_data_needing = 3;
-            } else command_status = 2;
-            break;
+            
     }
 
 
-    if(send_message) {
-        char message_length = strlen(ret_message);
-        switch(interface) {
-            case 0:
-                pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
-                break;
-            case 1:
-                bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
-                break;
+    if(send_message>0) {
+        if(send_message == 1){
+            char message_length = strlen(ret_message);
+            switch(interface) {
+                case 0:
+                    pc.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
+                    break;
+                case 1:
+                    bt.printf("%c%c%s",RESPONSE_MESSAGE_BYTE,message_length,ret_message);
+                    break;
+            }
+        psi.debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
         }
-        psi.debug("Received %s request message: %s %s [%02x%02x%02x]\nReply: %s [%d ch]\n",iface, command, subcommand,message[0],message[1],message[2],ret_message,message_length);
     } else {
         switch(interface) {
             case 0: