Revision 1555
Circle Formation: added wireless and updated
| trunk/code/behaviors/formation_control/circle2/circle.c | ||
|---|---|---|
| 59 | 59 |
distance =0; |
| 60 | 60 |
for (int i=0; i<kk; i++){
|
| 61 | 61 |
temp = range_read_distance(IR2); |
| 62 |
if (temp == -1) {temp=0; kk--;}
|
|
| 62 |
if (temp == -1) |
|
| 63 |
{
|
|
| 64 |
temp=0; |
|
| 65 |
kk--; |
|
| 66 |
} |
|
| 63 | 67 |
distance+= temp; |
| 64 | 68 |
delay_ms(3); |
| 65 | 69 |
} |
| ... | ... | |
| 135 | 139 |
go_straight(); |
| 136 | 140 |
} |
| 137 | 141 |
|
| 142 |
void blink(int num) {
|
|
| 143 |
for(int i = 0; i<num; i++) |
|
| 144 |
{
|
|
| 145 |
orb_set_color(ORB_OFF); |
|
| 146 |
delay_ms(200); |
|
| 147 |
orb_set_color(RED); |
|
| 148 |
delay_ms(200); |
|
| 149 |
} |
|
| 150 |
orb_set_color(ORB_OFF); |
|
| 151 |
} |
|
| 138 | 152 |
|
| 139 | 153 |
|
| 154 |
|
|
| 140 | 155 |
int main(void) |
| 141 | 156 |
{
|
| 142 | 157 |
/* Initialize dragonfly board */ |
| ... | ... | |
| 193 | 208 |
|
| 194 | 209 |
orient(); |
| 195 | 210 |
forward(speed); |
| 196 |
range_init(); |
|
| 211 |
//range_init();
|
|
| 197 | 212 |
|
| 198 |
orb_set_color(YELLOW);
|
|
| 213 |
orb_set_color(BLUE);
|
|
| 199 | 214 |
|
| 200 | 215 |
distance = get_distance(); |
| 201 | 216 |
while (distance>=onefoot || distance==0) |
| ... | ... | |
| 238 | 253 |
send_buffer[0]=CIRCLE_ACTION_ACK; |
| 239 | 254 |
wl_basic_send_global_packet(42,send_buffer,2); |
| 240 | 255 |
|
| 256 |
blink(numrobots); |
|
| 241 | 257 |
|
| 258 |
|
|
| 242 | 259 |
break; |
| 243 | 260 |
} |
| 244 | 261 |
} |
Also available in: Unified diff