Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: programs.cpp
- Revision:
- 27:7eb032772bc2
- Parent:
- 26:3869ae5eddd7
diff -r 3869ae5eddd7 -r 7eb032772bc2 programs.cpp --- a/programs.cpp Mon Jan 25 16:36:54 2016 +0000 +++ b/programs.cpp Sun Jan 31 15:14:54 2016 +0000 @@ -728,7 +728,8 @@ /// flocking_program void flocking_program() -{ +{ + char display_line[16] = " "; int average_heading = 0; int number_of_neighbours = 0; int number_of_flocking_headings = 0; @@ -770,9 +771,11 @@ cohesion_vector.distance /= number_of_neighbours; // Normalise obstacle_vector.distance /= sensors_activated; // Normalise + int obstacle_avoidance_angle; + if(sensors_activated > 0) { - int obstacle_avoidance_angle = obstacle_vector.angle + 180; + obstacle_avoidance_angle = obstacle_vector.angle + 180; if(obstacle_avoidance_angle > 180) obstacle_avoidance_angle -= 360; @@ -782,11 +785,13 @@ target_heading += obstacle_avoidance_angle; angles++; } + + int cohesion_angle; // Don't bother performing cohesion if robots are already close enough - if(number_of_neighbours > 0 && cohesion_vector.distance < 400) - { - int cohesion_angle = cohesion_vector.angle; + if(number_of_neighbours > 0 && cohesion_vector.distance < 200) + { + cohesion_angle = cohesion_vector.angle; if(cohesion_angle > 180) cohesion_angle -= 360; @@ -797,11 +802,13 @@ angles++; } + int relative_flocking_heading; + if(number_of_flocking_headings > 0) { average_heading /= number_of_flocking_headings; - int relative_flocking_heading = beacon_heading - average_heading; + relative_flocking_heading = beacon_heading - average_heading; if(relative_flocking_heading > 180) relative_flocking_heading -= 360; @@ -828,12 +835,16 @@ // Only transmit beacon heading if the beacon is visible if(beacon_found) - { + { float degrees_per_value = 256.0f / 360.0f; char beacon_heading_byte = (beacon_heading + 180) * degrees_per_value; // Convert beacon heading from +/-180 degrees into a single byte value bt.putc(beacon_heading_byte); } + + sprintf(display_line, "%d %d %d %d", target_heading, obstacle_avoidance_angle, cohesion_angle, relative_flocking_heading); + + set_program_info(display_line); } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////