Statistics
| Revision:

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