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