root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1571
History | View | Annotate | Download (1.71 KB)
| 1 |
#include <dragonfly_lib.h> |
|---|---|
| 2 |
#include <wl_basic.h> |
| 3 |
#include<encoders.h> |
| 4 |
|
| 5 |
|
| 6 |
int master = 0; |
| 7 |
int sending = 0; |
| 8 |
int stop = 0; |
| 9 |
|
| 10 |
|
| 11 |
void switch_sending ()
|
| 12 |
{
|
| 13 |
if(sending)
|
| 14 |
{
|
| 15 |
sending = 0;
|
| 16 |
bom_off(); |
| 17 |
} |
| 18 |
else
|
| 19 |
{
|
| 20 |
sending = 1;
|
| 21 |
bom_on(); |
| 22 |
} |
| 23 |
} |
| 24 |
|
| 25 |
|
| 26 |
|
| 27 |
int main(void) |
| 28 |
{
|
| 29 |
|
| 30 |
|
| 31 |
|
| 32 |
int sending_counter = 0; |
| 33 |
|
| 34 |
/* Initialize dragonfly board */
|
| 35 |
|
| 36 |
dragonfly_init(ALL_ON); |
| 37 |
xbee_init(); |
| 38 |
wl_basic_init_default(); |
| 39 |
wl_set_channel(12);
|
| 40 |
int data_length;
|
| 41 |
unsigned char *packet_data; |
| 42 |
char send_buffer[2]; |
| 43 |
|
| 44 |
|
| 45 |
if(master) orb1_set_color(RED);
|
| 46 |
else orb1_set_color(BLUE);
|
| 47 |
|
| 48 |
while(1) |
| 49 |
{
|
| 50 |
|
| 51 |
|
| 52 |
|
| 53 |
|
| 54 |
bom_refresh(BOM_ALL); |
| 55 |
|
| 56 |
|
| 57 |
|
| 58 |
if(master)
|
| 59 |
{
|
| 60 |
|
| 61 |
if(sending_counter++>2) |
| 62 |
{
|
| 63 |
switch_sending(); |
| 64 |
sending_counter = 0;
|
| 65 |
send_buffer[0] = 'a'; |
| 66 |
wl_basic_send_global_packet(42, send_buffer, 1); |
| 67 |
} |
| 68 |
|
| 69 |
|
| 70 |
if(sending)
|
| 71 |
{
|
| 72 |
|
| 73 |
|
| 74 |
|
| 75 |
|
| 76 |
|
| 77 |
} |
| 78 |
|
| 79 |
else // recieving |
| 80 |
{
|
| 81 |
if(bom_get_max()+1==5) |
| 82 |
{
|
| 83 |
send_buffer[0] = 's'; |
| 84 |
wl_basic_send_global_packet(42, send_buffer, 2); |
| 85 |
} |
| 86 |
} |
| 87 |
} |
| 88 |
|
| 89 |
|
| 90 |
|
| 91 |
else // slave |
| 92 |
{
|
| 93 |
if(packet_data[0]=='s') stop=1; |
| 94 |
if(packet_data[0]=='a') switch_sending(); |
| 95 |
|
| 96 |
if(sending)
|
| 97 |
{
|
| 98 |
/*
|
| 99 |
motor_l_set(FORWARD,0);
|
| 100 |
motor_r_set(FORWARD,0);
|
| 101 |
*/
|
| 102 |
} |
| 103 |
|
| 104 |
else // recieving |
| 105 |
{
|
| 106 |
|
| 107 |
if(stop)
|
| 108 |
{
|
| 109 |
motor_l_set(FORWARD,0);
|
| 110 |
motor_r_set(FORWARD,0);
|
| 111 |
orb1_set_color(GREEN); |
| 112 |
} |
| 113 |
|
| 114 |
else
|
| 115 |
{
|
| 116 |
|
| 117 |
|
| 118 |
if(bom_get_max()+1==8){ |
| 119 |
orb2_set_color(BLUE); |
| 120 |
motor_r_set(FORWARD,180);
|
| 121 |
motor_l_set(FORWARD,180);
|
| 122 |
|
| 123 |
} |
| 124 |
else {
|
| 125 |
orb2_set_color(GREEN); |
| 126 |
motor_l_set(FORWARD,0);
|
| 127 |
motor_r_set(FORWARD,180);
|
| 128 |
} |
| 129 |
} |
| 130 |
|
| 131 |
|
| 132 |
} |
| 133 |
|
| 134 |
|
| 135 |
} |
| 136 |
|
| 137 |
packet_data = wl_basic_do_default(&data_length); |
| 138 |
delay_ms(10);
|
| 139 |
} |
| 140 |
|
| 141 |
|
| 142 |
return 0; |
| 143 |
|
| 144 |
|
| 145 |
|
| 146 |
|
| 147 |
} |
| 148 |
|
| 149 |
|