root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1555
History | View | Annotate | Download (5.62 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 | 1552 | azirbel | AuTHORS: Niko, Alex, Reva, Echo, Steve
|
19 | 1524 | azirbel | */
|
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 | Wireless communication, initialization
|
||
31 | */
|
||
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 | temp=0;
|
||
65 | kk--; |
||
66 | } |
||
67 | 1517 | azirbel | distance+= temp; |
68 | delay_ms(3);
|
||
69 | } |
||
70 | if (kk>0) |
||
71 | return (int)(distance/kk); |
||
72 | else
|
||
73 | return 0; |
||
74 | } |
||
75 | void turn_to_beacon(int max){ |
||
76 | if (max>-1 && max<16){ |
||
77 | int index = (max+12)%16; |
||
78 | if (index==0) { |
||
79 | stop(); |
||
80 | } |
||
81 | else if (index<8) right(170); |
||
82 | else left(170); |
||
83 | } |
||
84 | } |
||
85 | |||
86 | 1524 | azirbel | void turn_to_beacon2(int max){ // like the previous but no stop() call |
87 | 1517 | azirbel | if (max>-1 && max<16){ |
88 | int index = (max+12)%16; |
||
89 | if (index==0) { |
||
90 | |||
91 | } |
||
92 | else if (index<8) right(170); |
||
93 | else left(170); |
||
94 | } |
||
95 | } |
||
96 | void orient(void){ |
||
97 | int max_index = -1; |
||
98 | while (max_index!=4) { |
||
99 | /* Refresh and make sure the table is updated */
|
||
100 | bom_refresh(BOM_ALL); |
||
101 | max_index = bom_get_max(); |
||
102 | turn_to_beacon(max_index); |
||
103 | delay_ms(22);
|
||
104 | } |
||
105 | } |
||
106 | void orient2(void){ |
||
107 | int max_index = -1; |
||
108 | while (max_index!=4) { |
||
109 | /* Refresh and make sure the table is updated */
|
||
110 | bom_refresh(BOM_ALL); |
||
111 | max_index = bom_get_max(); |
||
112 | turn_to_beacon2(max_index); |
||
113 | delay_ms(22);
|
||
114 | } |
||
115 | } |
||
116 | 1524 | azirbel | void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
117 | 1517 | azirbel | forward(200);
|
118 | encoder_rst_dx(LEFT); |
||
119 | encoder_rst_dx(RIGHT); |
||
120 | delay_ms(100);
|
||
121 | int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
|
||
122 | int count = 0; |
||
123 | int d;
|
||
124 | 1524 | azirbel | while (count<25){ //count = 25 when bot6; count <12 |
125 | 1517 | azirbel | x_left = encoder_get_x(LEFT); |
126 | x_right = encoder_get_x(RIGHT); |
||
127 | d = x_right-x_left; |
||
128 | if (d>13 || d<-13){ |
||
129 | if(d<50 && d>-50){ |
||
130 | d = round(1.0*d/4); |
||
131 | setforward(200+d, 200-d); |
||
132 | } |
||
133 | } |
||
134 | delay_ms(32);
|
||
135 | count++; |
||
136 | } |
||
137 | } |
||
138 | 1524 | azirbel | void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
139 | 1517 | azirbel | go_straight(); |
140 | } |
||
141 | |||
142 | 1555 | azirbel | void blink(int num) { |
143 | for(int i = 0; i<num; i++) |
||
144 | { |
||
145 | orb_set_color(ORB_OFF); |
||
146 | delay_ms(200);
|
||
147 | orb_set_color(RED); |
||
148 | delay_ms(200);
|
||
149 | } |
||
150 | orb_set_color(ORB_OFF); |
||
151 | } |
||
152 | 1552 | azirbel | |
153 | |||
154 | 1555 | azirbel | |
155 | 1517 | azirbel | int main(void) |
156 | { |
||
157 | /* Initialize dragonfly board */
|
||
158 | dragonfly_init(ALL_ON); |
||
159 | 1552 | azirbel | xbee_init(); |
160 | 1517 | azirbel | encoders_init(); |
161 | 1552 | azirbel | /* Initialize the basic wireless library */
|
162 | wl_basic_init_default(); |
||
163 | /* Set the XBee channel to your assigned channel */
|
||
164 | wl_set_channel(12);
|
||
165 | |||
166 | int robotid = get_robotid();
|
||
167 | char send_buffer[2]; |
||
168 | int data_length;
|
||
169 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
||
170 | 1517 | azirbel | |
171 | 1552 | azirbel | |
172 | int state = EDGE;
|
||
173 | if(wheel()<100) |
||
174 | { |
||
175 | state=EDGE; |
||
176 | } |
||
177 | else
|
||
178 | { |
||
179 | state=BEACON; |
||
180 | } |
||
181 | |||
182 | 1524 | azirbel | int distance=1000; // how far away to stop. |
183 | int onefoot=300, speed=200; // one foot is 490 for bot 1; one foot is 200 for bot6 |
||
184 | 1517 | azirbel | |
185 | 1552 | azirbel | while(1) { |
186 | switch(state) {
|
||
187 | case 0: // EDGE |
||
188 | |||
189 | orb_set_color(GREEN); |
||
190 | while(1) |
||
191 | { |
||
192 | orb_set_color(YELLOW); |
||
193 | packet_data=wl_basic_do_default(&data_length); |
||
194 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
||
195 | { |
||
196 | send_buffer[0]=CIRCLE_ACTION_ACK;
|
||
197 | send_buffer[1]=get_robotid();
|
||
198 | |||
199 | wl_basic_send_global_packet(42,send_buffer,2); |
||
200 | } |
||
201 | |||
202 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
||
203 | { |
||
204 | break;
|
||
205 | } |
||
206 | } |
||
207 | orb_set_color(GREEN); |
||
208 | |||
209 | orient(); |
||
210 | forward(speed); |
||
211 | 1555 | azirbel | //range_init();
|
212 | 1552 | azirbel | |
213 | 1555 | azirbel | orb_set_color(BLUE); |
214 | 1552 | azirbel | |
215 | distance = get_distance(); |
||
216 | while (distance>=onefoot || distance==0) |
||
217 | { |
||
218 | distance = get_distance(); |
||
219 | orient2(); |
||
220 | forward(speed); |
||
221 | delay_ms(14);
|
||
222 | } |
||
223 | stop(); |
||
224 | orient(); |
||
225 | |||
226 | //button1_wait (); // code for lab1.
|
||
227 | //go_straight_onefoot();
|
||
228 | stop(); |
||
229 | break;
|
||
230 | |||
231 | |||
232 | case 1: // BEACON |
||
233 | bom_on(); |
||
234 | orb_set_color(PURPLE); |
||
235 | |||
236 | int numrobots = 0; |
||
237 | |||
238 | while(!button1_click())
|
||
239 | { } |
||
240 | |||
241 | send_buffer[0]=CIRCLE_ACTION_EXIST;
|
||
242 | send_buffer[1]=get_robotid();
|
||
243 | |||
244 | wl_basic_send_global_packet(42,send_buffer,2); |
||
245 | |||
246 | packet_data=wl_basic_do_default(&data_length); |
||
247 | while(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
||
248 | { |
||
249 | // IF NEEDED: a good place to collect and store the robot id.
|
||
250 | numrobots++; |
||
251 | } |
||
252 | |||
253 | send_buffer[0]=CIRCLE_ACTION_ACK;
|
||
254 | wl_basic_send_global_packet(42,send_buffer,2); |
||
255 | |||
256 | 1555 | azirbel | blink(numrobots); |
257 | 1552 | azirbel | |
258 | 1555 | azirbel | |
259 | 1552 | azirbel | break;
|
260 | } |
||
261 | 1517 | azirbel | } |
262 | |||
263 | 1552 | azirbel | orb_set_color(RED); |
264 | 1517 | azirbel | while(1); /* END HERE */ |
265 | |||
266 | return 0; |
||
267 | } |