![](/media/cache/img/default_profile.jpg.50x50_q85.jpg)
ft. button press reset
Fork of BeaconDemo_RobotCode by
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