Statistics
| Revision:

root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1580

History | View | Annotate | Download (2 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
struct vector slave_position;
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
                        
82
                                bom_get_vector(&slave_position, NULL);
83
                                if(slave_position.y/slave_position.x > 10){
84
                        
85
                                //if(bom_get_max()+1==5){
86
                                
87
                                send_buffer[0] = 's';
88
                                wl_basic_send_global_packet(42, send_buffer, 2);
89
                                }
90
                        }
91
                }
92
                
93
                
94
                
95
                else // slave
96
                {
97
                        if(packet_data[0]=='s') stop=1;
98
                        if(packet_data[0]=='a') switch_sending();
99
                
100
                        if(sending)
101
                        {
102
                        /*
103
                                motor_l_set(FORWARD,0);
104
                                motor_r_set(FORWARD,0);
105
                        */
106
                        }
107
                        
108
                        else // recieving
109
                        {
110
                        
111
                                if(stop)
112
                                {
113
                                motor_l_set(FORWARD,0);
114
                                motor_r_set(FORWARD,0);
115
                                orb1_set_color(GREEN);
116
                                }
117
                                
118
                                else
119
                                {
120
                                
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){        
128
                                        orb2_set_color(BLUE);
129
                                        motor_r_set(FORWARD,180);
130
                                        motor_l_set(FORWARD,180);
131
                                        
132
                                   }
133
                                   else {
134
                                   orb2_set_color(GREEN);
135
                                        motor_l_set(FORWARD,0);
136
                                        motor_r_set(FORWARD,180);
137
                                        }
138
                                }
139
                        
140
                        
141
                        }
142
                
143
                
144
                }
145
                
146
                packet_data = wl_basic_do_default(&data_length);
147
                delay_ms(10);
148
        }
149
        
150
  
151
  return 0;
152
  
153
  
154
        
155
        
156
}
157

    
158