Diff: programs.cpp

Revision:

13:f5994956b1ba

Parent:

12:daa53285b6e4

Child:

14:f623db1e6184

--- a/programs.cpp	Tue Oct 27 13:03:10 2015 +0000
+++ b/programs.cpp	Tue Oct 27 22:49:41 2015 +0000
@@ -322,9 +322,104 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// find_space_program
 
-void find_space_program()
+char prog_debug = 1;
+float target_wheel_speed;
+int random_walk_bearing;
+
+void find_space_program(char bidirectional)
 {
+    // The find_space_program is a continuous turn-move vector program
 
+    if(program_run_init == 1) {
+        // Setup the LEDs to red
+        set_leds(0x00,0xFF);
+        set_center_led(1,1);
+        program_run_init = 0;
+        random_walk_bearing = rand() % 360;
+    }
+
+    // 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]) {
+                int res_bearing = robots_heading[i];
+                res_bearing += 180;
+                if(res_bearing > 180) res_bearing -= 360;   
+                target_vector = addVector(target_vector,res_bearing,robots_distance[i]);
+                if(prog_debug) 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);
+            }
+        }
+        if(target_vector.angle!=0){
+           set_leds(0xFF,0xFF);
+           set_center_led(3,1);  
+        }
+        // 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){  
+           //Choose new random walk bearing
+           set_leds(0x00,0xFF);
+           set_center_led(1,1);
+           random_walk_bearing = rand() % 360;
+           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);
+        }   
+        
+        if(target_vector.angle == 0 && target_vector.distance == 0){
+            set_leds(0xFF,0x00);
+            set_center_led(2,1);
+            random_walk_bearing += 180;
+            if(random_walk_bearing > 360) random_walk_bearing -= 360;
+            target_vector.distance = 100;   
+            int current_bearing = 360 - beacon_heading;
+            //Now work out turn needed to face intended heading
+            int target_turn = (random_walk_bearing - current_bearing) % 360;
+            if(target_turn > 180) target_turn -= 360;
+            if(target_turn < -180) target_turn += 360;
+            target_vector.angle = target_turn;
+            if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
+        }
+        char wheel_direction = 1;
+        if(bidirectional){
+            // Allow reverse wheel direction
+            if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
+            else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
+        }
+        //Now turn to angle
+        float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
+        if(target_vector.angle < 0){
+            if(target_vector.angle < -maximum_turn_angle){
+                   target_vector.angle = -maximum_turn_angle;
+                   target_vector.distance = 100;
+            }    
+        }else{
+            if(target_vector.angle > maximum_turn_angle){
+                   target_vector.angle = maximum_turn_angle;
+                   target_vector.distance = 100;
+            } 
+        }
+        //Set the wheel speed for next action
+        if(target_vector.distance < 120) target_wheel_speed = 0.25;
+        else if(target_vector.distance < 240) target_wheel_speed = 0.35;
+        else if(target_vector.distance < 480) target_wheel_speed = 0.45;
+        else if(target_vector.distance < 960) target_wheel_speed = 0.55;
+        else target_wheel_speed = 0.65;
+        if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
+        
+        //Now turn...
+        time_based_turn_degrees(1, (int) target_vector.angle, 1);
+    } else time_based_forward(target_wheel_speed,BEACON_PERIOD*6,0);
 }
 
 
@@ -334,11 +429,9 @@
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// clustering_program
 
-char prog_debug = 1;
 
-void clustering_program(char invert)
+void clustering_program(char invert, char bidirectional)
 {
-    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
@@ -356,10 +449,12 @@
             set_center_led(1,1);
         }
         program_run_init = 0;
+        random_walk_bearing = rand() % 360;
     }
 
     // When step_cycle = 0 we calculate a vector to move to and a target distance
     if(step_cycle == 0) {
+        char avoiding_friend = 0;
         struct FloatVector target_vector;
         target_vector.angle = 0;
         target_vector.distance = 0;
@@ -370,7 +465,7 @@
                 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;
+                if(attract==1 && robots_distance[i] > robot_avoidance_threshold) {attract = 0; avoiding_friend = 1;}
                 int res_bearing = robots_heading[i];
                 if(attract==0){
                     res_bearing += 180;
@@ -384,7 +479,6 @@
                         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);
                     }
                 }
-                
             }
         }
         
@@ -396,17 +490,77 @@
             if(peak_strength > obstacle_avoidance_threshold) obstacle = 1;
         }
         if(obstacle){  
+           //Choose new random walk bearing
+           random_walk_bearing = rand() % 360;
            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);
         }   
-    }
+        if(target_vector.angle == 0 && target_vector.distance == 0){
+            //I have nothing attracting me so persist with random walk: with a 2% chance pick new bearing
+            if(rand() % 100 > 97) random_walk_bearing = rand() % 360;
+            target_vector.distance = 100;   
+            int current_bearing = 360 - beacon_heading;
+            //Now work out turn needed to face intended heading
+            int target_turn = (random_walk_bearing - current_bearing) % 360;
+            if(target_turn > 180) target_turn -= 360;
+            if(target_turn < -180) target_turn += 360;
+            target_vector.angle = target_turn;
+            if(prog_debug) out("Random walk bearing:%d Current:%d Target:%f\n",random_walk_bearing,current_bearing,target_vector.angle);
+        }
+        char wheel_direction = 1;
+        if(bidirectional){
+            // Allow reverse wheel direction
+            if(target_vector.angle < -90) {target_vector.angle += 180; wheel_direction = 0;}
+            else if(target_vector.angle > 90) {target_vector.angle -= 180; wheel_direction = 0;}
+        }
+        //Now turn to angle
+        float maximum_turn_angle = get_maximum_turn_angle(BEACON_PERIOD*10);
+        if(target_vector.angle < 0){
+            if(target_vector.angle < -maximum_turn_angle){
+                   target_vector.angle = -maximum_turn_angle;
+                   target_vector.distance = 100;
+            }    
+        }else{
+            if(target_vector.angle > maximum_turn_angle){
+                   target_vector.angle = maximum_turn_angle;
+                   target_vector.distance = 100;
+            } 
+        }
+        if(avoiding_friend) target_vector.distance = 100;
+        //Set the wheel speed for next action
+        if(target_vector.distance < 120) target_wheel_speed = 0.25;
+        else if(target_vector.distance < 240) target_wheel_speed = 0.35;
+        else if(target_vector.distance < 480) target_wheel_speed = 0.5;
+        else if(target_vector.distance < 960) target_wheel_speed = 0.65;
+        else target_wheel_speed = 0.85;
+        if(wheel_direction == 0) target_wheel_speed = 0-target_wheel_speed;
+        
+        //Now turn...
+        time_based_turn_degrees(1, (int) target_vector.angle, 1);
+    } else time_based_forward(target_wheel_speed,BEACON_PERIOD*7,0);
+}
 
+
+/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
+/// stop_program - Pauses robot
+
+void stop_program()
+{
+    if(program_run_init == 1) {
+        save_led_states();
+        set_leds(0,0);
+        set_center_led(0,0);
+        stop();
+        program_run_init = 0;
+    }
 }
 
 
+
+
 /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
 /// generic_program - Framework for building typical programs