Statistics
| Revision:

root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1592

History | View | Annotate | Download (6.18 KB)

1
#include <dragonfly_lib.h>
2
#include <wl_basic.h>
3
#include <encoders.h>
4
#include "circle.h"
5

    
6
int EDGE = 0;
7
int BEACON = 1;
8

    
9
/*
10
        This program is used to make robots target a center (leader) robot using the BOM,
11
        then drive toward it and stop at a certain distance away.
12
        
13
        The distance will eventually be adjustable.
14

15
        With adjustment, the leader robot will be able to turn and use its standardized
16
        rangefinder to reposition or space the robots evenly.
17
        
18
        AuTHORS: Nico, Alex, Reva, Echo, Steve
19
*/
20

    
21

    
22
/* 
23
TODO:
24
                Check BOM more often and before final stop
25
                Make BOM check more comprehensively
26
                Use the center bot to check distances
27
                Count them ("spam" method)
28
                Use beacon to find relative positions
29
                Beacon tells them how to move to be at the right distance
30
                *done*Wireless communication, initialization
31
*/
32

    
33
void forward(int speed){                        // set the motors to this forward speed.
34
        motor_l_set(FORWARD,speed);
35
        motor_r_set(FORWARD,speed);
36
}
37
void left(int speed){                                // turn left at this speed.
38
        motor_l_set(FORWARD,speed);
39
        motor_r_set(BACKWARD,speed);
40
}
41
void right(int speed){
42
        motor_l_set(BACKWARD,speed);
43
        motor_r_set(FORWARD,speed);
44
}
45
void stop(void){                                        // could be set to motors_off(), or just use this as an alternative.
46
        motor_l_set(BACKWARD,0);
47
        motor_r_set(FORWARD,0);
48
}
49
void setforward(int spd1, int spd2){
50
        motor_l_set(FORWARD,spd1);
51
        motor_r_set(FORWARD,spd2);
52
}
53
void backward(int speed){
54
        motor_l_set(BACKWARD, speed);
55
        motor_r_set(BACKWARD, speed);
56
}
57
int get_distance(void){                                // takes an averaged reading of the front rangefinder
58
        int temp,distance,kk=5;                        // kk sets this to 5 readings.
59
        distance =0;
60
        for (int i=0; i<kk; i++){
61
                temp = range_read_distance(IR2);
62
                if (temp == -1)
63
                {
64
                        //temp=0;
65
                        i--;
66
                }
67
                else
68
                        distance+= temp;
69
                delay_ms(3);
70
        }
71
        if (kk>0)
72
                return (int)(distance/kk);
73
        else 
74
                return 0;
75
}
76
void turn_to_beacon(int max){
77
        if (max>-1 && max<16){
78
                int index = (max+12)%16;
79
                if (index==0) { 
80
                        stop(); 
81
                }
82
                else if (index<8) right(170);
83
                else left(170);
84
        }
85
}
86

    
87
void turn_to_beacon2(int max){                                // like the previous but no stop() call'
88

    
89

    
90

    
91

    
92
        if (max>-1 && max<16){
93
                int index = (max+12)%16;
94
                if (index==0) { 
95
                         
96
                }
97
                else if (index<8) right(170);
98
                else left(170);
99
        }
100
}
101
void orient(void){
102
        int max_index = -1;
103
        while (max_index!=4) {
104
                /* Refresh and make sure the table is updated */
105
                bom_refresh(BOM_ALL);
106
                max_index = bom_get_max();
107
                turn_to_beacon(max_index);
108
                delay_ms(22);
109
        }
110
}
111
void orient2(void){
112
        int max_index = -1;
113
        while (max_index!=4) {
114
                /* Refresh and make sure the table is updated */
115
                bom_refresh(BOM_ALL);
116
                max_index = bom_get_max();
117
                turn_to_beacon2(max_index);
118
                delay_ms(22);
119
        }
120
}
121
void go_straight(void){                                                // drives forward a hardcoded distance. May not be useful.
122
        forward(200);
123
        encoder_rst_dx(LEFT);
124
        encoder_rst_dx(RIGHT);
125
        delay_ms(100); 
126
        int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
127
        int count = 0;
128
        int d;
129
        while (count<25){                                                //count = 25 when bot6; count <12
130
                x_left = encoder_get_x(LEFT);
131
                x_right = encoder_get_x(RIGHT);
132
                d = x_right-x_left;
133
                if (d>13 || d<-13){
134
                        if(d<50 && d>-50){
135
                                d = round(1.0*d/4);
136
                                setforward(200+d, 200-d);
137
                        }
138
                }
139
                delay_ms(32);
140
                count++;
141
        }
142
}
143
void go_straight_onefoot(void){                                // essentially, another name for the above.  Should be removed.
144
        go_straight();
145
}
146

    
147
void blink(int num) {
148
        for(int i = 0; i<num; i++)
149
        {
150
                orb_set_color(ORB_OFF);
151
                delay_ms(500);
152
                orb_set_color(RED);
153
                delay_ms(500);
154
        }
155
        orb_set_color(ORB_OFF);
156
}
157

    
158

    
159

    
160
int main(void)
161
{
162
        /* Initialize dragonfly board */
163
        dragonfly_init(ALL_ON);
164
        xbee_init();
165
        encoders_init();
166
        /* Initialize the basic wireless library */
167
        wl_basic_init_default();
168
        /* Set the XBee channel to your assigned channel */
169
        wl_set_channel(24);
170

    
171
        int robotid = get_robotid();
172
        int used[16];
173
        char send_buffer[2];
174
        int data_length;
175
        unsigned char *packet_data=wl_basic_do_default(&data_length);
176
        
177
        
178
        int state = EDGE;
179
        int beacon_State=0;
180
        int NUM_ROBOTS=2;
181
        int robotsRecieved=0;
182
        if(wheel()<100)
183
        {
184
                state=EDGE;
185
        }
186
        else
187
        {
188
                state=BEACON;
189
        }
190
        
191
        int distance=1000;                                                // how far away to stop.
192
        int onefoot=300, speed=200;                                // one foot is 490 for bot 1; one foot is 200 for bot6
193
        
194
        while(1) {
195
                switch(state) {
196
                        case 0:                        // EDGE
197
                                
198
                                orb_set_color(GREEN);
199
                                while(1)
200
                                {
201
                                        orb_set_color(YELLOW);
202
                                        packet_data=wl_basic_do_default(&data_length);
203
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST)
204
                                        {
205
                                                send_buffer[0]=CIRCLE_ACTION_ACK;
206
                                                send_buffer[1]=robotid;
207
                                                
208
                                                wl_basic_send_global_packet(42,send_buffer,2);
209
                                                break;
210
                                        }
211
                                }
212
                                
213
                                while(1){
214
                                        orb_set_color(RED);
215
                                        packet_data=wl_basic_do_default(&data_length);
216
                                        wl_basic_send_global_packet(42,send_buffer,2);
217

    
218
                                        if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE)
219
                                        {
220
                                                break;
221
                                        }
222
                                }
223
                                
224
                                orb_set_color(GREEN);
225
                                
226
                                orient();
227
                                forward(speed);
228
                                //range_init();
229
                                
230
                                orb_set_color(BLUE);
231
                                
232
                                distance = get_distance();
233
                                while (distance>=onefoot || distance==0)
234
                                {
235
                                        if(distance==0)
236
                                                orb_set_color(WHITE);
237
                                        distance = get_distance();
238
                                        orient2();
239
                                        forward(speed);
240
                                        delay_ms(14);
241
                                }
242
                                stop();
243
                                orient();
244
                                
245
                                //button1_wait ();                        // code for lab1.
246
                                //go_straight_onefoot();
247
                                stop();
248
                                break;
249
                                
250
                                
251
                        case 1:                        // BEACON
252
                                switch(beacon_State) {
253
                                case 0:
254
                                        bom_on();
255
                                        orb_set_color(PURPLE);
256
                                        if(button1_click()) beacon_State=1;
257
                                break;        
258
                                case 1:
259
                                        send_buffer[0]=CIRCLE_ACTION_EXIST;
260
                                        send_buffer[1]=get_robotid();
261
                                        wl_basic_send_global_packet(42,send_buffer,2);
262
                                        beacon_State=2;
263
                                break;
264
                                case 2: 
265
                                orb_set_color(ORANGE);
266
                                packet_data=wl_basic_do_default(&data_length);
267
                                if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
268
                                {
269
                                        orb_set_color(WHITE);
270
                                        if(used[packet_data[1]]==0){//only add to robots seen if you haven't gotten an ACK from this robot
271
                                                robotsRecieved++;
272
                                                used[packet_data[1]]=1;
273
                                        }
274
                                }
275
                                if(robotsRecieved==NUM_ROBOTS)
276
                                        beacon_State=3;
277
                                break;
278
                                case 3:
279
                                send_buffer[0]=CIRCLE_ACTION_DONE;
280
                                wl_basic_send_global_packet(42,send_buffer,2);
281
                                break;
282
                                }
283
                        break;
284
                }
285
        }
286

    
287
        orb_set_color(RED);
288
        while(1); /* END HERE */
289

    
290
        return 0;
291
}