root / trunk / code / behaviors / formation_control / circle2 / circle.c @ 1568
History | View | Annotate | Download (6.02 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 | 1524 | azirbel | void turn_to_beacon2(int max){ // like the previous but no stop() call |
| 88 | 1517 | azirbel | 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 | 1524 | azirbel | void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
| 118 | 1517 | azirbel | 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 | 1524 | azirbel | while (count<25){ //count = 25 when bot6; count <12 |
| 126 | 1517 | azirbel | 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 | 1524 | azirbel | void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
| 140 | 1517 | azirbel | go_straight(); |
| 141 | } |
||
| 142 | |||
| 143 | 1555 | azirbel | void blink(int num) { |
| 144 | for(int i = 0; i<num; i++) |
||
| 145 | {
|
||
| 146 | orb_set_color(ORB_OFF); |
||
| 147 | 1568 | sdevince | delay_ms(500);
|
| 148 | 1555 | azirbel | orb_set_color(RED); |
| 149 | 1568 | sdevince | delay_ms(500);
|
| 150 | 1555 | azirbel | } |
| 151 | orb_set_color(ORB_OFF); |
||
| 152 | } |
||
| 153 | 1552 | azirbel | |
| 154 | |||
| 155 | 1555 | azirbel | |
| 156 | 1517 | azirbel | int main(void) |
| 157 | {
|
||
| 158 | /* Initialize dragonfly board */
|
||
| 159 | dragonfly_init(ALL_ON); |
||
| 160 | 1552 | azirbel | xbee_init(); |
| 161 | 1517 | azirbel | encoders_init(); |
| 162 | 1552 | azirbel | /* Initialize the basic wireless library */
|
| 163 | wl_basic_init_default(); |
||
| 164 | /* Set the XBee channel to your assigned channel */
|
||
| 165 | 1568 | sdevince | wl_set_channel(24);
|
| 166 | 1552 | azirbel | |
| 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 | 1517 | azirbel | |
| 172 | 1552 | azirbel | |
| 173 | int state = EDGE;
|
||
| 174 | 1568 | sdevince | int beacon_State=0; |
| 175 | int NUM_ROBOTS=2; |
||
| 176 | int robotsRecieved=0; |
||
| 177 | 1552 | azirbel | if(wheel()<100) |
| 178 | {
|
||
| 179 | state=EDGE; |
||
| 180 | } |
||
| 181 | else
|
||
| 182 | {
|
||
| 183 | state=BEACON; |
||
| 184 | } |
||
| 185 | |||
| 186 | 1524 | azirbel | 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 | 1517 | azirbel | |
| 189 | 1552 | azirbel | 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 | 1568 | sdevince | break;
|
| 205 | 1552 | azirbel | } |
| 206 | 1568 | sdevince | } |
| 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 | 1552 | azirbel | {
|
| 215 | break;
|
||
| 216 | } |
||
| 217 | } |
||
| 218 | 1568 | sdevince | |
| 219 | 1552 | azirbel | orb_set_color(GREEN); |
| 220 | |||
| 221 | orient(); |
||
| 222 | forward(speed); |
||
| 223 | 1555 | azirbel | //range_init();
|
| 224 | 1552 | azirbel | |
| 225 | 1555 | azirbel | orb_set_color(BLUE); |
| 226 | 1552 | azirbel | |
| 227 | distance = get_distance(); |
||
| 228 | while (distance>=onefoot || distance==0) |
||
| 229 | {
|
||
| 230 | 1568 | sdevince | if(distance==0) |
| 231 | orb_set_color(WHITE); |
||
| 232 | 1552 | azirbel | 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 | 1568 | sdevince | 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 | 1552 | azirbel | packet_data=wl_basic_do_default(&data_length); |
| 262 | 1568 | sdevince | if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
| 263 | 1552 | azirbel | {
|
| 264 | 1568 | sdevince | orb_set_color(WHITE); |
| 265 | robotsRecieved++; |
||
| 266 | 1552 | azirbel | } |
| 267 | 1568 | sdevince | if(robotsRecieved==NUM_ROBOTS)
|
| 268 | beacon_State=3;
|
||
| 269 | break;
|
||
| 270 | case 3: |
||
| 271 | send_buffer[0]=CIRCLE_ACTION_DONE;
|
||
| 272 | 1552 | azirbel | wl_basic_send_global_packet(42,send_buffer,2); |
| 273 | 1568 | sdevince | break;
|
| 274 | } |
||
| 275 | 1552 | azirbel | break;
|
| 276 | } |
||
| 277 | 1517 | azirbel | } |
| 278 | |||
| 279 | 1552 | azirbel | orb_set_color(RED); |
| 280 | 1517 | azirbel | while(1); /* END HERE */ |
| 281 | |||
| 282 | return 0; |
||
| 283 | } |