Diff: programs.cpp

Revision:

12:daa53285b6e4

Parent:

11:7b3ee540ba56

Child:

13:f5994956b1ba

--- a/programs.cpp	Tue Oct 27 00:13:49 2015 +0000
+++ b/programs.cpp	Tue Oct 27 13:03:10 2015 +0000
@@ -7,6 +7,9 @@
 
 #include "main.h"
 
+int obstacle_avoidance_threshold = 300;
+int robot_avoidance_threshold = 2000;
+
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// head_to_bearing_program
 char was_turning = 0;
@@ -180,7 +183,6 @@
 char random_walk_timeout = 0;
 float previous_left_motor_speed = 0.5;
 float previous_right_motor_speed = 0.5;
-int obstacle_avoidance_threshold = 300;
 
 void curved_random_walk_with_interaction_program()
 {
@@ -297,7 +299,7 @@
             action_timeout++;
         else
             set_program_info("RANDOM WALK");
-            internal_state = random_walk;
+        internal_state = random_walk;
     }
 }
 
@@ -306,7 +308,7 @@
 
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// straight_random_walk_with_interaction_program 
+/// straight_random_walk_with_interaction_program
 
 void straight_random_walk_with_interaction_program()
 {
@@ -318,9 +320,110 @@
 
 
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
-/// find_space_program 
+/// find_space_program
 
 void find_space_program()
 {
 
+}
+
+
+
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// clustering_program
+
+char prog_debug = 1;
+
+void clustering_program(char invert)
+{
+    out("Clustering program loop\n");
+    // The clustering program is a continuous turn-move vector program
+    // In normal mode (invert = 0) it is attracted to same-group robots and repels opposite-group, walls and very close same-group robots
+    // In invert mode (invert = 1) it avoids same-group and is attracted to opposite group
+
+    // Store the robot group: even robots (0) are green, odd robots (1) are red
+    char group = robot_id % 2;
+
+    if(program_run_init == 1) {
+        // Setup the LEDs based on robot_id
+        if(group == 0) {
+            set_leds(0xFF,0x00);
+            set_center_led(2,1);
+        } else {
+            set_leds(0x00,0xFF);
+            set_center_led(1,1);
+        }
+        program_run_init = 0;
+    }
+
+    // When step_cycle = 0 we calculate a vector to move to and a target distance
+    if(step_cycle == 0) {
+        struct FloatVector target_vector;
+        target_vector.angle = 0;
+        target_vector.distance = 0;
+        // Check for near robots within range
+        for(int i = 1; i < 8; i++) {
+            if(robots_found[i]) {
+                // Determine if the robot is an attractor or a repellor
+                char attract = 0;
+                if((invert==0 && ((i%2) == group)) || (invert==1 && ((i%2) != group))) attract = 1;
+                // Avoid very close attractors to stop collisions
+                if(attract==1 && robots_distance[i] > robot_avoidance_threshold) attract = 0;
+                int res_bearing = robots_heading[i];
+                if(attract==0){
+                    res_bearing += 180;
+                    if(res_bearing > 180) res_bearing -= 360;   
+                }
+                target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
+                if(prog_debug) {
+                    if(attract) {
+                        out("Attracted to robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
+                    } else {
+                        out("Repelled from robot %d at bearing %d, strength %d,  resultant b:%f, d:%f\n",i,robots_heading[i],robots_distance[i],target_vector.angle,target_vector.distance);
+                    }
+                }
+                
+            }
+        }
+        
+        // Check for obstacles
+        char obstacle = 0;
+        int peak_strength = 0;
+        for(int i=0; i<8; i++){
+            if(reflected_sensor_data[i] > peak_strength) peak_strength = reflected_sensor_data[i];
+            if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
+        }
+        if(obstacle){  
+           int obstacle_bearing = get_bearing_from_ir_array (reflected_sensor_data);
+           int obs_bearing = obstacle_bearing + 180;
+           if(obs_bearing > 180) obs_bearing -= 360;
+           target_vector = addVector(target_vector,obs_bearing,peak_strength);
+           if(prog_debug) out("Repelled from obstacle at bearing %d, strength %d,  resultant b:%f, d:%f\n",obstacle_bearing,peak_strength,target_vector.angle,target_vector.distance);
+        }   
+    }
+
+}
+
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// generic_program - Framework for building typical programs
+
+void generic_program()
+{
+    // Do something on the first run of a program
+    if(program_run_init == 1) {
+        // Initialisation code goes here...
+
+        program_run_init = 0;
+    }
+
+    // step_cycle is either zero or one; use this avoid estimating bearings on the cycle after a turn (as the results will be skewed by the turn)
+    if(step_cycle == 0) {
+        // Do something based on sensor data (eg turn)
+    } else {
+        // Do something ignoring sensor data (eg move, or nothing!)
+    }
+
 }
\ No newline at end of file