Revision 1566
branches/ir_branch/behaviors/formation_control/circle_spacing/circle_spacing.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include <wl_basic.h> |
|
3 |
#include<encoders.h> |
|
4 |
|
|
5 |
|
|
6 |
int master = 0; |
|
7 |
int sending = 0; |
|
8 |
int stop = 0; |
|
9 |
|
|
10 |
|
|
11 |
void switch_sending () |
|
12 |
{ |
|
13 |
if(sending) |
|
14 |
{ |
|
15 |
sending = 0; |
|
16 |
bom_off(); |
|
17 |
} |
|
18 |
else |
|
19 |
{ |
|
20 |
sending = 1; |
|
21 |
bom_on(); |
|
22 |
} |
|
23 |
} |
|
24 |
|
|
25 |
|
|
26 |
|
|
27 |
int main(void) |
|
28 |
{ |
|
29 |
|
|
30 |
|
|
31 |
|
|
32 |
int sending_counter = 0; |
|
33 |
|
|
34 |
/* Initialize dragonfly board */ |
|
35 |
|
|
36 |
dragonfly_init(ALL_ON); |
|
37 |
xbee_init(); |
|
38 |
wl_basic_init_default(); |
|
39 |
wl_set_channel(12); |
|
40 |
int data_length; |
|
41 |
unsigned char *packet_data; |
|
42 |
char send_buffer; |
|
43 |
|
|
44 |
|
|
45 |
if(master) orb1_set_color(RED); |
|
46 |
else orb1_set_color(BLUE); |
|
47 |
|
|
48 |
while(1) |
|
49 |
{ |
|
50 |
|
|
51 |
|
|
52 |
|
|
53 |
|
|
54 |
bom_refresh(BOM_ALL); |
|
55 |
|
|
56 |
|
|
57 |
|
|
58 |
if(master) |
|
59 |
{ |
|
60 |
|
|
61 |
if(sending_counter++>100) |
|
62 |
{ |
|
63 |
switch_sending(); |
|
64 |
sending_counter = 0; |
|
65 |
send_buffer = 'a'; |
|
66 |
wl_basic_send_global_packet(42, send_buffer, 1); |
|
67 |
} |
|
68 |
|
|
69 |
|
|
70 |
if(sending) |
|
71 |
{ |
|
72 |
|
|
73 |
|
|
74 |
|
|
75 |
|
|
76 |
|
|
77 |
} |
|
78 |
|
|
79 |
else // recieving |
|
80 |
{ |
|
81 |
if(bom_get_max()+1==5) |
|
82 |
{ |
|
83 |
send_buffer = 's'; |
|
84 |
wl_basic_send_global_packet(42, send_buffer, 1); |
|
85 |
} |
|
86 |
} |
|
87 |
} |
|
88 |
|
|
89 |
|
|
90 |
|
|
91 |
else // slave |
|
92 |
{ |
|
93 |
if(packet_data[0]=='s') stop=1; |
|
94 |
if(packet_data[0]=='a') switch_sending(); |
|
95 |
|
|
96 |
if(sending) |
|
97 |
{ |
|
98 |
motor_l_set(FORWARD,0); |
|
99 |
motor_r_set(FORWARD,0); |
|
100 |
} |
|
101 |
|
|
102 |
else // recieving |
|
103 |
{ |
|
104 |
|
|
105 |
if(stop) |
|
106 |
{ |
|
107 |
} |
|
108 |
|
|
109 |
else |
|
110 |
{ |
|
111 |
|
|
112 |
|
|
113 |
if(bom_get_max()+1==8){ |
|
114 |
orb1_set_color(RED); |
|
115 |
motor_r_set(FORWARD,180); |
|
116 |
motor_l_set(FORWARD,180); |
|
117 |
|
|
118 |
} |
|
119 |
else { |
|
120 |
motor_l_set(FORWARD,0); |
|
121 |
motor_r_set(FORWARD,180); |
|
122 |
} |
|
123 |
} |
|
124 |
|
|
125 |
|
|
126 |
} |
|
127 |
|
|
128 |
|
|
129 |
} |
|
130 |
|
|
131 |
packet_data = wl_basic_do_default(&data_length); |
|
132 |
delay_ms(10); |
|
133 |
} |
|
134 |
|
|
135 |
|
|
136 |
return 0; |
|
137 |
|
|
138 |
|
|
139 |
|
|
140 |
|
|
141 |
} |
|
142 |
|
|
143 |
|
branches/ir_branch/behaviors/formation_control/circle_spacing/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/formation_control/circle/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/formation_control/circle/circle.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include <encoders.h> |
|
3 |
|
|
4 |
#define RADIUS 300 |
|
5 |
|
|
6 |
/* |
|
7 |
This is a PREVIOUS VERSION of circle2. Circle2 uses Echo's more exact code, while this uses a different method which is more buggy. |
|
8 |
This code has been kept for the time being, in case some of the code is useful for recylcling (the control loop, for instance) - without |
|
9 |
reverting the entire repositiory. |
|
10 |
|
|
11 |
This program is used to make robots target a center (leader) robot using the BOM, |
|
12 |
then drive toward it and stop at a certain distance away. |
|
13 |
|
|
14 |
The distance will eventually be adjustable. |
|
15 |
|
|
16 |
With adjustment, the leader robot will be able to turn and use its standardized |
|
17 |
rangefinder to reposition or space the robots evenly. |
|
18 |
|
|
19 |
AuTHORS: Niko, Alex |
|
20 |
*/ |
|
21 |
|
|
22 |
extern void blink(int); |
|
23 |
extern void search(void); |
|
24 |
extern void approach(void); |
|
25 |
extern void lastDrive(void); |
|
26 |
|
|
27 |
int main(void) |
|
28 |
{ |
|
29 |
|
|
30 |
/* Initialize dragonfly board */ |
|
31 |
dragonfly_init(ALL_ON); |
|
32 |
encoders_init(); |
|
33 |
|
|
34 |
orb_set_color(GREEN); |
|
35 |
delay_ms(300); |
|
36 |
|
|
37 |
search(); |
|
38 |
|
|
39 |
approach(); |
|
40 |
|
|
41 |
while(1); /* END HERE */ |
|
42 |
|
|
43 |
return 0; |
|
44 |
} |
|
45 |
|
|
46 |
|
|
47 |
|
|
48 |
void blink(int num) // blinks the number of times specifed, with the orbs. |
|
49 |
{ // useful for error checking. |
|
50 |
for(int i = 0; i<num; i++) |
|
51 |
{ |
|
52 |
orb_set_color(ORB_OFF); |
|
53 |
delay_ms(200); |
|
54 |
orb_set_color(RED); |
|
55 |
delay_ms(200); |
|
56 |
} |
|
57 |
orb_set_color(ORB_OFF); |
|
58 |
} |
|
59 |
void search() //makes the robot face the beacon |
|
60 |
{ |
|
61 |
orb_set_color(YELLOW); //show that the robot is searching |
|
62 |
bom_refresh(BOM_ALL); |
|
63 |
int top=bom_get_max(); |
|
64 |
|
|
65 |
if ((top<4) || (top>11)) //determine which way to turn |
|
66 |
{ |
|
67 |
motor_r_set(BACKWARD,200); |
|
68 |
motor_l_set(FORWARD,200); |
|
69 |
} |
|
70 |
else if((top>4) && (top<12)) |
|
71 |
{ |
|
72 |
motor_r_set(FORWARD,200); |
|
73 |
motor_l_set(BACKWARD,200); |
|
74 |
} |
|
75 |
while (top != 4) |
|
76 |
{ |
|
77 |
bom_refresh(BOM_ALL); |
|
78 |
top=bom_get_max(); |
|
79 |
} |
|
80 |
motors_off(); |
|
81 |
} |
|
82 |
void approach() // drives to the right distance. |
|
83 |
{ |
|
84 |
int dist = 0; |
|
85 |
int temp = 0; |
|
86 |
int counter = 0; |
|
87 |
// int correct = 0; |
|
88 |
|
|
89 |
while(dist != RADIUS) //puts the robots in the given distance RADIUS |
|
90 |
{ |
|
91 |
temp=0; |
|
92 |
counter = 0; |
|
93 |
|
|
94 |
while(counter<8) // averages the first 8 legitimate (non -1) readings |
|
95 |
{ |
|
96 |
if(range_read_distance(IR2) == -1){} |
|
97 |
|
|
98 |
else |
|
99 |
{ |
|
100 |
temp += range_read_distance(IR2); |
|
101 |
counter++; |
|
102 |
} |
|
103 |
delay_ms(20); |
|
104 |
} |
|
105 |
dist = temp/8; |
|
106 |
|
|
107 |
/*if (correct >2){ |
|
108 |
orb_set_color(RED); |
|
109 |
break; |
|
110 |
|
|
111 |
} // stop within the dead zone: it knows it is in the dead zone if it hits +- 15 from RADIUS 3 times*/ |
|
112 |
bom_refresh(BOM_ALL); |
|
113 |
int top = bom_get_max(); |
|
114 |
while(top!=4) |
|
115 |
{ |
|
116 |
search(); |
|
117 |
bom_refresh(BOM_ALL); |
|
118 |
top = bom_get_max(); |
|
119 |
} |
|
120 |
|
|
121 |
if ( dist<(RADIUS+30) && dist>(RADIUS-30)) { // defines the dead zone +- 30 from the given radius |
|
122 |
orb_set_color(RED); |
|
123 |
break; |
|
124 |
} |
|
125 |
|
|
126 |
else if ((dist>RADIUS) || (dist==-1)) |
|
127 |
{ |
|
128 |
///delay_ms(200); |
|
129 |
search(); |
|
130 |
|
|
131 |
motor_r_set(FORWARD,200); |
|
132 |
motor_l_set(FORWARD,200); |
|
133 |
} |
|
134 |
else if ((dist<RADIUS)) |
|
135 |
{ |
|
136 |
//delay_ms(200); |
|
137 |
search(); |
|
138 |
|
|
139 |
motor_r_set(BACKWARD,200); |
|
140 |
motor_l_set(BACKWARD,200); |
|
141 |
} |
|
142 |
|
|
143 |
} |
|
144 |
|
|
145 |
orb_set_color(BLUE); |
|
146 |
|
|
147 |
motors_off(); |
|
148 |
|
|
149 |
/*usb_puti(range_read_distance(IR2)); |
|
150 |
usb_puts("\n\r"); |
|
151 |
motor_r_set(FORWARD,200); |
|
152 |
motor_l_set(FORWARD,200); |
|
153 |
delay_ms(300); |
|
154 |
usb_puti(range_read_distance(IR2)); |
|
155 |
usb_puts("\n\r"); |
|
156 |
delay_ms(300); |
|
157 |
usb_puti(range_read_distance(IR2)); |
|
158 |
usb_puts("\n\r"); |
|
159 |
delay_ms(300); |
|
160 |
usb_puti(range_read_distance(IR2)); |
|
161 |
usb_puts("\n\r");*/ |
|
162 |
} |
|
163 |
void lastDrive() // used for the final drive in lab1 and not useful here; control loop code may be recycled. |
|
164 |
{ |
|
165 |
int lspeed = 190; |
|
166 |
int rspeed = 190; |
|
167 |
int firstldist = encoder_get_x(LEFT); |
|
168 |
int firstrdist = encoder_get_x(RIGHT); |
|
169 |
int ldist = firstldist; |
|
170 |
int rdist = firstrdist; |
|
171 |
while ( (ldist-firstldist<800) && (rdist-firstrdist<800) ) |
|
172 |
{ |
|
173 |
motor_l_set(FORWARD,lspeed); |
|
174 |
motor_r_set(FORWARD,rspeed); |
|
175 |
|
|
176 |
int lvel = encoder_get_v(LEFT); |
|
177 |
int rvel = encoder_get_v(RIGHT); |
|
178 |
|
|
179 |
usb_puts("\n\rLeft distance: "); |
|
180 |
usb_puti(ldist-firstldist); |
|
181 |
usb_puts("\n\rRight distance: "); |
|
182 |
usb_puti(rdist-firstrdist); |
|
183 |
usb_puts("\n\rLeft velocity: "); |
|
184 |
usb_puti(lvel); |
|
185 |
usb_puts("\n\rRight velocity: "); |
|
186 |
usb_puti(rvel); |
|
187 |
usb_puts("\n\r\n\r"); |
|
188 |
|
|
189 |
if (rvel<lvel) // adjust the speeds if the encoders have a mismatch. |
|
190 |
{ |
|
191 |
rspeed += 1; |
|
192 |
lspeed -= 1; |
|
193 |
} |
|
194 |
else if (rvel>lvel) |
|
195 |
{ |
|
196 |
rspeed -= 1; |
|
197 |
lspeed += 1; |
|
198 |
} |
|
199 |
ldist = encoder_get_x(LEFT); |
|
200 |
rdist = encoder_get_x(RIGHT); |
|
201 |
|
|
202 |
delay_ms(25); |
|
203 |
} |
|
204 |
motors_off(); |
|
205 |
orb_set_color(GREEN); |
|
206 |
} |
branches/ir_branch/behaviors/formation_control/circle2/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/formation_control/circle2/circle.c | ||
---|---|---|
1 |
#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 |
|
|
9 |
/* |
|
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 |
|
|
15 |
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 |
AuTHORS: Niko, Alex, Reva, Echo, Steve |
|
19 |
*/ |
|
20 |
|
|
21 |
|
|
22 |
/* |
|
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 |
|
27 |
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 |
Wireless communication, initialization |
|
31 |
*/ |
|
32 |
|
|
33 |
void forward(int speed){ // set the motors to this forward speed. |
|
34 |
motor_l_set(FORWARD,speed); |
|
35 |
motor_r_set(FORWARD,speed); |
|
36 |
} |
|
37 |
void left(int speed){ // turn left at this speed. |
|
38 |
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 |
void stop(void){ // could be set to motors_off(), or just use this as an alternative. |
|
46 |
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 |
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 |
distance =0; |
|
60 |
for (int i=0; i<kk; i++){ |
|
61 |
temp = range_read_distance(IR2); |
|
62 |
if (temp == -1) |
|
63 |
{ |
|
64 |
temp=0; |
|
65 |
kk--; |
|
66 |
} |
|
67 |
distance+= temp; |
|
68 |
delay_ms(3); |
|
69 |
} |
|
70 |
if (kk>0) |
|
71 |
return (int)(distance/kk); |
|
72 |
else |
|
73 |
return 0; |
|
74 |
} |
|
75 |
void turn_to_beacon(int max){ |
|
76 |
if (max>-1 && max<16){ |
|
77 |
int index = (max+12)%16; |
|
78 |
if (index==0) { |
|
79 |
stop(); |
|
80 |
} |
|
81 |
else if (index<8) right(170); |
|
82 |
else left(170); |
|
83 |
} |
|
84 |
} |
|
85 |
|
|
86 |
void turn_to_beacon2(int max){ // like the previous but no stop() call |
|
87 |
if (max>-1 && max<16){ |
|
88 |
int index = (max+12)%16; |
|
89 |
if (index==0) { |
|
90 |
|
|
91 |
} |
|
92 |
else if (index<8) right(170); |
|
93 |
else left(170); |
|
94 |
} |
|
95 |
} |
|
96 |
void orient(void){ |
|
97 |
int max_index = -1; |
|
98 |
while (max_index!=4) { |
|
99 |
/* Refresh and make sure the table is updated */ |
|
100 |
bom_refresh(BOM_ALL); |
|
101 |
max_index = bom_get_max(); |
|
102 |
turn_to_beacon(max_index); |
|
103 |
delay_ms(22); |
|
104 |
} |
|
105 |
} |
|
106 |
void orient2(void){ |
|
107 |
int max_index = -1; |
|
108 |
while (max_index!=4) { |
|
109 |
/* Refresh and make sure the table is updated */ |
|
110 |
bom_refresh(BOM_ALL); |
|
111 |
max_index = bom_get_max(); |
|
112 |
turn_to_beacon2(max_index); |
|
113 |
delay_ms(22); |
|
114 |
} |
|
115 |
} |
|
116 |
void go_straight(void){ // drives forward a hardcoded distance. May not be useful. |
|
117 |
forward(200); |
|
118 |
encoder_rst_dx(LEFT); |
|
119 |
encoder_rst_dx(RIGHT); |
|
120 |
delay_ms(100); |
|
121 |
int x_left = encoder_get_x(LEFT), x_right = encoder_get_x(RIGHT); |
|
122 |
int count = 0; |
|
123 |
int d; |
|
124 |
while (count<25){ //count = 25 when bot6; count <12 |
|
125 |
x_left = encoder_get_x(LEFT); |
|
126 |
x_right = encoder_get_x(RIGHT); |
|
127 |
d = x_right-x_left; |
|
128 |
if (d>13 || d<-13){ |
|
129 |
if(d<50 && d>-50){ |
|
130 |
d = round(1.0*d/4); |
|
131 |
setforward(200+d, 200-d); |
|
132 |
} |
|
133 |
} |
|
134 |
delay_ms(32); |
|
135 |
count++; |
|
136 |
} |
|
137 |
} |
|
138 |
void go_straight_onefoot(void){ // essentially, another name for the above. Should be removed. |
|
139 |
go_straight(); |
|
140 |
} |
|
141 |
|
|
142 |
void blink(int num) { |
|
143 |
for(int i = 0; i<num; i++) |
|
144 |
{ |
|
145 |
orb_set_color(ORB_OFF); |
|
146 |
delay_ms(200); |
|
147 |
orb_set_color(RED); |
|
148 |
delay_ms(200); |
|
149 |
} |
|
150 |
orb_set_color(ORB_OFF); |
|
151 |
} |
|
152 |
|
|
153 |
|
|
154 |
|
|
155 |
int main(void) |
|
156 |
{ |
|
157 |
/* Initialize dragonfly board */ |
|
158 |
dragonfly_init(ALL_ON); |
|
159 |
xbee_init(); |
|
160 |
encoders_init(); |
|
161 |
/* Initialize the basic wireless library */ |
|
162 |
wl_basic_init_default(); |
|
163 |
/* Set the XBee channel to your assigned channel */ |
|
164 |
wl_set_channel(12); |
|
165 |
|
|
166 |
int robotid = get_robotid(); |
|
167 |
char send_buffer[2]; |
|
168 |
int data_length; |
|
169 |
unsigned char *packet_data=wl_basic_do_default(&data_length); |
|
170 |
|
|
171 |
|
|
172 |
int state = EDGE; |
|
173 |
if(wheel()<100) |
|
174 |
{ |
|
175 |
state=EDGE; |
|
176 |
} |
|
177 |
else |
|
178 |
{ |
|
179 |
state=BEACON; |
|
180 |
} |
|
181 |
|
|
182 |
int distance=1000; // how far away to stop. |
|
183 |
int onefoot=300, speed=200; // one foot is 490 for bot 1; one foot is 200 for bot6 |
|
184 |
|
|
185 |
while(1) { |
|
186 |
switch(state) { |
|
187 |
case 0: // EDGE |
|
188 |
|
|
189 |
orb_set_color(GREEN); |
|
190 |
while(1) |
|
191 |
{ |
|
192 |
orb_set_color(YELLOW); |
|
193 |
packet_data=wl_basic_do_default(&data_length); |
|
194 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_EXIST) |
|
195 |
{ |
|
196 |
send_buffer[0]=CIRCLE_ACTION_ACK; |
|
197 |
send_buffer[1]=get_robotid(); |
|
198 |
|
|
199 |
wl_basic_send_global_packet(42,send_buffer,2); |
|
200 |
} |
|
201 |
|
|
202 |
if(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
|
203 |
{ |
|
204 |
break; |
|
205 |
} |
|
206 |
} |
|
207 |
orb_set_color(GREEN); |
|
208 |
|
|
209 |
orient(); |
|
210 |
forward(speed); |
|
211 |
//range_init(); |
|
212 |
|
|
213 |
orb_set_color(BLUE); |
|
214 |
|
|
215 |
distance = get_distance(); |
|
216 |
while (distance>=onefoot || distance==0) |
|
217 |
{ |
|
218 |
distance = get_distance(); |
|
219 |
orient2(); |
|
220 |
forward(speed); |
|
221 |
delay_ms(14); |
|
222 |
} |
|
223 |
stop(); |
|
224 |
orient(); |
|
225 |
|
|
226 |
//button1_wait (); // code for lab1. |
|
227 |
//go_straight_onefoot(); |
|
228 |
stop(); |
|
229 |
break; |
|
230 |
|
|
231 |
|
|
232 |
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 |
|
|
246 |
packet_data=wl_basic_do_default(&data_length); |
|
247 |
while(packet_data != 0 && data_length>=2 && packet_data[0]==CIRCLE_ACTION_ACK) |
|
248 |
{ |
|
249 |
// IF NEEDED: a good place to collect and store the robot id. |
|
250 |
numrobots++; |
|
251 |
} |
|
252 |
|
|
253 |
send_buffer[0]=CIRCLE_ACTION_ACK; |
|
254 |
wl_basic_send_global_packet(42,send_buffer,2); |
|
255 |
|
|
256 |
blink(numrobots); |
|
257 |
|
|
258 |
|
|
259 |
break; |
|
260 |
} |
|
261 |
} |
|
262 |
|
|
263 |
orb_set_color(RED); |
|
264 |
while(1); /* END HERE */ |
|
265 |
|
|
266 |
return 0; |
|
267 |
} |
branches/ir_branch/behaviors/formation_control/circle2/circle.h | ||
---|---|---|
1 |
#ifndef _CIRCLE_H |
|
2 |
#define _CIRCLE_H |
|
3 |
|
|
4 |
#include <inttypes.h> |
|
5 |
|
|
6 |
/**** This file should not be edited! ****/ |
|
7 |
|
|
8 |
/* |
|
9 |
* The packet structure is 2 bytes |
|
10 |
* byte 0 is the action, which is one of the values below |
|
11 |
* byte 1 is the robot id |
|
12 |
*/ |
|
13 |
|
|
14 |
#define CIRCLE_ACTION_EXIST 'E' |
|
15 |
#define CIRCLE_ACTION_POSITION 'P' |
|
16 |
#define CIRCLE_ACTION_ACK 'A' |
|
17 |
//#define EDGE 0; |
|
18 |
//#define BEACON 1; |
|
19 |
|
|
20 |
#endif |
branches/ir_branch/behaviors/formation_control/push_pull/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/formation_control/push_pull/push_pull.c | ||
---|---|---|
1 |
#include <stdint.h> |
|
2 |
#include <dragonfly_lib.h> |
|
3 |
#include <wl_basic.h> |
|
4 |
#include <stdlib.h> |
|
5 |
|
|
6 |
int main (void) { |
|
7 |
|
|
8 |
Vector v; |
|
9 |
|
|
10 |
dragonfly_init(ALL_ON); |
|
11 |
xbee_init(); |
|
12 |
encoders_init(); |
|
13 |
|
|
14 |
orbs_set_color(BLUE, GREEN); |
|
15 |
delay_ms(1000); |
|
16 |
orbs_set_color(GREEN, BLUE); |
|
17 |
delay_ms(1000); |
|
18 |
orbs_set_color(RED, RED); |
|
19 |
|
|
20 |
while (1) { |
|
21 |
bom_print_usb(NULL); |
|
22 |
|
|
23 |
bom_get_vector(&v, NULL); |
|
24 |
|
|
25 |
usb_puts("x: "); |
|
26 |
usb_puti(v.x); |
|
27 |
usb_puts("\ty: "); |
|
28 |
usb_puti(v.y); |
|
29 |
usb_puts("\n"); |
|
30 |
|
|
31 |
delay_ms(50); |
|
32 |
} |
|
33 |
|
|
34 |
while(1); |
|
35 |
|
|
36 |
} |
branches/ir_branch/behaviors/formation_control/push_pull/vectorSizeSpecs.m | ||
---|---|---|
1 |
% vectorSizeSpecs - Script used to help calculate the max scalar value by |
|
2 |
% which to multiply the BOM unit vectors (thus increasing the |
|
3 |
% precision of the scaled vectors) such that in the worst case, the |
|
4 |
% total net sum will not overflow the data type used to represent |
|
5 |
% the net vector components. |
|
6 |
% |
|
7 |
% Author: John Sexton, Colony Project, CMU Robotics Club |
|
8 |
|
|
9 |
|
|
10 |
% Parameters |
|
11 |
maxIntensity = 255; |
|
12 |
dataBits = 16; |
|
13 |
scalar = 25; |
|
14 |
|
|
15 |
n = 0:8; |
|
16 |
angle = n * 2 * pi / 16; |
|
17 |
|
|
18 |
vector_components = sin(angle); |
|
19 |
max_net_component = sum(vector_components); |
|
20 |
|
|
21 |
% Formula for max_net_component (mnc), obtained by summing |
|
22 |
% values from the unit circle at angles of 0 : pi/8 : pi |
|
23 |
% in quadrants I and II |
|
24 |
mnc = 1; %sin(pi/2) |
|
25 |
mnc = mnc + (2* (1/2)*sqrt(2+sqrt(2))); %sin(3pi/8) and sin(5pi/8) |
|
26 |
mnc = mnc + (2* sqrt(2)/2); %sin(pi/4) and sin(3pi/4) |
|
27 |
mnc = mnc + (2* (1/2)*sqrt(2-sqrt(2))); %sin(pi/8) and sin(7pi/8) |
|
28 |
|
|
29 |
|
|
30 |
calc_scalar = (2^(dataBits-1)-1) / (max_net_component * maxIntensity); |
|
31 |
% calc_scalar = 25.560 for 16 bits => use 25 as scalar for int data type |
|
32 |
|
|
33 |
fprintf('With %d data bits, calculated scalar value: %.3f\n\n', dataBits, calc_scalar); |
|
34 |
|
|
35 |
|
|
36 |
% Check worst case |
|
37 |
scaled_vector_components = scalar * vector_components; |
|
38 |
|
|
39 |
worst_case = fix(scaled_vector_components) * maxIntensity; |
|
40 |
worst_sum = sum(worst_case); |
|
41 |
fprintf('With scalar %d, max worst case sum: %d\n', scalar, round(worst_sum)); |
|
42 |
fprintf('Max number: 2^(%d-1) - 1 = %d\n', dataBits, (2^(dataBits-1)-1)); |
|
43 |
|
|
44 |
|
|
45 |
% Calculate the x and y component arrays which should be used in the |
|
46 |
% BOM Vector Component Tables in push_pull.c |
|
47 |
N = 0:15; |
|
48 |
x_comp = fix(scalar * cos(2 * pi / 16 * N)) |
|
49 |
y_comp = fix(scalar * sin(2 * pi / 16 * N)) |
branches/ir_branch/behaviors/formation_control/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/bfs_fsm/test_decoy/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/bfs_fsm/test_decoy/decoy.c | ||
---|---|---|
1 |
/** driver for orbit code |
|
2 |
sit and activate bom, let other robot orbit this |
|
3 |
*/ |
|
4 |
|
|
5 |
#include <dragonfly_lib.h> |
|
6 |
#include <wireless.h> |
|
7 |
#include <wl_token_ring.h> |
|
8 |
|
|
9 |
|
|
10 |
|
|
11 |
int main(void) { |
|
12 |
// enable everything |
|
13 |
dragonfly_init(ALL_ON); |
|
14 |
orb_enable(); |
|
15 |
orb_init(); |
|
16 |
orb_set_color(PURPLE); |
|
17 |
wl_init(); |
|
18 |
wl_set_channel(0xF); |
|
19 |
wl_token_ring_register(); |
|
20 |
wl_token_ring_join(); // join token ring |
|
21 |
usb_init(); |
|
22 |
usb_puts("start"); |
|
23 |
usb_puti(wl_get_xbee_id()); |
|
24 |
|
|
25 |
|
|
26 |
while(1) { |
|
27 |
wl_do(); /* do wireless */ |
|
28 |
} |
|
29 |
|
|
30 |
orb_set_color(RED); |
|
31 |
usb_puts("end"); |
|
32 |
|
|
33 |
return 0; |
|
34 |
} |
branches/ir_branch/behaviors/bfs_fsm/Makefile | ||
---|---|---|
1 |
# this is a local makefile |
|
2 |
|
|
3 |
# Relative path to the root directory (containing lib directory) |
|
4 |
ifndef COLONYROOT |
|
5 |
COLONYROOT := .. |
|
6 |
|
|
7 |
# Target file name (without extension). |
|
8 |
TARGET = main |
|
9 |
|
|
10 |
# Uncomment this to use the wireless library |
|
11 |
USE_WIRELESS = 1 |
|
12 |
|
|
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 |
|
|
16 |
else |
|
17 |
COLONYROOT := ../$(COLONYROOT) |
|
18 |
endif |
|
19 |
|
|
20 |
include $(COLONYROOT)/Makefile |
branches/ir_branch/behaviors/bfs_fsm/bfs_fsm.h | ||
---|---|---|
1 |
// BFS FSM header file |
|
2 |
|
|
3 |
|
|
4 |
#ifndef _BFS_FSM_H_ |
|
5 |
#define _BFS_FSM_H_ |
|
6 |
|
|
7 |
//The States: |
|
8 |
#define BFS_SEEK 12 //do run around |
|
9 |
#define BFS_FOLLOW 13 //follow other robots to location |
|
10 |
#define BFS_ORBIT 15 //Orbit robot |
|
11 |
#define BFS_STOP 16 //Stop. The default and ending state |
|
12 |
|
|
13 |
|
|
14 |
#define BFS_STRAIGHT_SPEED 170 |
|
15 |
|
|
16 |
#define BFS_SLOW_DISTANCE 250 |
|
17 |
#define BFS_SLOW_SPEED 160 |
|
18 |
|
|
19 |
#define BFS_ORBIT_DISTANCE 175 /* distance to start orbit around robot */ |
|
20 |
|
|
21 |
#define BFS_STOP_DISTANCE 120 /* distance to stop for object */ |
|
22 |
|
|
23 |
#define BFS_MAX_ROBOTS 20 /* max id of robot in project */ |
|
24 |
|
|
25 |
#define BFS_NO_VAL 255 |
|
26 |
|
|
27 |
#define BFS_CHECK_ID_TIME 100 |
|
28 |
|
|
29 |
|
|
30 |
|
|
31 |
int bfs_state; /*State machine variable.*/ |
|
32 |
int bfs_speed; |
|
33 |
|
|
34 |
int bfs_otherRobot; /* the robot we are seeking */ |
|
35 |
int bfs_my_id; /* my wireless id */ |
|
36 |
int bfs_follow_id; /* robot to follow */ |
|
37 |
int bfs_check_id; /* timer to check robot id to follow */ |
|
38 |
|
|
39 |
|
|
40 |
int bfs_pControl; /*Proportional control variable, determines turn direction.*/ |
|
41 |
int bfs_d1,bfs_d2,bfs_d3,bfs_d4,bfs_d5; /*The five distances taken in by IR.*/ |
|
42 |
int bfs_bom; /* bom data */ |
|
43 |
int bfs_bom_count; |
|
44 |
|
|
45 |
#define BFS_MAX_BOM_COUNT 5 /* number of missing bom packets before reverting to seek state */ |
|
46 |
|
|
47 |
/* bfs_init |
|
48 |
argument: robot_id that you want to find |
|
49 |
notes: must call before bfs_fsm |
|
50 |
*/ |
|
51 |
void bfs_init(int robot); |
|
52 |
|
|
53 |
|
|
54 |
/* bfs_fsm |
|
55 |
argument: none |
|
56 |
notes: call in a while loop to perform FSM action |
|
57 |
*/ |
|
58 |
void bfs_fsm(void); |
|
59 |
|
|
60 |
#endif |
branches/ir_branch/behaviors/bfs_fsm/orbit_fsm.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include <wireless.h> |
|
3 |
#include <wl_token_ring.h> |
|
4 |
#include "orbit_fsm.h" |
|
5 |
#include "bfs_fsm.h" |
|
6 |
|
|
7 |
/* Used to orbit a robot |
|
8 |
|
|
9 |
Must be within BOM range of robot before activating |
|
10 |
|
|
11 |
orbit_theta_stop requires encoders, and is incomplete |
|
12 |
|
|
13 |
Assumes: |
|
14 |
both robots are already in a token ring |
|
15 |
*/ |
|
16 |
|
|
17 |
|
|
18 |
/* private function prototype */ |
|
19 |
void orbit_evaluate_state(void); |
|
20 |
|
|
21 |
|
|
22 |
/* primary init */ |
|
23 |
void orbit_init(int robot) { |
|
24 |
//range_init(); |
|
25 |
//analog_init(1); |
|
26 |
//motors_init(); |
|
27 |
//orb_init(); |
|
28 |
//orb_enable(); |
|
29 |
//usb_init(); |
|
30 |
|
|
31 |
/*Start in the start state, ORBIT_SEEK */ |
|
32 |
orbit_state = ORBIT_INSERTION; |
|
33 |
|
|
34 |
orbit_otherRobot = robot; // set which robot to seek and orbit |
|
35 |
|
|
36 |
orbit_pControl=0; |
|
37 |
orbit_bom = bfs_bom; |
|
38 |
orbit_theta = 0; |
|
39 |
orbit_theta_stop = 1000; |
|
40 |
|
|
41 |
/*Initialize distances to zero.*/ |
|
42 |
orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000; |
|
43 |
|
|
44 |
} |
|
45 |
/* secondary init */ |
|
46 |
void orbit_init_theta(int robot,int theta) { |
|
47 |
//range_init(); |
|
48 |
//analog_init(1); |
|
49 |
//motors_init(); |
|
50 |
//orb_init(); |
|
51 |
//orb_enable(); |
|
52 |
//usb_init(); |
|
53 |
|
|
54 |
/*Start in the start state, ORBIT_INSERTION */ |
|
55 |
orbit_state = ORBIT_INSERTION; |
|
56 |
|
|
57 |
orbit_otherRobot = robot; // set which robot to seek and orbit |
|
58 |
|
|
59 |
orbit_pControl=0; |
|
60 |
orbit_bom = bfs_bom; |
|
61 |
orbit_theta = 0; |
|
62 |
orbit_theta_stop = (theta>0)?theta:-1; // check theta_stop |
|
63 |
|
|
64 |
/*Initialize distances to zero.*/ |
|
65 |
orbit_d1=1000; orbit_d2=1000; orbit_d3=1000; orbit_d4=1000; orbit_d5=1000; |
|
66 |
|
|
67 |
} |
|
68 |
|
|
69 |
/*The main function, call this to update states as frequently as possible.*/ |
|
70 |
void orbit_fsm(void) { |
|
71 |
|
|
72 |
/*The following lines ensure that undefined (-1) values |
|
73 |
will not update the distances.*/ |
|
74 |
int temp; |
|
75 |
|
|
76 |
//temp=range_read_distance(IR1); |
|
77 |
//orbit_d1=(temp == -1) ? orbit_d1 : temp; |
|
78 |
|
|
79 |
temp=range_read_distance(IR2); |
|
80 |
orbit_d2=(temp == -1) ? orbit_d2 : temp; |
|
81 |
|
|
82 |
//temp=range_read_distance(IR3); |
|
83 |
//orbit_d3=(temp == -1) ? orbit_d3 : temp; |
|
84 |
|
|
85 |
//temp=range_read_distance(IR4); |
|
86 |
//orbit_d4=(temp == -1) ? orbit_d4 : temp; |
|
87 |
|
|
88 |
//temp=range_read_distance(IR5); |
|
89 |
//orbit_d5=(temp == -1) ? orbit_d5 : temp; |
|
90 |
|
|
91 |
wl_do(); // update wireless |
|
92 |
|
|
93 |
// get bom reading |
|
94 |
temp = wl_token_get_my_sensor_reading(orbit_otherRobot); |
|
95 |
orbit_bom = (temp == -1) ? orbit_bom : temp; |
|
96 |
|
|
97 |
// modify bom reading so right is negative, left is positive |
|
98 |
if (orbit_bom <= 12) |
|
99 |
orbit_bom -= 4; |
|
100 |
else |
|
101 |
orbit_bom -= 20; |
|
102 |
|
|
103 |
|
|
104 |
if (orbit_state == ORBIT_SEEK && orbit_d2 < ORBIT_DISTANCE) |
|
105 |
orbit_state = ORBIT_INSERTION; // begin to orbit |
|
106 |
if (orbit_state == ORBIT_INSERTION && |
|
107 |
(orbit_bom == ORBIT_DIRECTION || (orbit_bom + 1) == ORBIT_DIRECTION || (orbit_bom - 1) == ORBIT_DIRECTION)) |
|
108 |
orbit_state = ORBIT_ORBITING; // orbit achieved |
|
109 |
if (orbit_state == ORBIT_ORBITING && ((orbit_d2 < ORBIT_STOP_DISTANCE) |
|
110 |
|| (orbit_theta >= orbit_theta_stop))) |
|
111 |
orbit_state = ORBIT_STOP; // orbit obstructed |
|
112 |
|
|
113 |
// evaluate state |
|
114 |
orbit_evaluate_state(); |
|
115 |
} |
|
116 |
|
|
117 |
|
|
118 |
//Acts on state change. |
|
119 |
void orbit_evaluate_state(){ |
|
120 |
switch(orbit_state){ |
|
121 |
case(ORBIT_SEEK): orb_set_color(RED); |
|
122 |
// move towards robot |
|
123 |
orbit_pControl = orbit_bom*10; |
|
124 |
move(ORBIT_STRAIGHT_SPEED,orbit_pControl); |
|
125 |
break; |
|
126 |
|
|
127 |
case(ORBIT_INSERTION): orb_set_color(GREEN); |
|
128 |
// rotate into orbit, perpendicular to other robot |
|
129 |
orbit_pControl = -ORBIT_DIRECTION*10; |
|
130 |
move(ORBIT_STRAIGHT_SPEED/2,orbit_pControl); |
|
131 |
break; |
|
132 |
|
|
133 |
case(ORBIT_ORBITING): orb_set_color(BLUE); |
|
134 |
// go straight with slight rotation |
|
135 |
if (orbit_bom == ORBIT_DIRECTION) |
|
136 |
orbit_pControl = ORBIT_DIRECTION; |
|
137 |
else if (orbit_bom < ORBIT_DIRECTION) |
|
138 |
orbit_pControl = ORBIT_DIRECTION-ORBIT_CORRECTION; |
|
139 |
else |
|
140 |
orbit_pControl = ORBIT_DIRECTION+ORBIT_CORRECTION; |
|
141 |
|
|
142 |
move(ORBIT_STRAIGHT_SPEED,orbit_pControl); |
|
143 |
break; |
|
144 |
|
|
145 |
case(ORBIT_STOP): orb_set_color(YELLOW); |
|
146 |
move(0,0); |
|
147 |
break; |
|
148 |
|
|
149 |
default: orb_set_color(YELLOW); |
|
150 |
/*Should never get here, so stop.*/ |
|
151 |
move(0,0); |
|
152 |
break; |
|
153 |
} |
|
154 |
} |
|
155 |
|
|
156 |
|
branches/ir_branch/behaviors/bfs_fsm/orbit_fsm.h | ||
---|---|---|
1 |
//Obstacle Avoid Numbers |
|
2 |
|
|
3 |
|
|
4 |
#ifndef _ORBIT_FSM_H_ |
|
5 |
#define _ORBIT_FSM_H_ |
|
6 |
|
|
7 |
//The States: |
|
8 |
#define ORBIT_SEEK 12 //Move straight towards robot |
|
9 |
#define ORBIT_INSERTION 13 //Rotate into orbit |
|
10 |
#define ORBIT_ORBITING 15 //Orbit robot |
|
11 |
#define ORBIT_STOP 16 //Stop. The default and ending state |
|
12 |
|
|
13 |
|
|
14 |
#define ORBIT_STRAIGHT_SPEED 160 |
|
15 |
#define ORBIT_CORRECTION 9 |
|
16 |
|
|
17 |
|
|
18 |
#define ORBIT_DIRECTION 4 // if 4, then orbit to the right; if -4, then orbit to the left |
|
19 |
|
|
20 |
#define ORBIT_DISTANCE 175 // orbit distance as measured by rangefinders |
|
21 |
#define ORBIT_STOP_DISTANCE 120 |
|
22 |
|
|
23 |
|
|
24 |
int orbit_state; /*State machine variable.*/ |
|
25 |
|
|
26 |
int orbit_otherRobot; /* must be set prior to running FSM */ |
|
27 |
|
|
28 |
|
|
29 |
int orbit_pControl; /*Proportional control variable, determines turn direction.*/ |
|
30 |
int orbit_d1,orbit_d2,orbit_d3,orbit_d4,orbit_d5; /*The five distances taken in by IR.*/ |
|
31 |
int orbit_bom; /* bom data */ |
|
32 |
int orbit_theta; /* number of degrees traveled in orbit */ |
|
33 |
int orbit_theta_stop; /* stop condition in number of degrees traveled */ |
|
34 |
|
|
35 |
/* orbit_init |
|
36 |
argument: robot_id that you want to orbit |
|
37 |
notes: must call before orbit_fsm |
|
38 |
*/ |
|
39 |
void orbit_init(int robot); |
|
40 |
|
|
41 |
/* orbit_init_theta |
|
42 |
argument: robot_id that you want to orbit |
|
43 |
theta that you want to stop orbiting at |
|
44 |
notes: must call before orbit_fsm |
|
45 |
*/ |
|
46 |
void orbit_init_theta(int robot,int theta); |
|
47 |
|
|
48 |
/* orbit_fsm |
|
49 |
argument: none |
|
50 |
notes: call in a while loop to perform FSM action |
|
51 |
*/ |
|
52 |
void orbit_fsm(void); |
|
53 |
|
|
54 |
#endif |
branches/ir_branch/behaviors/bfs_fsm/driver.c | ||
---|---|---|
1 |
/** driver for bfs swarming |
|
2 |
execute bfs behavior |
|
3 |
*/ |
|
4 |
|
|
5 |
#include <dragonfly_lib.h> |
|
6 |
#include <wireless.h> |
|
7 |
#include <wl_token_ring.h> |
|
8 |
#include "bfs_fsm.h" |
|
9 |
|
|
10 |
|
|
11 |
int main(void) { |
|
12 |
// enable everything |
|
13 |
dragonfly_init(ALL_ON); |
|
14 |
orb_enable(); |
|
15 |
orb_init(); |
|
16 |
orb_set_color(PURPLE); |
|
17 |
wl_init(); |
|
18 |
wl_set_channel(0xF); |
|
19 |
wl_token_ring_register(); |
|
20 |
wl_token_ring_join(); // join token ring |
|
21 |
usb_init(); |
|
22 |
|
|
23 |
|
|
24 |
bfs_init(6); // set robot_id to find |
|
25 |
|
|
26 |
usb_puts("start 4\n\r"); |
|
27 |
|
|
28 |
while(1) { |
|
29 |
bfs_fsm(); // do bfs |
|
30 |
} |
|
31 |
|
|
32 |
return 0; |
|
33 |
} |
branches/ir_branch/behaviors/bfs_fsm/bfs_fsm.c | ||
---|---|---|
1 |
#include <dragonfly_lib.h> |
|
2 |
#include <wireless.h> |
|
3 |
#include <wl_token_ring.h> |
|
4 |
#include "queue.h" |
|
5 |
#include <string.h> |
|
6 |
#include "bfs_fsm.h" |
|
7 |
#include "smart_run_around_fsm.h" |
|
8 |
#include "orbit_fsm.h" |
|
9 |
|
|
10 |
/* Used to find a robot (or other object) |
|
11 |
|
|
12 |
Uses bom and token ring and rangefinders |
|
13 |
|
|
14 |
Assumes: |
|
15 |
robots are already in a token ring |
|
16 |
*/ |
|
17 |
|
|
18 |
|
|
19 |
/* private function prototypes */ |
|
20 |
void bfs_evaluate_state(void); |
|
21 |
int bfs_follow(void); |
|
22 |
|
|
23 |
|
|
24 |
/* init */ |
|
25 |
void bfs_init(int robot) { |
|
26 |
range_init(); |
|
27 |
analog_init(1); |
|
28 |
motors_init(); |
|
29 |
orb_init(); |
|
30 |
orb_enable(); |
|
31 |
usb_init(); |
|
32 |
run_around_init(); |
|
33 |
|
|
34 |
/*Start in the start state, BFS_SEEK */ |
|
35 |
bfs_state = BFS_SEEK; |
|
36 |
|
|
37 |
bfs_otherRobot = robot; // set which robot to seek |
|
38 |
bfs_my_id = wl_get_xbee_id(); |
|
39 |
bfs_follow_id = -1; |
|
40 |
|
|
41 |
bfs_pControl=0; |
|
42 |
bfs_bom = 0; |
|
43 |
bfs_bom_count = 0; |
|
44 |
|
|
45 |
|
|
46 |
/*Initialize distances to zero.*/ |
|
47 |
bfs_d1=1000; bfs_d2=1000; bfs_d3=1000; bfs_d4=1000; bfs_d5=1000; |
|
48 |
|
|
49 |
} |
|
50 |
|
|
51 |
/*The main function, call this to update states as frequently as possible.*/ |
|
52 |
void bfs_fsm(void) { |
|
53 |
|
|
54 |
/* reset some values */ |
|
55 |
bfs_speed = BFS_STRAIGHT_SPEED; |
|
56 |
|
|
57 |
/*The following lines ensure that undefined (-1) values |
|
58 |
will not update the distances.*/ |
|
59 |
int temp; |
|
60 |
|
|
61 |
wl_do(); // update wireless |
|
62 |
usb_puts("\n\rwl_do"); |
|
63 |
usb_puts("|state="); |
|
64 |
usb_puti(bfs_state); |
|
65 |
usb_puts("|bfs_follow="); |
|
66 |
usb_puti(bfs_follow_id); |
|
67 |
usb_puts("|"); |
|
68 |
|
|
69 |
|
|
70 |
if (bfs_state == BFS_SEEK) { |
|
71 |
bfs_follow_id = bfs_follow(); |
|
72 |
wl_do(); // update wireless |
|
73 |
usb_puti(bfs_follow_id); |
|
74 |
if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) { |
|
75 |
bfs_state = BFS_FOLLOW; // move to follow state |
|
76 |
bfs_d2=1000; |
|
77 |
bfs_check_id=0; |
|
78 |
} |
|
79 |
} |
|
80 |
if (bfs_state == BFS_FOLLOW) { |
|
81 |
/* check robot to follow */ |
|
82 |
if (++bfs_check_id > BFS_CHECK_ID_TIME) { |
|
83 |
bfs_check_id=0; |
|
84 |
temp = bfs_follow(); |
|
85 |
wl_do(); |
|
86 |
if (temp == BFS_NO_VAL) { |
|
87 |
bfs_state = BFS_SEEK; |
|
88 |
return; |
|
89 |
} else if (temp != bfs_follow_id) |
|
90 |
bfs_follow_id = temp; |
|
91 |
} |
|
92 |
// get bom reading |
|
93 |
temp = wl_token_get_my_sensor_reading(bfs_follow_id); |
|
94 |
if (temp == -1 || temp > 16) { |
|
95 |
bfs_bom_count++; |
|
96 |
if (bfs_bom_count > BFS_MAX_BOM_COUNT) { |
|
97 |
bfs_state = BFS_SEEK; |
|
98 |
bfs_bom = 255; |
|
99 |
bfs_bom_count = 0; |
|
100 |
} |
|
101 |
} |
|
102 |
else { |
|
103 |
bfs_bom = temp; |
|
104 |
|
|
105 |
// modify bom reading so right is negative, left is positive |
|
106 |
if (bfs_bom <= 12) |
|
107 |
bfs_bom -= 4; |
|
108 |
else |
|
109 |
bfs_bom -= 20; |
|
110 |
|
|
111 |
bfs_pControl = bfs_bom*4; |
|
112 |
usb_puti(bfs_bom); |
|
113 |
|
|
114 |
|
|
115 |
// get range reading for front |
|
116 |
temp=range_read_distance(IR2); |
|
117 |
bfs_d2=(temp == -1) ? bfs_d2 : temp; |
|
118 |
usb_puti(bfs_d2); |
|
119 |
|
|
120 |
if (bfs_d2 < BFS_SLOW_DISTANCE) |
|
121 |
bfs_speed = BFS_SLOW_SPEED; |
|
122 |
|
|
123 |
if (bfs_d2 < BFS_ORBIT_DISTANCE) { |
|
124 |
if (bfs_follow_id != bfs_otherRobot) { |
|
125 |
// check if we can now see a robot that's closer to our goal |
|
126 |
temp = bfs_follow(); |
|
127 |
wl_do(); |
|
128 |
if (temp == BFS_NO_VAL || temp == bfs_follow_id) |
|
129 |
bfs_state = BFS_SEEK; |
|
130 |
else if (temp != bfs_follow_id) |
|
131 |
bfs_follow_id = temp; |
|
132 |
return; // redo this function in light of the state changes |
|
133 |
} else { |
|
134 |
// orbit the robot |
|
135 |
bfs_state = BFS_ORBIT; // move to orbit state |
|
136 |
orbit_init(bfs_follow_id); |
|
137 |
bfs_d2=1000; |
|
138 |
} |
|
139 |
} |
|
140 |
} |
|
141 |
} |
|
142 |
if (bfs_state == BFS_ORBIT) { |
|
143 |
usb_puts("orbiting\n"); |
|
144 |
orbit_state = ORBIT_INSERTION; |
|
145 |
while(orbit_state != ORBIT_STOP) |
|
146 |
{ orbit_fsm(); } |
|
147 |
bfs_state = BFS_STOP; // move to stop state |
|
148 |
} |
|
149 |
if (bfs_state == BFS_STOP && bfs_follow_id != bfs_otherRobot) { |
|
150 |
bfs_state = BFS_SEEK; // seek the otherRobot |
|
151 |
return; |
|
152 |
} |
|
153 |
|
|
154 |
// evaluate state |
|
155 |
bfs_evaluate_state(); |
|
156 |
} |
|
157 |
|
|
158 |
|
|
159 |
//Acts on state change. |
|
160 |
void bfs_evaluate_state(){ |
|
161 |
switch(bfs_state){ |
|
162 |
case(BFS_SEEK): orb_set_color(RED); |
|
163 |
// move around |
|
164 |
run_around_FSM(); // note: better to just incorporate into this file later one |
|
165 |
break; |
|
166 |
|
|
167 |
case(BFS_FOLLOW): orb_set_color(GREEN); |
|
168 |
// follow another robot |
|
169 |
move(bfs_speed,bfs_pControl); |
|
170 |
break; |
|
171 |
|
|
172 |
case(BFS_ORBIT): //orb_set_color(BLUE); |
|
173 |
// orbit a robot |
|
174 |
//orbit_fsm(); |
|
175 |
//move(0,0); |
|
176 |
break; |
|
177 |
|
|
178 |
case(BFS_STOP): orb_set_color(YELLOW); |
|
179 |
// stop |
|
180 |
move(0,0); |
|
181 |
break; |
|
182 |
|
|
183 |
default: orb_set_color(MAGENTA); |
|
184 |
/*Should never get here, so stop.*/ |
|
185 |
move(0,0); |
|
186 |
break; |
|
187 |
} |
|
188 |
} |
|
189 |
|
|
190 |
/* find a robot to follow using BFS |
|
191 |
ported from colonybfs by Felix |
|
192 |
*/ |
|
193 |
int bfs_follow() |
|
194 |
{ |
|
195 |
|
|
196 |
/* pseudocode for BFS |
|
197 |
|
|
198 |
procedure bfs(v) |
|
199 |
q := make_queue() |
|
200 |
enqueue(q,v) |
Also available in: Unified diff