Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
Diff: programs.cpp
- Revision:
- 23:e59040ac05c4
- Parent:
- 22:fd75afdadfb2
- Child:
- 24:31707fec20a5
diff -r fd75afdadfb2 -r e59040ac05c4 programs.cpp --- a/programs.cpp Sat Jan 16 12:01:49 2016 +0000 +++ b/programs.cpp Wed Jan 20 14:55:38 2016 +0000 @@ -762,17 +762,31 @@ cohesion_vector.distance /= number_of_neighbours; // Normalise - float left_motor_speed = 0.5; - float right_motor_speed = 0.5; +// float left_motor_speed = 0.5; +// float right_motor_speed = 0.5; + + float left_motor_speed; + float right_motor_speed; + + int target_heading = 0; + int angles = 0; struct FloatVector obstacle_vector; obstacle_vector.angle = 0; obstacle_vector.distance = 0; + int sensors_activated = 0; + for(int i = 1; i < 8; i++) - obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); + { + if(reflected_sensor_data[i] > 0) + { + obstacle_vector = addVector(obstacle_vector, get_bearing_from_ir_array(reflected_sensor_data), reflected_sensor_data[i]); + sensors_activated++; + } + } - obstacle_vector.distance /= 8; // Normalise + obstacle_vector.distance /= sensors_activated; // Normalise char robot_nearby = 0; @@ -788,31 +802,110 @@ } } - if(obstacle_vector.distance > 150 && robot_nearby == 0) - { - if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5) - left_motor_speed *= -1; - else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5) - right_motor_speed *= -1; - } - else if(number_of_neighbours != 0) + if(obstacle_vector.distance > 150) { - average_heading /= number_of_flocking_headings; // Should this include self? - out("average heading: %d\n", average_heading); - - // Turn towards the average of the cohesion and alignment angles? - - if(beacon_heading < average_heading) - left_motor_speed = 0.25; - else if (beacon_heading > average_heading) - right_motor_speed = 0.25; + target_heading += obstacle_vector.angle + 180; + angles++; + } + + if(cohesion_vector.distance > 0) + { + target_heading += cohesion_vector.angle; + angles++; } - set_left_motor_speed(left_motor_speed); - set_right_motor_speed(right_motor_speed); +// if(obstacle_vector.distance > 150 && number_of_neighbours == 0) +// { +// if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5) +// left_motor_speed *= -1; +// else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5) +// right_motor_speed *= -1; +// } +// else if(number_of_neighbours > 0) +// { + if(number_of_flocking_headings > 0) + { + average_heading /= number_of_flocking_headings; + + //out("beacon heading: %d\n", beacon_heading); +// out("average heading: %d\n", average_heading); +// out("difference: %d\n", beacon_heading - average_heading); + + + + int relative_flocking_heading = beacon_heading - average_heading; + + target_heading += relative_flocking_heading; + angles++; + +// if(relative_flocking_heading > 0) +// { +// right_motor_speed = -0.25; +// left_motor_speed = 0.25; +// } +// else if(relative_flocking_heading < 0) +// { +// right_motor_speed = 0.25; +// left_motor_speed = -0.25; +// } + +// if(relative_flocking_heading > 22.5) +// { +// right_motor_speed = -0.5; +// left_motor_speed = 0.5; +// } +// else if(relative_flocking_heading < -22.5) +// { +// right_motor_speed = 0.5; +// left_motor_speed = -0.5; +// } + } + + if(angles > 0) + target_heading /= angles; // Calculate average + + if(target_heading > 180) + target_heading -= 360; + if(target_heading < -180) + target_heading += 360; + + if(step_cycle == 0) + time_based_turn_degrees(1, target_heading, 1); + else + forward(0.25); + + +// if(cohesion_vector.angle > 22.5) +// right_motor_speed *= -1; +// else if(cohesion_vector.angle < -22.5) +// left_motor_speed *= -1; +// } - if(step_cycle == 0) - { +// if(obstacle_vector.distance > 150 && robot_nearby == 0) +// { +// if(obstacle_vector.angle > 0 && obstacle_vector.angle < 67.5) +// left_motor_speed *= -1; +// else if(obstacle_vector.angle < 0 && obstacle_vector.angle > -67.5) +// right_motor_speed *= -1; +// } +// else if(number_of_neighbours != 0) +// { +// average_heading /= number_of_flocking_headings; // Should this include self? +// out("average heading: %d\n", average_heading); +// +// // Turn towards the average of the cohesion and alignment angles? +// +// if(beacon_heading < average_heading) +// left_motor_speed = 0.25; +// else if (beacon_heading > average_heading) +// right_motor_speed = 0.25; +// } + +// set_left_motor_speed(left_motor_speed); +// set_right_motor_speed(right_motor_speed); + +// if(step_cycle == 0) +// { // Only transmit beacon heading if the beacon is visible if(beacon_found) { @@ -821,7 +914,7 @@ bt.putc(beacon_heading_byte); } - } +// } } /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////