root / trunk / code / behaviors / formation_control / circle_spacing / circle_spacing.c @ 1622
History | View | Annotate | Download (3.54 KB)
| 1 | 1558 | gnagaraj | #include <dragonfly_lib.h> |
|---|---|---|---|
| 2 | #include <wl_basic.h> |
||
| 3 | #include<encoders.h> |
||
| 4 | |||
| 5 | 1564 | gnagaraj | |
| 6 | int master = 0; |
||
| 7 | int sending = 0; |
||
| 8 | int stop = 0; |
||
| 9 | 1580 | gnagaraj | struct vector slave_position;
|
| 10 | 1622 | gnagaraj | int desired_max_bom;
|
| 11 | int bom_max_counter;
|
||
| 12 | 1564 | gnagaraj | |
| 13 | 1622 | gnagaraj | |
| 14 | void set_desired_max_bom(int desired_angle) |
||
| 15 | {
|
||
| 16 | if (desired_angle >= 348 || desired_angle < 11) desired_max_bom = 0; |
||
| 17 | if (desired_angle >= 11 && desired_angle < 33) desired_max_bom = 1; |
||
| 18 | if (desired_angle >= 33 && desired_angle < 56) desired_max_bom = 2; |
||
| 19 | if (desired_angle >= 56 && desired_angle < 78) desired_max_bom = 3; |
||
| 20 | if (desired_angle >= 78 && desired_angle < 101) desired_max_bom = 4; |
||
| 21 | if (desired_angle >= 101 && desired_angle < 123) desired_max_bom = 5; |
||
| 22 | if (desired_angle >= 123 && desired_angle < 145) desired_max_bom = 6; |
||
| 23 | if (desired_angle >= 145 && desired_angle < 167) desired_max_bom = 7; |
||
| 24 | if (desired_angle >= 167 && desired_angle < 190) desired_max_bom = 8; |
||
| 25 | if (desired_angle >= 190 && desired_angle < 212) desired_max_bom = 9; |
||
| 26 | if (desired_angle >= 212 && desired_angle < 235) desired_max_bom = 10; |
||
| 27 | if (desired_angle >= 235 && desired_angle < 257) desired_max_bom = 11; |
||
| 28 | if (desired_angle >= 257 && desired_angle < 280) desired_max_bom = 12; |
||
| 29 | if (desired_angle >= 280 && desired_angle < 302) desired_max_bom = 13; |
||
| 30 | if (desired_angle >= 302 && desired_angle < 325) desired_max_bom = 14; |
||
| 31 | if (desired_angle >= 325 && desired_angle < 348) desired_max_bom = 15; |
||
| 32 | } |
||
| 33 | |||
| 34 | |||
| 35 | |||
| 36 | |||
| 37 | |||
| 38 | |||
| 39 | 1564 | gnagaraj | void switch_sending ()
|
| 40 | {
|
||
| 41 | if(sending)
|
||
| 42 | {
|
||
| 43 | sending = 0;
|
||
| 44 | bom_off(); |
||
| 45 | } |
||
| 46 | else
|
||
| 47 | {
|
||
| 48 | sending = 1;
|
||
| 49 | bom_on(); |
||
| 50 | } |
||
| 51 | } |
||
| 52 | |||
| 53 | |||
| 54 | |||
| 55 | 1558 | gnagaraj | int main(void) |
| 56 | {
|
||
| 57 | |||
| 58 | |||
| 59 | 1564 | gnagaraj | |
| 60 | int sending_counter = 0; |
||
| 61 | 1622 | gnagaraj | |
| 62 | 1558 | gnagaraj | /* Initialize dragonfly board */
|
| 63 | |||
| 64 | dragonfly_init(ALL_ON); |
||
| 65 | xbee_init(); |
||
| 66 | wl_basic_init_default(); |
||
| 67 | wl_set_channel(12);
|
||
| 68 | int data_length;
|
||
| 69 | 1564 | gnagaraj | unsigned char *packet_data; |
| 70 | 1571 | gnagaraj | char send_buffer[2]; |
| 71 | 1564 | gnagaraj | |
| 72 | |||
| 73 | 1622 | gnagaraj | if(master) {orb1_set_color(RED); set_desired_max_bom(30);} |
| 74 | 1564 | gnagaraj | else orb1_set_color(BLUE);
|
| 75 | |||
| 76 | 1622 | gnagaraj | |
| 77 | 1564 | gnagaraj | while(1) |
| 78 | {
|
||
| 79 | |||
| 80 | |||
| 81 | |||
| 82 | |||
| 83 | 1558 | gnagaraj | bom_refresh(BOM_ALL); |
| 84 | |||
| 85 | |||
| 86 | 1564 | gnagaraj | |
| 87 | if(master)
|
||
| 88 | {
|
||
| 89 | |||
| 90 | 1622 | gnagaraj | if(sending_counter++>4) |
| 91 | 1564 | gnagaraj | {
|
| 92 | switch_sending(); |
||
| 93 | sending_counter = 0;
|
||
| 94 | 1571 | gnagaraj | send_buffer[0] = 'a'; |
| 95 | 1564 | gnagaraj | wl_basic_send_global_packet(42, send_buffer, 1); |
| 96 | } |
||
| 97 | |||
| 98 | |||
| 99 | if(sending)
|
||
| 100 | {
|
||
| 101 | |||
| 102 | |||
| 103 | |||
| 104 | |||
| 105 | |||
| 106 | } |
||
| 107 | |||
| 108 | else // recieving |
||
| 109 | {
|
||
| 110 | 1622 | gnagaraj | int max_bom = bom_get_max();
|
| 111 | usb_puts("bom_get_max : ");
|
||
| 112 | usb_puti(max_bom); |
||
| 113 | usb_puts("\n\r");
|
||
| 114 | 1580 | gnagaraj | |
| 115 | 1622 | gnagaraj | if(max_bom == desired_max_bom)
|
| 116 | {
|
||
| 117 | if(bom_max_counter) // only stops the slave if two consecutive boms reading give the desired bom as the max one. Filters the noise. |
||
| 118 | {
|
||
| 119 | send_buffer[0] = 's'; |
||
| 120 | wl_basic_send_global_packet(42, send_buffer, 2); |
||
| 121 | } |
||
| 122 | bom_max_counter =1;
|
||
| 123 | } |
||
| 124 | else bom_max_counter = 0; |
||
| 125 | 1580 | gnagaraj | |
| 126 | 1564 | gnagaraj | } |
| 127 | } |
||
| 128 | |||
| 129 | |||
| 130 | |||
| 131 | else // slave |
||
| 132 | {
|
||
| 133 | if(packet_data[0]=='s') stop=1; |
||
| 134 | if(packet_data[0]=='a') switch_sending(); |
||
| 135 | |||
| 136 | if(sending)
|
||
| 137 | {
|
||
| 138 | 1622 | gnagaraj | |
| 139 | 1564 | gnagaraj | } |
| 140 | |||
| 141 | else // recieving |
||
| 142 | {
|
||
| 143 | |||
| 144 | if(stop)
|
||
| 145 | {
|
||
| 146 | 1571 | gnagaraj | motor_l_set(FORWARD,0);
|
| 147 | motor_r_set(FORWARD,0);
|
||
| 148 | orb1_set_color(GREEN); |
||
| 149 | 1564 | gnagaraj | } |
| 150 | |||
| 151 | else
|
||
| 152 | {
|
||
| 153 | 1622 | gnagaraj | int max_bom = bom_get_max();
|
| 154 | usb_puts("bom_get_max : ");
|
||
| 155 | usb_puti(max_bom); |
||
| 156 | usb_puts("/n/r");
|
||
| 157 | 1580 | gnagaraj | |
| 158 | 1622 | gnagaraj | |
| 159 | if(max_bom == 8) |
||
| 160 | {
|
||
| 161 | 1571 | gnagaraj | orb2_set_color(BLUE); |
| 162 | 1564 | gnagaraj | motor_r_set(FORWARD,180);
|
| 163 | motor_l_set(FORWARD,180);
|
||
| 164 | |||
| 165 | } |
||
| 166 | 1622 | gnagaraj | else if(max_bom == 7 || max_bom == 6 || max_bom == 5) |
| 167 | {
|
||
| 168 | motor_l_set(FORWARD,180);
|
||
| 169 | motor_r_set(FORWARD,0);
|
||
| 170 | } |
||
| 171 | else if(max_bom == -1); |
||
| 172 | else
|
||
| 173 | {
|
||
| 174 | orb2_set_color(GREEN); |
||
| 175 | 1564 | gnagaraj | motor_l_set(FORWARD,0);
|
| 176 | motor_r_set(FORWARD,180);
|
||
| 177 | } |
||
| 178 | } |
||
| 179 | |||
| 180 | |||
| 181 | } |
||
| 182 | |||
| 183 | |||
| 184 | } |
||
| 185 | |||
| 186 | packet_data = wl_basic_do_default(&data_length); |
||
| 187 | delay_ms(10);
|
||
| 188 | } |
||
| 189 | 1558 | gnagaraj | |
| 190 | |||
| 191 | return 0; |
||
| 192 | |||
| 193 | |||
| 194 | |||
| 195 | |||
| 196 | } |
||
| 197 | |||
| 198 | |||
| 199 | 1622 | gnagaraj | |
| 200 | |||
| 201 |