root / trunk / code / behaviors / formation_control / circle / circle.c @ 1618
History | View | Annotate | Download (7.97 KB)
| 1 | 1594 | azirbel | #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 | 1618 | jmcarrol | int timeout = 0; |
| 9 | 1594 | azirbel | |
| 10 | 1618 | jmcarrol | |
| 11 | 1594 | azirbel | /*
|
| 12 | This program is used to make robots target a center (leader) robot using the BOM,
|
||
| 13 | then drive toward it and stop at a certain distance away.
|
||
| 14 | |||
| 15 | The distance will eventually be adjustable.
|
||
| 16 | |||
| 17 | With adjustment, the leader robot will be able to turn and use its standardized
|
||
| 18 | rangefinder to reposition or space the robots evenly.
|
||
| 19 | |||
| 20 | AuTHORS: Nico, Alex, Reva, Echo, Steve
|
||
| 21 | */
|
||
| 22 | |||
| 23 | |||
| 24 | /*
|
||
| 25 | TODO:
|
||
| 26 | 1610 | azirbel | Used: Bots 1, 7
|
| 27 | 16 Performed Badly
|
||
| 28 | 12 worked ok as beacon, not well as edge
|
||
| 29 | |||
| 30 | Fix orient code so the bot does not toggle back and forth when it tries to turn
|
||
| 31 | |||
| 32 | 1594 | azirbel | Use the center bot to check distances
|
| 33 | 1610 | azirbel | Done--> Count them ("spam" method)
|
| 34 | 1594 | azirbel | Use beacon to find relative positions
|
| 35 | Beacon tells them how to move to be at the right distance
|
||
| 36 | *done*Wireless communication, initialization
|
||
| 37 | */
|
||
| 38 | |||
| 39 | 1618 | jmcarrol | |
| 40 | |||
| 41 | 1594 | azirbel | void forward(int speed){ // set the motors to this forward speed. |
| 42 | motor_l_set(FORWARD,speed); |
||
| 43 | motor_r_set(FORWARD,speed); |
||
| 44 | } |
||
| 45 | void left(int speed){ // turn left at this speed. |
||
| 46 | motor_l_set(FORWARD,speed); |
||
| 47 | motor_r_set(BACKWARD,speed); |
||
| 48 | } |
||
| 49 | void right(int speed){ |
||
| 50 | motor_l_set(BACKWARD,speed); |
||
| 51 | motor_r_set(FORWARD,speed); |
||
| 52 | } |
||
| 53 | void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
||
| 54 | motor_l_set(BACKWARD,0);
|
||
| 55 | motor_r_set(FORWARD,0);
|
||
| 56 | } |
||
| 57 | void setforward(int spd1, int spd2){ |
||
| 58 | motor_l_set(FORWARD,spd1); |
||
| 59 | motor_r_set(FORWARD,spd2); |
||
| 60 | } |
||
| 61 | void backward(int speed){ |
||
| 62 | motor_l_set(BACKWARD, speed); |
||
| 63 | motor_r_set(BACKWARD, speed); |
||
| 64 | } |
||
| 65 | int get_distance(void){ // takes an averaged reading of the front rangefinder |
||
| 66 | int temp,distance,kk=5; // kk sets this to 5 readings. |
||
| 67 | distance =0;
|
||
| 68 | for (int i=0; i<kk; i++){ |
||
| 69 | temp = range_read_distance(IR2); |
||
| 70 | if (temp == -1) |
||
| 71 | {
|
||
| 72 | //temp=0;
|
||
| 73 | i--; |
||
| 74 | } |
||
| 75 | else
|
||
| 76 | distance+= temp; |
||
| 77 | delay_ms(3);
|
||
| 78 | } |
||
| 79 | if (kk>0) |
||
| 80 | return (int)(distance/kk); |
||
| 81 | else
|
||
| 82 | return 0; |
||
| 83 | } |
||
| 84 | 1618 | jmcarrol | |
| 85 | |||
| 86 | int correctTurn(void) |
||
| 87 | {
|
||
| 88 | int bomNum = 0; |
||
| 89 | bom_refresh(BOM_ALL); |
||
| 90 | bomNum = bom_get_max(); |
||
| 91 | usb_puti(bomNum); |
||
| 92 | if(bomNum == 4) |
||
| 93 | { timeout = 0;
|
||
| 94 | motor_l_set(1, 200); |
||
| 95 | motor_r_set(1, 200); |
||
| 96 | return 0; |
||
| 97 | } |
||
| 98 | else
|
||
| 99 | {
|
||
| 100 | if(bomNum == -1){ |
||
| 101 | timeout++; |
||
| 102 | if(timeout > 500000) |
||
| 103 | {
|
||
| 104 | motor_r_set(FORWARD, 210);
|
||
| 105 | motor_l_set(BACKWARD, 190);
|
||
| 106 | } |
||
| 107 | } |
||
| 108 | else if((bomNum >= 12) || (bomNum < 4)) |
||
| 109 | {
|
||
| 110 | motor_l_set(FORWARD, 200);timeout = 0; |
||
| 111 | motor_r_set(BACKWARD, 200);
|
||
| 112 | } |
||
| 113 | else
|
||
| 114 | {
|
||
| 115 | motor_l_set(BACKWARD, 200);timeout = 0; |
||
| 116 | motor_r_set(FORWARD, 200);
|
||
| 117 | } |
||
| 118 | } |
||
| 119 | return 1; |
||
| 120 | } |
||
| 121 | |||
| 122 | void correctApproach(void) |
||
| 123 | {
|
||
| 124 | int bomNum = 0; |
||
| 125 | bom_refresh(BOM_ALL); |
||
| 126 | bomNum = bom_get_max(); |
||
| 127 | usb_puti(bomNum); |
||
| 128 | if(bomNum == 4) |
||
| 129 | {
|
||
| 130 | motor_l_set(1, 200); |
||
| 131 | motor_r_set(1, 200); |
||
| 132 | } |
||
| 133 | else
|
||
| 134 | {
|
||
| 135 | if(bomNum == -1){} |
||
| 136 | else if((bomNum >= 12) || (bomNum < 4)) |
||
| 137 | {
|
||
| 138 | motor_l_set(FORWARD, 200);
|
||
| 139 | motor_r_set(BACKWARD, 200);
|
||
| 140 | } |
||
| 141 | else
|
||
| 142 | {
|
||
| 143 | motor_l_set(BACKWARD, 200);
|
||
| 144 | motor_r_set(FORWARD, 200);
|
||
| 145 | } |
||
| 146 | delay_ms(100);
|
||
| 147 | } |
||
| 148 | } |
||
| 149 | |||
| 150 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||
| 151 | 1594 | azirbel | void turn_to_beacon(int max){ |
| 152 | if (max>-1 && max<16){ |
||
| 153 | int index = (max+12)%16; |
||
| 154 | if (index==0) { |
||
| 155 | stop(); |
||
| 156 | } |
||
| 157 | else if (index<8) right(170); |
||
| 158 | else left(170); |
||
| 159 | } |
||
| 160 | } |
||
| 161 | |||
| 162 | void turn_to_beacon2(int max){ // like the previous but no stop() call' |
||
| 163 | |||
| 164 | |||
| 165 | |||
| 166 | |||
| 167 | if (max>-1 && max<16){ |
||
| 168 | int index = (max+12)%16; |
||
| 169 | if (index==0) { |
||
| 170 | |||
| 171 | } |
||
| 172 | else if (index<8) right(170); |
||
| 173 | else left(170); |
||
| 174 | } |
||
| 175 | } |
||
| 176 | 1618 | jmcarrol | |
| 177 | 1594 | azirbel | void orient(void){ |
| 178 | int max_index = -1; |
||
| 179 | while (max_index!=4) { |
||
| 180 | /* Refresh and make sure the table is updated */
|
||
| 181 | bom_refresh(BOM_ALL); |
||
| 182 | max_index = bom_get_max(); |
||
| 183 | turn_to_beacon(max_index); |
||
| 184 | delay_ms(22);
|
||
| 185 | } |
||
| 186 | } |
||
| 187 | void orient2(void){ |
||
| 188 | int max_index = -1; |
||
| 189 | while (max_index!=4) { |
||
| 190 | /* Refresh and make sure the table is updated */
|
||
| 191 | bom_refresh(BOM_ALL); |
||
| 192 | max_index = bom_get_max(); |
||
| 193 | turn_to_beacon2(max_index); |
||
| 194 | delay_ms(22);
|
||
| 195 | } |
||
| 196 | } |
||
| 197 | 1618 | jmcarrol | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
| 198 | 1594 | azirbel | void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
| 199 | forward(200);
|
||
| 200 | encoder_rst_dx(LEFT); |
||
| 201 | encoder_rst_dx(RIGHT); |
||
| 202 | delay_ms(100);
|
||
| 203 | int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT);
|
||
| 204 | int count = 0; |
||
| 205 | int d;
|
||
| 206 | while (count<25){ //count = 25 when bot6; count <12 |
||
| 207 | x_left = encoder_get_x(LEFT); |
||
| 208 | x_right = encoder_get_x(RIGHT); |
||
| 209 | d = x_right-x_left; |
||
| 210 | if (d>13 || d<-13){ |
||
| 211 | if(d<50 && d>-50){ |
||
| 212 | d = round(1.0*d/4); |
||
| 213 | setforward(200+d, 200-d); |
||
| 214 | } |
||
| 215 | } |
||
| 216 | delay_ms(32);
|
||
| 217 | count++; |
||
| 218 | } |
||
| 219 | } |
||
| 220 | void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
||
| 221 | go_straight(); |
||
| 222 | } |
||
| 223 | |||
| 224 | void blink(int num) { |
||
| 225 | for(int i = 0; i<num; i++) |
||
| 226 | {
|
||
| 227 | orb_set_color(ORB_OFF); |
||
| 228 | 1610 | azirbel | delay_ms(200);
|
| 229 | 1594 | azirbel | orb_set_color(RED); |
| 230 | 1610 | azirbel | delay_ms(200);
|
| 231 | 1594 | azirbel | } |
| 232 | orb_set_color(ORB_OFF); |
||
| 233 | } |
||
| 234 | |||
| 235 | |||
| 236 | |||
| 237 | int main(void) |
||
| 238 | {
|
||
| 239 | /* Initialize dragonfly board */
|
||
| 240 | 1618 | jmcarrol | dragonfly_init(ALL_ON); |
| 241 | /* Initialize the basic wireless library */
|
||
| 242 | wl_basic_init_default(); |
||
| 243 | /* Set the XBee channel to your assigned channel */ /* Set the XBee channel to your assigned channel */ |
||
| 244 | 1594 | azirbel | wl_set_channel(24);
|
| 245 | |||
| 246 | int robotid = get_robotid();
|
||
| 247 | int used[16]; |
||
| 248 | 1597 | azirbel | for (int i=0; i<16; i++) used[i] = 0; |
| 249 | 1594 | azirbel | char send_buffer[2]; |
| 250 | int data_length;
|
||
| 251 | unsigned char *packet_data=wl_basic_do_default(&data_length); |
||
| 252 | |||
| 253 | |||
| 254 | int state = EDGE;
|
||
| 255 | int beacon_State=0; |
||
| 256 | 1610 | azirbel | int waitingCounter=0; |
| 257 | int robotsReceived=0; |
||
| 258 | 1594 | azirbel | if(wheel()<100) |
| 259 | {
|
||
| 260 | state=EDGE; |
||
| 261 | } |
||
| 262 | else
|
||
| 263 | {
|
||
| 264 | state=BEACON; |
||
| 265 | } |
||
| 266 | |||
| 267 | int distance=1000; // how far away to stop. |
||
| 268 | 1610 | azirbel | int onefoot=300, speed=250; // one foot is 490 for bot 1; one foot is 200 for bot6 |
| 269 | 1594 | azirbel | |
| 270 | 1596 | azirbel | while(1) |
| 271 | {
|
||
| 272 | switch(state)
|
||
| 273 | {
|
||
| 274 | 1594 | azirbel | case 0: // EDGE |
| 275 | |||
| 276 | 1618 | jmcarrol | bom_off(); |
| 277 | 1594 | azirbel | while(1) |
| 278 | {
|
||
| 279 | 1618 | jmcarrol | orb1_set_color(YELLOW);orb2_set_color(CYAN); |
| 280 | 1594 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 281 | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
||
| 282 | {
|
||
| 283 | send_buffer[0]=CIRCLE_ACTION_ACK;
|
||
| 284 | send_buffer[1]=robotid;
|
||
| 285 | |||
| 286 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 287 | break;
|
||
| 288 | } |
||
| 289 | } |
||
| 290 | |||
| 291 | 1596 | azirbel | while(1) // wait for the center bot to count all the robots and send a "done" packet |
| 292 | {
|
||
| 293 | 1618 | jmcarrol | orb_set_color(YELLOW);orb2_set_color(PURPLE); |
| 294 | 1594 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 295 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 296 | 1596 | azirbel | |
| 297 | 1594 | azirbel | if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) |
| 298 | {
|
||
| 299 | break;
|
||
| 300 | } |
||
| 301 | } |
||
| 302 | |||
| 303 | 1618 | jmcarrol | orb_set_color(MAGENTA); |
| 304 | //~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
|
||
| 305 | while(correctTurn()){
|
||
| 306 | } |
||
| 307 | 1594 | azirbel | //range_init();
|
| 308 | |||
| 309 | |||
| 310 | distance = get_distance(); |
||
| 311 | while (distance>=onefoot || distance==0) |
||
| 312 | {
|
||
| 313 | if(distance==0) |
||
| 314 | orb_set_color(WHITE); |
||
| 315 | 1618 | jmcarrol | //correctApproach();
|
| 316 | 1594 | azirbel | distance = get_distance(); |
| 317 | delay_ms(14);
|
||
| 318 | } |
||
| 319 | 1618 | jmcarrol | |
| 320 | 1594 | azirbel | stop(); |
| 321 | 1618 | jmcarrol | orb_set_color(LIME); |
| 322 | 1594 | azirbel | //button1_wait (); // code for lab1.
|
| 323 | //go_straight_onefoot();
|
||
| 324 | break;
|
||
| 325 | |||
| 326 | |||
| 327 | 1596 | azirbel | |
| 328 | |||
| 329 | 1594 | azirbel | case 1: // BEACON |
| 330 | switch(beacon_State) {
|
||
| 331 | 1596 | azirbel | case 0: // wait for a button to be pressed, then go to state 1 |
| 332 | 1594 | azirbel | bom_on(); |
| 333 | orb_set_color(PURPLE); |
||
| 334 | if(button1_click()) beacon_State=1; |
||
| 335 | break;
|
||
| 336 | 1596 | azirbel | case 1: // sends a global exist packet to see how many robots there are |
| 337 | 1618 | jmcarrol | orb_set_color(RED); |
| 338 | 1594 | azirbel | send_buffer[0]=CIRCLE_ACTION_EXIST;
|
| 339 | send_buffer[1]=get_robotid();
|
||
| 340 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 341 | beacon_State=2;
|
||
| 342 | break;
|
||
| 343 | 1596 | azirbel | case 2: // find out how many robots there are |
| 344 | 1610 | azirbel | waitingCounter++; |
| 345 | 1618 | jmcarrol | orb1_set_color(YELLOW); |
| 346 | orb2_set_color(BLUE); |
||
| 347 | 1594 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 348 | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
||
| 349 | {
|
||
| 350 | 1618 | jmcarrol | orb_set_color(RED);orb2_set_color(BLUE); |
| 351 | 1596 | azirbel | if(used[packet_data[1]]==0){ //only add to robots seen if you haven't gotten an ACK from this robot |
| 352 | 1610 | azirbel | robotsReceived++; |
| 353 | 1594 | azirbel | used[packet_data[1]]=1; |
| 354 | } |
||
| 355 | } |
||
| 356 | 1610 | azirbel | if(waitingCounter >= 1500){ |
| 357 | 1594 | azirbel | beacon_State=3;
|
| 358 | 1610 | azirbel | //blink(robotsReceived);
|
| 359 | } |
||
| 360 | 1594 | azirbel | break;
|
| 361 | case 3: |
||
| 362 | 1618 | jmcarrol | orb_set_color(GREEN); |
| 363 | 1594 | azirbel | send_buffer[0]=CIRCLE_ACTION_DONE;
|
| 364 | wl_basic_send_global_packet(42,send_buffer,2); |
||
| 365 | break;
|
||
| 366 | } |
||
| 367 | break;
|
||
| 368 | } |
||
| 369 | } |
||
| 370 | |||
| 371 | orb_set_color(RED); |
||
| 372 | while(1); /* END HERE */ |
||
| 373 | |||
| 374 | return 0; |
||
| 375 | } |