Revision 1622
New version of cicle_spacing. (It works now...). Before merging with circle.
| trunk/code/behaviors/formation_control/circle_spacing/circle_spacing.c | ||
|---|---|---|
| 7 | 7 |
int sending = 0; |
| 8 | 8 |
int stop = 0; |
| 9 | 9 |
struct vector slave_position; |
| 10 |
int desired_max_bom; |
|
| 11 |
int bom_max_counter; |
|
| 10 | 12 |
|
| 13 |
|
|
| 14 |
void set_desired_max_bom(int desired_angle) |
|
| 15 |
{
|
|
| 16 |
if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0; |
|
| 17 |
if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1; |
|
| 18 |
if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2; |
|
| 19 |
if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3; |
|
| 20 |
if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4; |
|
| 21 |
if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5; |
|
| 22 |
if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6; |
|
| 23 |
if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7; |
|
| 24 |
if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8; |
|
| 25 |
if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9; |
|
| 26 |
if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10; |
|
| 27 |
if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11; |
|
| 28 |
if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12; |
|
| 29 |
if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13; |
|
| 30 |
if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14; |
|
| 31 |
if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
|
| 32 |
} |
|
| 33 |
|
|
| 34 |
|
|
| 35 |
|
|
| 36 |
|
|
| 37 |
|
|
| 38 |
|
|
| 11 | 39 |
void switch_sending () |
| 12 | 40 |
{
|
| 13 | 41 |
if(sending) |
| ... | ... | |
| 30 | 58 |
|
| 31 | 59 |
|
| 32 | 60 |
int sending_counter = 0; |
| 33 |
|
|
| 61 |
|
|
| 34 | 62 |
/* Initialize dragonfly board */ |
| 35 | 63 |
|
| 36 | 64 |
dragonfly_init(ALL_ON); |
| ... | ... | |
| 42 | 70 |
char send_buffer[2]; |
| 43 | 71 |
|
| 44 | 72 |
|
| 45 |
if(master) orb1_set_color(RED);
|
|
| 73 |
if(master) {orb1_set_color(RED); set_desired_max_bom(30);}
|
|
| 46 | 74 |
else orb1_set_color(BLUE); |
| 47 | 75 |
|
| 76 |
|
|
| 48 | 77 |
while(1) |
| 49 | 78 |
{
|
| 50 | 79 |
|
| ... | ... | |
| 58 | 87 |
if(master) |
| 59 | 88 |
{
|
| 60 | 89 |
|
| 61 |
if(sending_counter++>2)
|
|
| 90 |
if(sending_counter++>4)
|
|
| 62 | 91 |
{
|
| 63 | 92 |
switch_sending(); |
| 64 | 93 |
sending_counter = 0; |
| ... | ... | |
| 78 | 107 |
|
| 79 | 108 |
else // recieving |
| 80 | 109 |
{
|
| 110 |
int max_bom = bom_get_max(); |
|
| 111 |
usb_puts("bom_get_max : ");
|
|
| 112 |
usb_puti(max_bom); |
|
| 113 |
usb_puts("\n\r");
|
|
| 81 | 114 |
|
| 82 |
bom_get_vector(&slave_position, NULL); |
|
| 83 |
if(slave_position.y/slave_position.x > 10){
|
|
| 84 |
|
|
| 85 |
//if(bom_get_max()+1==5){
|
|
| 115 |
if(max_bom == desired_max_bom) |
|
| 116 |
{
|
|
| 117 |
if(bom_max_counter) // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise. |
|
| 118 |
{
|
|
| 119 |
send_buffer[0] = 's'; |
|
| 120 |
wl_basic_send_global_packet(42, send_buffer, 2); |
|
| 121 |
} |
|
| 122 |
bom_max_counter =1; |
|
| 123 |
} |
|
| 124 |
else bom_max_counter = 0; |
|
| 86 | 125 |
|
| 87 |
send_buffer[0] = 's'; |
|
| 88 |
wl_basic_send_global_packet(42, send_buffer, 2); |
|
| 89 |
} |
|
| 90 | 126 |
} |
| 91 | 127 |
} |
| 92 | 128 |
|
| ... | ... | |
| 99 | 135 |
|
| 100 | 136 |
if(sending) |
| 101 | 137 |
{
|
| 102 |
/* |
|
| 103 |
motor_l_set(FORWARD,0); |
|
| 104 |
motor_r_set(FORWARD,0); |
|
| 105 |
*/ |
|
| 138 |
|
|
| 106 | 139 |
} |
| 107 | 140 |
|
| 108 | 141 |
else // recieving |
| ... | ... | |
| 117 | 150 |
|
| 118 | 151 |
else |
| 119 | 152 |
{
|
| 153 |
int max_bom = bom_get_max(); |
|
| 154 |
usb_puts("bom_get_max : ");
|
|
| 155 |
usb_puti(max_bom); |
|
| 156 |
usb_puts("/n/r");
|
|
| 120 | 157 |
|
| 121 |
|
|
| 122 |
|
|
| 123 |
|
|
| 124 |
//bom_get_vector(&slave_position, NULL); |
|
| 125 |
//if(abs(slave_position.x)>2 && (double)slave_position.y/(double)slave_position.x < 0.1){
|
|
| 126 |
|
|
| 127 |
if(bom_get_max()+1==8){
|
|
| 158 |
|
|
| 159 |
if(max_bom == 8) |
|
| 160 |
{
|
|
| 128 | 161 |
orb2_set_color(BLUE); |
| 129 | 162 |
motor_r_set(FORWARD,180); |
| 130 | 163 |
motor_l_set(FORWARD,180); |
| 131 | 164 |
|
| 132 | 165 |
} |
| 133 |
else {
|
|
| 134 |
orb2_set_color(GREEN); |
|
| 166 |
else if(max_bom == 7 || max_bom == 6 || max_bom == 5) |
|
| 167 |
{
|
|
| 168 |
motor_l_set(FORWARD,180); |
|
| 169 |
motor_r_set(FORWARD,0); |
|
| 170 |
} |
|
| 171 |
else if(max_bom == -1); |
|
| 172 |
else |
|
| 173 |
{
|
|
| 174 |
orb2_set_color(GREEN); |
|
| 135 | 175 |
motor_l_set(FORWARD,0); |
| 136 | 176 |
motor_r_set(FORWARD,180); |
| 137 | 177 |
} |
| ... | ... | |
| 156 | 196 |
} |
| 157 | 197 |
|
| 158 | 198 |
|
| 199 |
|
|
| 200 |
|
|
| 201 |
|
|
| 202 |
|
|
Also available in: Unified diff