Diff: main.cpp

Revision:

6:a13d06616ae1

Parent:

5:66e1a4c974dd

diff -r 66e1a4c974dd -r a13d06616ae1 main.cpp
--- a/main.cpp	Wed Jun 11 13:50:09 2014 +0000
+++ b/main.cpp	Wed Aug 13 10:19:14 2014 +0000
@@ -16,23 +16,23 @@
 
 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
 
+//Define constants
+#define delay 0.05
 #define rearSensors 90
 #define frontSensors 90
 #define wallsSensors 55
-#define cohesion 50
-#define separation  75
+#define cohesion 45
+#define separation  85
 
+//Define global variables
 uint8_t loop_counter = 0;
 float sensor_data[8] = {0};
-volatile uint8_t proximity_signal[4] = {0};
-float speed = 0;
-float speed_incrementor = 0;
-float delay = 0;
+uint8_t noise_indicator = 0;
+float speed = 0.075;
 
 DigitalOut irFront(p12);
 DigitalOut irBack(p13);
 Timer t;
-Timer speed_timer;
 
 
 PiSwarm piswarm;
@@ -42,67 +42,12 @@
 int main() {
     init();
     t.start();
-    speed_timer.start();
-    speed = 0.15;
     while(1) {
-        
-        speed_incrementor = 0.08 + speed / 10;
-        delay = 0.12 - (speed / 10) * 4;
-        get_sensors_values();
-        if( speed_timer.read_ms() > 1000 ){ 
-            if( speed > 0.05 ) speed -= 0.005;
-            for( loop_counter = 0; loop_counter < 3; loop_counter++ ){
-                if( proximity_signal[loop_counter] == 1 && speed < 0.2 ) speed += 0.005; 
-            }
-            speed_timer.reset();    
-            //piswarm.cls();
-            //piswarm.printf("%0.2f %d%d%d\n\r", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
-        }    
-        
-        //Forward and backward condition 
-        while( ( sensor_data[0] + sensor_data[7] ) / 2 > ( separation + 15 ) )
-        {
-            attraction();
-            piswarm.forward( speed );
-            piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
-            get_sensors_values();
-            wait( delay );
-        }
-        while( ( sensor_data[0] + sensor_data[7] ) / 2 < ( separation - 15 ) && ( sensor_data[3] + sensor_data[4] ) / 2 > ( separation - 15 ) )
-        {
-            piswarm.backward( speed_incrementor );
-            piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
-            get_sensors_values();
-            wait( delay );
-        }
-        //Front-left separation
-        while( sensor_data[6] < separation || sensor_data[5] < separation )
-        {
-            t.stop();
-            t.reset();
-            piswarm.left( speed_incrementor );
-            piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
-            get_sensors_values();
-            wait( delay );
-            t.start();
-        }
-        
-        //Front-right separation
-        while( sensor_data[1] < separation || sensor_data[2] < separation)
-        {
-            t.stop();
-            t.reset();
-            piswarm.right( speed_incrementor );
-            piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
-            get_sensors_values();
-            wait( delay );
-            t.start();
-        } 
-        piswarm.stop();     
-        attraction();
+        if ( noise_indicator == 1) flocking();
+        else aggregation();
     }
 }
-
+//This function collects all outer IR sensors information and store them in an array.
 void get_sensors_values()
 {
     for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
@@ -110,8 +55,8 @@
         sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
     }
 }
-
-void attraction(void){
+//Detect strongest source of IR and face towards it. Cosntant defines rate of spin.
+void attraction( float direction_constant ){
     if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
         t.stop();
         t.reset();
@@ -201,8 +146,98 @@
         irFront = 1;
         irBack = 1;
     }
+}
 
+//Do flocking behaviour
+void flocking( void ){
+    get_sensors_values();
+    //Forward and backward condition 
+    while( ( sensor_data[0] + sensor_data[7] ) / 2 > separation && sensor_data[1] > cohesion && sensor_data[6] > cohesion && sensor_data[5] > cohesion && sensor_data[2] > cohesion)
+    {
+        attraction( 0.5 );
+        piswarm.forward( speed );
+        piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
+        get_sensors_values();
+        wait( delay );
+    }
+    while( ( sensor_data[0] + sensor_data[7] ) / 2 < cohesion && ( sensor_data[3] + sensor_data[4] ) / 2 > cohesion )
+    {
+        piswarm.backward( speed );
+        piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+    }
+    //Front-left separation
+    while( sensor_data[6] < cohesion || sensor_data[5] < cohesion )
+    {
+        t.stop();
+        t.reset();
+        piswarm.right( speed );
+        piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+        t.start();
+    }
+    
+    //Front-right separation
+    while( sensor_data[1] < separation || sensor_data[2] < cohesion)
+    {
+        t.stop();
+        t.reset();
+        piswarm.left( speed );
+        piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+        t.start();
+    } 
+    piswarm.stop();     
+    attraction( 1.0 );
 }
+//Do aggregation behaviour
+void aggregation( void ){
+    get_sensors_values();
+    //Forward and backward condition 
+    while( ( sensor_data[0] + sensor_data[7] ) / 2 > separation && sensor_data[1] > cohesion && sensor_data[6] > cohesion && sensor_data[5] > cohesion && sensor_data[2] > cohesion)
+    {
+        attraction( 0.5 );
+        piswarm.forward( speed );
+        piswarm.set_oleds( 1, 0, 0, 0, 0, 0, 0, 0, 1, 1 );
+        get_sensors_values();
+        wait( delay );
+    }
+    /*while( ( sensor_data[0] + sensor_data[7] ) / 2 < cohesion && ( sensor_data[3] + sensor_data[4] ) / 2 > cohesion )
+    {
+        piswarm.backward( speed );
+        piswarm.set_oleds( 0, 0, 0, 1, 1, 1, 0, 0, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+    }*/
+    //Front-left separation
+    while( sensor_data[6] < cohesion || sensor_data[5] < cohesion )
+    {
+        t.stop();
+        t.reset();
+        piswarm.left( speed );
+        piswarm.set_oleds( 0, 0, 0, 0, 0, 0, 1, 1, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+        t.start();
+    }
+    
+    //Front-right separation
+    while( sensor_data[1] < separation || sensor_data[2] < cohesion)
+    {
+        t.stop();
+        t.reset();
+        piswarm.right( speed );
+        piswarm.set_oleds( 0, 1, 1, 0, 0, 0, 0, 0, 0, 0 );
+        get_sensors_values();
+        wait( delay );
+        t.start();
+    } 
+    piswarm.stop();     
+    attraction( 1.0 );
+}    
 /***************************************************************************************************************************************
  *
  * Beyond this point, empty code blocks for optional functions is given
@@ -226,13 +261,14 @@
     // function = The function identifier.  Range 0 to 15
     // * data = Array containing extra data bytes
     // length = Length of extra data bytes held (range 0 to 57)
-     proximity_signal[0] = data[0];
-     proximity_signal[1] = data[1];
-     proximity_signal[2] = data[2];
-     piswarm.cls();
-     piswarm.printf("%0.2f %d%d%d", speed, proximity_signal[0], proximity_signal[1], proximity_signal[2]);
-       
+      
     //Do something...
+    uint8_t speed_digit_1 = data[2] - '0';
+    uint8_t speed_digit_2 = data[3] - '0';
+    speed = speed_digit_1 * 0.1 + speed_digit_2 * 0.01;
+    noise_indicator = data[4];
+    piswarm.cls();
+    piswarm.printf("%0.2f %i", speed, noise_indicator);
 }    
 
 void handleUserRFResponse(char sender, char broadcast_message, char success, char id, char is_command, char function, char * data, char length){