Bluetooth communication for flocking.
Fork of BeautifulMemeProject by
vector.cpp@27:7eb032772bc2, 2016-01-31 (annotated)
- Committer:
- alanmillard
- Date:
- Sun Jan 31 15:14:54 2016 +0000
- Revision:
- 27:7eb032772bc2
- Parent:
- 12:daa53285b6e4
Flocking seems to work better now, however the robot controller sometimes "crashes" (unsure why).
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jah128 | 12:daa53285b6e4 | 1 | #include "main.h" |
jah128 | 12:daa53285b6e4 | 2 | #include <math.h> |
jah128 | 12:daa53285b6e4 | 3 | |
jah128 | 12:daa53285b6e4 | 4 | |
jah128 | 12:daa53285b6e4 | 5 | struct FloatVector addVector(struct FloatVector in_vector, int angle, int distance){ |
jah128 | 12:daa53285b6e4 | 6 | struct FloatVector out_vector; |
jah128 | 12:daa53285b6e4 | 7 | float sin_component_old = sin(in_vector.angle * TO_RAD) * in_vector.distance; |
jah128 | 12:daa53285b6e4 | 8 | float cos_component_old = cos(in_vector.angle * TO_RAD) * in_vector.distance; |
jah128 | 12:daa53285b6e4 | 9 | float sin_component_new = sin(angle * TO_RAD) * distance; |
jah128 | 12:daa53285b6e4 | 10 | float cos_component_new = cos(angle * TO_RAD) * distance; |
jah128 | 12:daa53285b6e4 | 11 | float sin_component_sum = sin_component_old + sin_component_new; |
jah128 | 12:daa53285b6e4 | 12 | float cos_component_sum = cos_component_old + cos_component_new; |
jah128 | 12:daa53285b6e4 | 13 | out_vector.distance = sqrt((sin_component_sum * sin_component_sum) + (cos_component_sum * cos_component_sum)); |
jah128 | 12:daa53285b6e4 | 14 | out_vector.angle = atan2(sin_component_sum ,cos_component_sum) * TO_DEG; |
jah128 | 12:daa53285b6e4 | 15 | //out("Angle:%f Distance:%f\n",out_vector.angle,out_vector.distance); |
jah128 | 12:daa53285b6e4 | 16 | return out_vector; |
jah128 | 12:daa53285b6e4 | 17 | } |