C++ Library for the PsiSwarm Robot - Version 0.8

Dependents:   PsiSwarm_V8_Blank_CPP Autonomia_RndmWlk

Fork of PsiSwarmV7_CPP by Psi Swarm Robot

Files at this revision

API Documentation at this revision

Comitter:
JessicaGao
Date:
Fri Jul 14 12:34:22 2017 +0000
Parent:
14:d32d948f7a76
Commit message:
detect beacon;

Changed in this revision

psiswarm.cpp Show annotated file Show diff for this revision Revisions of this file
psiswarm.h Show annotated file Show diff for this revision Revisions of this file
sensors.cpp Show annotated file Show diff for this revision Revisions of this file
sensors.h Show annotated file Show diff for this revision Revisions of this file
serial.cpp Show annotated file Show diff for this revision Revisions of this file
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: