Statistics
| Revision:

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

 1 ```#include ``` ```#include ``` ```#include ``` ```#include "circle.h" ``` ```int EDGE = 0; ``` ```int BEACON = 1; ``` ```/* ``` ``` This program is used to make robots target a center (leader) robot using the BOM, ``` ``` then drive toward it and stop at a certain distance away. ``` ``` ``` ``` The distance will eventually be adjustable. ``` ``` ``` ``` With adjustment, the leader robot will be able to turn and use its standardized ``` ``` rangefinder to reposition or space the robots evenly. ``` ``` ``` ``` AuTHORS: Nico, Alex, Reva, Echo, Steve ``` ```*/ ``` ```/* ``` ```TODO: ``` ``` Check BOM more often and before final stop ``` ``` Make BOM check more comprehensively ``` ``` Use the center bot to check distances ``` ``` Count them ("spam" method) ``` ``` Use beacon to find relative positions ``` ``` Beacon tells them how to move to be at the right distance ``` ``` *done*Wireless communication, initialization ``` ```*/ ``` ```void forward(int speed){ // set the motors to this forward speed. ``` ``` motor_l_set(FORWARD,speed); ``` ``` motor_r_set(FORWARD,speed); ``` ```} ``` ```void left(int speed){ // turn left at this speed. ``` ``` motor_l_set(FORWARD,speed); ``` ``` motor_r_set(BACKWARD,speed); ``` ```} ``` ```void right(int speed){ ``` ``` motor_l_set(BACKWARD,speed); ``` ``` motor_r_set(FORWARD,speed); ``` ```} ``` ```void stop(void){ // could be set to motors_off(), or just use this as an alternative. ``` ``` motor_l_set(BACKWARD,0); ``` ``` motor_r_set(FORWARD,0); ``` ```} ``` ```void setforward(int spd1, int spd2){ ``` ``` motor_l_set(FORWARD,spd1); ``` ``` motor_r_set(FORWARD,spd2); ``` ```} ``` ```void backward(int speed){ ``` ``` motor_l_set(BACKWARD, speed); ``` ``` motor_r_set(BACKWARD, speed); ``` ```} ``` ```int get_distance(void){ // takes an averaged reading of the front rangefinder ``` ``` int temp,distance,kk=5; // kk sets this to 5 readings. ``` ``` distance =0; ``` ``` for (int i=0; i0) ``` ``` return (int)(distance/kk); ``` ``` else ``` ``` return 0; ``` ```} ``` ```void turn_to_beacon(int max){ ``` ``` if (max>-1 && max<16){ ``` ``` int index = (max+12)%16; ``` ``` if (index==0) { ``` ``` stop(); ``` ``` } ``` ``` else if (index<8) right(170); ``` ``` else left(170); ``` ``` } ``` ```} ``` ```void turn_to_beacon2(int max){ // like the previous but no stop() call ``` ``` if (max>-1 && max<16){ ``` ``` int index = (max+12)%16; ``` ``` if (index==0) { ``` ``` ``` ``` } ``` ``` else if (index<8) right(170); ``` ``` else left(170); ``` ``` } ``` ```} ``` ```void orient(void){ ``` ``` int max_index = -1; ``` ``` while (max_index!=4) { ``` ``` /* Refresh and make sure the table is updated */ ``` ``` bom_refresh(BOM_ALL); ``` ``` max_index = bom_get_max(); ``` ``` turn_to_beacon(max_index); ``` ``` delay_ms(22); ``` ``` } ``` ```} ``` ```void orient2(void){ ``` ``` int max_index = -1; ``` ``` while (max_index!=4) { ``` ``` /* Refresh and make sure the table is updated */ ``` ``` bom_refresh(BOM_ALL); ``` ``` max_index = bom_get_max(); ``` ``` turn_to_beacon2(max_index); ``` ``` delay_ms(22); ``` ``` } ``` ```} ``` ```void go_straight(void){ // drives forward a hardcoded distance. May not be useful. ``` ``` forward(200); ``` ``` encoder_rst_dx(LEFT); ``` ``` encoder_rst_dx(RIGHT); ``` ``` delay_ms(100); ``` ``` int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT); ``` ``` int count = 0; ``` ``` int d; ``` ``` while (count<25){ //count = 25 when bot6; count <12 ``` ``` x_left = encoder_get_x(LEFT); ``` ``` x_right = encoder_get_x(RIGHT); ``` ``` d = x_right-x_left; ``` ``` if (d>13 || d<-13){ ``` ``` if(d<50 && d>-50){ ``` ``` d = round(1.0*d/4); ``` ``` setforward(200+d, 200-d); ``` ``` } ``` ``` } ``` ``` delay_ms(32); ``` ``` count++; ``` ``` } ``` ```} ``` ```void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. ``` ``` go_straight(); ``` ```} ``` ```void blink(int num) { ``` ``` for(int i = 0; i=2 && packet_data[0]==CIRCLE_ACTION_EXIST) ``` ``` { ``` ``` send_buffer[0]=CIRCLE_ACTION_ACK; ``` ``` send_buffer[1]=get_robotid(); ``` ``` ``` ``` wl_basic_send_global_packet(42,send_buffer,2); ``` ``` break; ``` ``` } ``` ``` } ``` ``` ``` ``` while(1){ ``` ``` orb_set_color(RED); ``` ``` packet_data=wl_basic_do_default(&data_length); ``` ``` wl_basic_send_global_packet(42,send_buffer,2); ``` ``` if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_DONE) ``` ``` { ``` ``` break; ``` ``` } ``` ``` } ``` ``` ``` ``` orb_set_color(GREEN); ``` ``` ``` ``` orient(); ``` ``` forward(speed); ``` ``` //range_init(); ``` ``` ``` ``` orb_set_color(BLUE); ``` ``` ``` ``` distance = get_distance(); ``` ``` while (distance>=onefoot || distance==0) ``` ``` { ``` ``` if(distance==0) ``` ``` orb_set_color(WHITE); ``` ``` distance = get_distance(); ``` ``` orient2(); ``` ``` forward(speed); ``` ``` delay_ms(14); ``` ``` } ``` ``` stop(); ``` ``` orient(); ``` ``` ``` ``` //button1_wait (); // code for lab1. ``` ``` //go_straight_onefoot(); ``` ``` stop(); ``` ``` break; ``` ``` ``` ``` ``` ``` case 1: // BEACON ``` ``` switch(beacon_State) { ``` ``` case 0: ``` ``` bom_on(); ``` ``` orb_set_color(PURPLE); ``` ``` if(button1_click()) beacon_State=1; ``` ``` break; ``` ``` case 1: ``` ``` send_buffer[0]=CIRCLE_ACTION_EXIST; ``` ``` send_buffer[1]=get_robotid(); ``` ``` wl_basic_send_global_packet(42,send_buffer,2); ``` ``` beacon_State=2; ``` ``` break; ``` ``` case 2: ``` ``` orb_set_color(ORANGE); ``` ``` packet_data=wl_basic_do_default(&data_length); ``` ``` if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) ``` ``` { ``` ``` orb_set_color(WHITE); ``` ``` robotsRecieved++; ``` ``` } ``` ``` if(robotsRecieved==NUM_ROBOTS) ``` ``` beacon_State=3; ``` ``` break; ``` ``` case 3: ``` ``` send_buffer[0]=CIRCLE_ACTION_DONE; ``` ``` wl_basic_send_global_packet(42,send_buffer,2); ``` ``` break; ``` ``` } ``` ``` break; ``` ``` } ``` ``` } ``` ``` orb_set_color(RED); ``` ``` while(1); /* END HERE */ ``` ``` return 0; ``` ```} ```