Diff: main.cpp

Revision:

5:66e1a4c974dd

Parent:

4:823174be9a6b

Child:

6:a13d06616ae1

diff -r 823174be9a6b -r 66e1a4c974dd main.cpp
--- a/main.cpp	Tue Feb 18 17:18:43 2014 +0000
+++ b/main.cpp	Wed Jun 11 13:50:09 2014 +0000
@@ -16,6 +16,24 @@
 
 #include "main.h"   // Certain parameters can be set by changing the defines in piswarm.h
 
+#define rearSensors 90
+#define frontSensors 90
+#define wallsSensors 55
+#define cohesion 50
+#define separation  75
+
+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;
+
+DigitalOut irFront(p12);
+DigitalOut irBack(p13);
+Timer t;
+Timer speed_timer;
+
 
 PiSwarm piswarm;
 Serial pc (USBTX,USBRX);
@@ -23,25 +41,168 @@
 //This is where the program code goes.  In the simple demo, the outer LEDs are blinked.
 int main() {
     init();
-    
-    int step = 0;
-    
+    t.start();
+    speed_timer.start();
+    speed = 0.15;
     while(1) {
-        step ++;
-        if(step==6)step=0;
-        switch (step%3){
-            case 0:  piswarm.set_oled_colour(50,0,0); break;
-            case 1:  piswarm.set_oled_colour(0,50,0); break;
-            case 2:  piswarm.set_oled_colour(0,0,50); break;
+        
+        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 );
         }
-        switch (step%2){
-            case 0:  piswarm.set_oleds(1,0,1,0,1,0,1,0,1,0); break;
-            case 1:  piswarm.set_oleds(0,1,0,1,0,1,0,1,0,1); break;
+        //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();
         }
-        wait(0.25);
+        
+        //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();
+    }
+}
+
+void get_sensors_values()
+{
+    for(loop_counter = 0; loop_counter < 8; loop_counter++ ){
+        sensor_data[loop_counter] = 0.0;
+        sensor_data[loop_counter] = piswarm.read_reflected_ir_distance(loop_counter);
     }
 }
 
+void attraction(void){
+    if(t.read_ms() > 2500){//limit for going without seeing any robot, otherwise check where they are
+        t.stop();
+        t.reset();
+        irFront = 0;
+        irBack = 0;
+        int maxIrValue = 0;
+        int maxIrLoc = 0;
+        unsigned short irValue = 0;
+        for(int i=0;i<8;i++){
+            piswarm.read_adc_value(i);
+        }
+        for(int i=0;i<8;i++){
+            irValue = piswarm.read_adc_value(i);
+            if(irValue > maxIrValue){
+                maxIrValue = irValue;
+                maxIrLoc = i;
+            }
+        }
+        t.start();
+        t.reset();
+        switch(maxIrLoc){
+            case 0:
+            piswarm.right(0.36);
+            while(t.read_ms() < 30){//15 degrees
+            }
+            piswarm.stop();
+            t.stop();
+            break;
+            
+            case 1:
+            piswarm.right(0.36);
+            while(t.read_ms() < 90){//45 degrees
+            }
+            piswarm.stop();
+            t.stop();                
+            break;
+
+            case 2:
+            piswarm.right(0.36);
+            while(t.read_ms() < 180){//90 degrees
+            }
+            piswarm.stop();
+            t.stop();                
+            break;
+
+            case 3:                
+            piswarm.right(0.36);
+            while(t.read_ms() < 288){//144 degrees
+            }
+            piswarm.stop();
+            t.stop();
+            break;
+
+            case 4:
+            piswarm.left(0.36);
+            while(t.read_ms() < 288){//216 degrees
+            }
+            piswarm.stop();
+            t.stop();
+            break;
+
+            case 5:
+            piswarm.left(0.36);
+            while(t.read_ms() < 180){//270 degrees
+            }
+            piswarm.stop();
+            t.stop();                
+            break;
+
+            case 6:
+            piswarm.left(0.36);
+            while(t.read_ms() < 90){//315 degrees
+            }
+            piswarm.stop();
+            t.stop();                
+            break;
+
+            case 7:
+            piswarm.left(0.36);
+            while(t.read_ms() < 30){//345 degrees
+            }
+            piswarm.stop();
+            t.stop();                
+            break;
+        }
+        t.start();
+        irFront = 1;
+        irBack = 1;
+    }
+
+}
 /***************************************************************************************************************************************
  *
  * Beyond this point, empty code blocks for optional functions is given
@@ -65,7 +226,12 @@
     // 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...
 }