root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1592
History | View | Annotate | Download (6.18 KB)
1 | 1517 | azirbel | #include <dragonfly_lib.h> |
---|---|---|---|
2 | 1552 | azirbel | #include <wl_basic.h> |
3 | 1517 | azirbel | #include <encoders.h> |
4 | 1552 | azirbel | #include "circle.h" |
5 | 1517 | azirbel | |
6 | 1552 | azirbel | int EDGE = 0; |
7 | int BEACON = 1; |
||
8 | |||
9 | 1524 | azirbel | /*
|
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 | 1517 | azirbel | |
15 | 1524 | azirbel | 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 | 1568 | sdevince | AuTHORS: Nico, Alex, Reva, Echo, Steve
|
19 | 1524 | azirbel | */
|
20 | |||
21 | |||
22 | /*
|
||
23 | TODO:
|
||
24 | 1568 | sdevince | Check BOM more often and before final stop
|
25 | Make BOM check more comprehensively
|
||
26 | Use the center bot to check distances
|
||
27 | 1524 | azirbel | 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 | 1568 | sdevince | *done*Wireless communication, initialization
|
31 | 1524 | azirbel | */
|
32 | |||
33 | void forward(int speed){ // set the motors to this forward speed. |
||
34 | 1517 | azirbel | motor_l_set(FORWARD,speed); |
35 | motor_r_set(FORWARD,speed); |
||
36 | } |
||
37 | 1524 | azirbel | void left(int speed){ // turn left at this speed. |
38 | 1517 | azirbel | 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 | 1524 | azirbel | void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
46 | 1517 | azirbel | 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 | 1524 | azirbel | 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 | 1517 | azirbel | distance =0;
|
60 | for (int i=0; i<kk; i++){ |
||
61 | temp = range_read_distance(IR2); |
||
62 | 1555 | azirbel | if (temp == -1) |
63 | { |
||
64 | 1568 | sdevince | //temp=0;
|
65 | i--; |
||
66 | 1555 | azirbel | } |
67 | 1568 | sdevince | else
|
68 | distance+= temp; |
||
69 | 1517 | azirbel | 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 | 1592 | sdevince | void turn_to_beacon2(int max){ // like the previous but no stop() call' |
88 | |||
89 | |||
90 | |||
91 | |||
92 | 1517 | azirbel | 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 | 1524 | azirbel | void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
122 | 1517 | azirbel | 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 | 1524 | azirbel | while (count<25){ //count = 25 when bot6; count <12 |
130 | 1517 | azirbel | 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 | 1524 | azirbel | void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
144 | 1517 | azirbel | go_straight(); |
145 | } |
||
146 | |||
147 | 1555 | azirbel | void blink(int num) { |
148 | for(int i = 0; i<num; i++) |
||
149 | { |
||
150 | orb_set_color(ORB_OFF); |
||
151 | 1568 | sdevince | delay_ms(500);
|
152 | 1555 | azirbel | orb_set_color(RED); |
153 | 1568 | sdevince | delay_ms(500);
|
154 | 1555 | azirbel | } |
155 | orb_set_color(ORB_OFF); |
||
156 | } |
||
157 | 1552 | azirbel | |
158 | |||
159 | 1555 | azirbel | |
160 | 1517 | azirbel | int main(void) |
161 | { |
||
162 | /* Initialize dragonfly board */
|
||
163 | dragonfly_init(ALL_ON); |
||
164 | 1552 | azirbel | xbee_init(); |
165 | 1517 | azirbel | encoders_init(); |
166 | 1552 | azirbel | /* Initialize the basic wireless library */
|
167 | wl_basic_init_default(); |
||
168 | /* Set the XBee channel to your assigned channel */
|
||
169 | 1568 | sdevince | wl_set_channel(24);
|
170 | 1552 | azirbel | |
171 | int robotid = get_robotid();
|
||
172 | 1592 | sdevince | int used[16]; |
173 | 1552 | azirbel | char send_buffer[2]; |
174 | int data_length;
|
||
175 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
||
176 | 1517 | azirbel | |
177 | 1552 | azirbel | |
178 | int state = EDGE;
|
||
179 | 1568 | sdevince | int beacon_State=0; |
180 | int NUM_ROBOTS=2; |
||
181 | int robotsRecieved=0; |
||
182 | 1552 | azirbel | if(wheel()<100) |
183 | { |
||
184 | state=EDGE; |
||
185 | } |
||
186 | else
|
||
187 | { |
||
188 | state=BEACON; |
||
189 | } |
||
190 | |||
191 | 1524 | azirbel | 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 | 1517 | azirbel | |
194 | 1552 | azirbel | 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 | 1592 | sdevince | send_buffer[1]=robotid;
|
207 | 1552 | azirbel | |
208 | wl_basic_send_global_packet(42,send_buffer,2); |
||
209 | 1568 | sdevince | break;
|
210 | 1552 | azirbel | } |
211 | 1568 | sdevince | } |
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 | 1552 | azirbel | { |
220 | break;
|
||
221 | } |
||
222 | } |
||
223 | 1568 | sdevince | |
224 | 1552 | azirbel | orb_set_color(GREEN); |
225 | |||
226 | orient(); |
||
227 | forward(speed); |
||
228 | 1555 | azirbel | //range_init();
|
229 | 1552 | azirbel | |
230 | 1555 | azirbel | orb_set_color(BLUE); |
231 | 1552 | azirbel | |
232 | distance = get_distance(); |
||
233 | while (distance>=onefoot || distance==0) |
||
234 | { |
||
235 | 1568 | sdevince | if(distance==0) |
236 | orb_set_color(WHITE); |
||
237 | 1552 | azirbel | 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 | 1568 | sdevince | 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 | 1552 | azirbel | packet_data=wl_basic_do_default(&data_length); |
267 | 1568 | sdevince | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
268 | 1552 | azirbel | { |
269 | 1568 | sdevince | orb_set_color(WHITE); |
270 | 1592 | sdevince | 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 | 1552 | azirbel | } |
275 | 1568 | sdevince | if(robotsRecieved==NUM_ROBOTS)
|
276 | beacon_State=3;
|
||
277 | break;
|
||
278 | case 3: |
||
279 | send_buffer[0]=CIRCLE_ACTION_DONE;
|
||
280 | 1552 | azirbel | wl_basic_send_global_packet(42,send_buffer,2); |
281 | 1568 | sdevince | break;
|
282 | } |
||
283 | 1552 | azirbel | break;
|
284 | } |
||
285 | 1517 | azirbel | } |
286 | |||
287 | 1552 | azirbel | orb_set_color(RED); |
288 | 1517 | azirbel | while(1); /* END HERE */ |
289 | |||
290 | return 0; |
||
291 | } |