Statistics
| Revision:

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

History | View | Annotate | Download (3.6 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
int desired_max_bom;
11
int bom_max_counter;
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

    
39
void switch_sending ()
40
{
41
        if(sending)
42
        {
43
                sending = 0;
44
                bom_off();
45
        }
46
        else
47
        {
48
                sending = 1;
49
                bom_on();
50
        }
51
}
52

    
53

    
54

    
55
int main(void)
56
{
57

    
58
        
59
        
60
        int sending_counter = 0;
61
        
62
    /* Initialize dragonfly board */
63
        
64
    dragonfly_init(ALL_ON);
65
        xbee_init();
66
        wl_basic_init_default();
67
        wl_set_channel(12);
68
    int data_length;
69
        unsigned char *packet_data;
70
        char send_buffer[2];
71
        
72
        
73
        if(master) {orb1_set_color(RED); set_desired_max_bom(30);}
74
        else orb1_set_color(BLUE);
75
        
76
        
77
        while(1)
78
        {
79
        
80
        
81
                
82
        
83
                bom_refresh(BOM_ALL);
84
                
85
                
86
                
87
                if(master)
88
                {
89
                
90
                        if(sending_counter++>4)        // clock for switching the BOMs between the master and slave
91
                        {
92
                                switch_sending();
93
                                sending_counter = 0;
94
                                send_buffer[0] = 'a';
95
                                wl_basic_send_global_packet(42, send_buffer, 1);
96
                        }
97
                
98
                        
99
                        if(sending)
100
                        {
101
                
102
                                
103
                        
104
                        
105

    
106
                        }
107
                        
108
                        else // recieving
109
                        {
110
                                int max_bom = bom_get_max();
111
                                        usb_puts("bom_get_max : ");
112
                                        usb_puti(max_bom);
113
                                        usb_puts("\n\r");
114
                        
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;
125
                                
126
                        }
127
                }
128
                
129
                
130
                
131
                else // slave
132
                {
133
                        if(packet_data[0]=='s') stop=1;
134
                        if(packet_data[0]=='a') switch_sending();
135
                
136
                        if(sending)
137
                        {
138
                        
139
                        }
140
                        
141
                        else // recieving
142
                        {
143
                        
144
                                if(stop)
145
                                {
146
                                motor_l_set(FORWARD,0);
147
                                motor_r_set(FORWARD,0);
148
                                orb1_set_color(GREEN);
149
                                }
150
                                
151
                                else
152
                                {
153
                                        int max_bom = bom_get_max();
154
                                        /*usb_puts("bom_get_max : ");
155
                                        usb_puti(max_bom);
156
                                        usb_puts("/n/r");*/
157
                                
158
                                
159
                                   if(max_bom == 8)
160
                                   {        
161
                                        orb2_set_color(BLUE);
162
                                        motor_r_set(FORWARD,180);
163
                                        motor_l_set(FORWARD,180);
164
                                        
165
                                   }
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);
175
                                        motor_l_set(FORWARD,0);
176
                                        motor_r_set(FORWARD,180);
177
                                        }
178
                                }
179
                        
180
                        
181
                        }
182
                
183
                
184
                }
185
                
186
                packet_data = wl_basic_do_default(&data_length);
187
                delay_ms(10);
188
        }
189
        
190
  
191
  return 0;
192
  
193
  
194
        
195
        
196
}
197

    
198

    
199

    
200

    
201

    
202