Revision 1568
Fixed beacon robot wireless and working on Edge robot wireless
| trunk/code/behaviors/formation_control/circle2/circle.c | ||
|---|---|---|
| 15 | 15 |
With adjustment, the leader robot will be able to turn and use its standardized |
| 16 | 16 |
rangefinder to reposition or space the robots evenly. |
| 17 | 17 |
|
| 18 |
AuTHORS: Niko, Alex, Reva, Echo, Steve
|
|
| 18 |
AuTHORS: Nico, Alex, Reva, Echo, Steve
|
|
| 19 | 19 |
*/ |
| 20 | 20 |
|
| 21 | 21 |
|
| 22 | 22 |
/* |
| 23 | 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 |
|
| 24 |
Check BOM more often and before final stop
|
|
| 25 |
Make BOM check more comprehensively
|
|
| 26 |
Use the center bot to check distances
|
|
| 27 | 27 |
Count them ("spam" method)
|
| 28 | 28 |
Use beacon to find relative positions |
| 29 | 29 |
Beacon tells them how to move to be at the right distance |
| 30 |
Wireless communication, initialization |
|
| 30 |
*done*Wireless communication, initialization
|
|
| 31 | 31 |
*/ |
| 32 | 32 |
|
| 33 | 33 |
void forward(int speed){ // set the motors to this forward speed.
|
| ... | ... | |
| 61 | 61 |
temp = range_read_distance(IR2); |
| 62 | 62 |
if (temp == -1) |
| 63 | 63 |
{
|
| 64 |
temp=0; |
|
| 65 |
kk--;
|
|
| 64 |
//temp=0;
|
|
| 65 |
i--;
|
|
| 66 | 66 |
} |
| 67 |
distance+= temp; |
|
| 67 |
else |
|
| 68 |
distance+= temp; |
|
| 68 | 69 |
delay_ms(3); |
| 69 | 70 |
} |
| 70 | 71 |
if (kk>0) |
| ... | ... | |
| 143 | 144 |
for(int i = 0; i<num; i++) |
| 144 | 145 |
{
|
| 145 | 146 |
orb_set_color(ORB_OFF); |
| 146 |
delay_ms(200);
|
|
| 147 |
delay_ms(500);
|
|
| 147 | 148 |
orb_set_color(RED); |
| 148 |
delay_ms(200);
|
|
| 149 |
delay_ms(500);
|
|
| 149 | 150 |
} |
| 150 | 151 |
orb_set_color(ORB_OFF); |
| 151 | 152 |
} |
| ... | ... | |
| 161 | 162 |
/* Initialize the basic wireless library */ |
| 162 | 163 |
wl_basic_init_default(); |
| 163 | 164 |
/* Set the XBee channel to your assigned channel */ |
| 164 |
wl_set_channel(12);
|
|
| 165 |
wl_set_channel(24);
|
|
| 165 | 166 |
|
| 166 | 167 |
int robotid = get_robotid(); |
| 167 | 168 |
char send_buffer[2]; |
| ... | ... | |
| 170 | 171 |
|
| 171 | 172 |
|
| 172 | 173 |
int state = EDGE; |
| 174 |
int beacon_State=0; |
|
| 175 |
int NUM_ROBOTS=2; |
|
| 176 |
int robotsRecieved=0; |
|
| 173 | 177 |
if(wheel()<100) |
| 174 | 178 |
{
|
| 175 | 179 |
state=EDGE; |
| ... | ... | |
| 197 | 201 |
send_buffer[1]=get_robotid(); |
| 198 | 202 |
|
| 199 | 203 |
wl_basic_send_global_packet(42,send_buffer,2); |
| 204 |
break; |
|
| 200 | 205 |
} |
| 201 |
|
|
| 202 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
|
| 206 |
} |
|
| 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) |
|
| 203 | 214 |
{
|
| 204 | 215 |
break; |
| 205 | 216 |
} |
| 206 | 217 |
} |
| 218 |
|
|
| 207 | 219 |
orb_set_color(GREEN); |
| 208 | 220 |
|
| 209 | 221 |
orient(); |
| ... | ... | |
| 215 | 227 |
distance = get_distance(); |
| 216 | 228 |
while (distance>=onefoot || distance==0) |
| 217 | 229 |
{
|
| 230 |
if(distance==0) |
|
| 231 |
orb_set_color(WHITE); |
|
| 218 | 232 |
distance = get_distance(); |
| 219 | 233 |
orient2(); |
| 220 | 234 |
forward(speed); |
| ... | ... | |
| 230 | 244 |
|
| 231 | 245 |
|
| 232 | 246 |
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 |
|
|
| 247 |
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); |
|
| 246 | 261 |
packet_data=wl_basic_do_default(&data_length); |
| 247 |
while(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
|
|
| 262 |
if(packet_data!=0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK)
|
|
| 248 | 263 |
{
|
| 249 |
// IF NEEDED: a good place to collect and store the robot id.
|
|
| 250 |
numrobots++;
|
|
| 264 |
orb_set_color(WHITE);
|
|
| 265 |
robotsRecieved++;
|
|
| 251 | 266 |
} |
| 252 |
|
|
| 253 |
send_buffer[0]=CIRCLE_ACTION_ACK; |
|
| 267 |
if(robotsRecieved==NUM_ROBOTS) |
|
| 268 |
beacon_State=3; |
|
| 269 |
break; |
|
| 270 |
case 3: |
|
| 271 |
send_buffer[0]=CIRCLE_ACTION_DONE; |
|
| 254 | 272 |
wl_basic_send_global_packet(42,send_buffer,2); |
| 255 |
|
|
| 256 |
blink(numrobots); |
|
| 257 |
|
|
| 258 |
|
|
| 273 |
break; |
|
| 274 |
} |
|
| 259 | 275 |
break; |
| 260 | 276 |
} |
| 261 | 277 |
} |
| trunk/code/behaviors/formation_control/circle2/circle.h | ||
|---|---|---|
| 14 | 14 |
#define CIRCLE_ACTION_EXIST 'E' |
| 15 | 15 |
#define CIRCLE_ACTION_POSITION 'P' |
| 16 | 16 |
#define CIRCLE_ACTION_ACK 'A' |
| 17 |
#define CIRCLE_ACTION_DONE 'D' |
|
| 17 | 18 |
//#define EDGE 0; |
| 18 | 19 |
//#define BEACON 1; |
| 19 | 20 |
|
| trunk/code/behaviors/formation_control/circle2/Makefile | ||
|---|---|---|
| 11 | 11 |
USE_WIRELESS = 1 |
| 12 | 12 |
|
| 13 | 13 |
# com1 = serial port. Use lpt1 to connect to parallel port. |
| 14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM4:'; else echo '/dev/ttyUSB0'; fi) |
|
| 15 |
|
|
| 14 |
AVRDUDE_PORT = $(shell if uname -s |grep -i w32 >/dev/null; then echo 'COM7:'; else echo '/dev/ttyUSB0'; fi) |
|
| 16 | 15 |
else |
| 17 | 16 |
COLONYROOT := ../$(COLONYROOT) |
| 18 | 17 |
endif |
Also available in: Unified diff