Statistics
| Revision:

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

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

    
143
void blink(int num) {
144
        for(int i = 0; i<num; i++)
145
        {
146
                orb_set_color(ORB_OFF);
147
                delay_ms(500);
148
                orb_set_color(RED);
149
                delay_ms(500);
150
        }
151
        orb_set_color(ORB_OFF);
152
}
153

    
154

    
155

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

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

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

    
279
        orb_set_color(RED);
280
        while(1); /* END HERE */
281

    
282
        return 0;
283
}