Revision 615
It worked in initial trials without the orbit code, and worked a couple of times after the addition of the orbit code. Still not stable though.
branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.c | ||
---|---|---|
5 | 5 |
#include <string.h> |
6 | 6 |
#include "bfs_fsm.h" |
7 | 7 |
#include "smart_run_around_fsm.h" |
8 |
//#include "orbit_fsm.h"
|
|
8 |
#include "orbit_fsm.h" |
|
9 | 9 |
|
10 | 10 |
/* Used to find a robot (or other object) |
11 | 11 |
|
... | ... | |
28 | 28 |
motors_init(); |
29 | 29 |
orb_init(); |
30 | 30 |
orb_enable(); |
31 |
//usb_init();
|
|
31 |
usb_init(); |
|
32 | 32 |
run_around_init(); |
33 | 33 |
|
34 | 34 |
/*Start in the start state, BFS_SEEK */ |
... | ... | |
56 | 56 |
int temp; |
57 | 57 |
|
58 | 58 |
wl_do(); // update wireless |
59 |
usb_puts("\n\rwl_do"); |
|
60 |
usb_puts("|state="); |
|
61 |
usb_puti(bfs_state); |
|
62 |
usb_puts("|"); |
|
59 | 63 |
|
60 | 64 |
|
61 | 65 |
if (bfs_state == BFS_SEEK) { |
62 | 66 |
bfs_follow_id = bfs_follow(); |
63 |
if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) |
|
67 |
usb_puti(bfs_follow_id); |
|
68 |
if (bfs_follow_id != BFS_NO_VAL && bfs_follow_id < BFS_MAX_ROBOTS) { |
|
64 | 69 |
bfs_state = BFS_FOLLOW; // move to follow state |
70 |
bfs_d2=1000; |
|
71 |
} |
|
65 | 72 |
} |
66 | 73 |
if (bfs_state == BFS_FOLLOW) { |
67 | 74 |
// get bom reading |
... | ... | |
76 | 83 |
} |
77 | 84 |
else { |
78 | 85 |
bfs_bom = temp; |
79 |
|
|
86 |
|
|
80 | 87 |
// modify bom reading so right is negative, left is positive |
81 | 88 |
if (bfs_bom <= 12) |
82 | 89 |
bfs_bom -= 4; |
... | ... | |
84 | 91 |
bfs_bom -= 20; |
85 | 92 |
|
86 | 93 |
bfs_pControl = bfs_bom*4; |
94 |
usb_puti(bfs_bom); |
|
87 | 95 |
|
88 | 96 |
|
89 | 97 |
// get range reading for front |
90 | 98 |
temp=range_read_distance(IR2); |
91 | 99 |
bfs_d2=(temp == -1) ? bfs_d2 : temp; |
100 |
usb_puti(bfs_d2); |
|
92 | 101 |
|
93 | 102 |
if (bfs_d2 < BFS_ORBIT_DISTANCE) { |
94 | 103 |
bfs_state = BFS_ORBIT; // move to orbit state |
95 |
//orbit_init(bfs_follow_id); |
|
104 |
orbit_init(bfs_follow_id); |
|
105 |
bfs_d2=1000; |
|
96 | 106 |
} |
97 | 107 |
} |
98 | 108 |
} |
99 |
else if (bfs_state == BFS_ORBIT) { |
|
100 |
|
|
101 |
// get range reading for front |
|
102 |
temp=range_read_distance(IR2); |
|
103 |
bfs_d2=(temp == -1) ? bfs_d2 : temp; |
|
104 |
|
|
105 |
if (bfs_d2 < BFS_ORBIT_DISTANCE) |
|
106 |
bfs_state = BFS_STOP; // move to stop state |
|
109 |
if (bfs_state == BFS_ORBIT) { |
|
110 |
while(orbit_state != ORBIT_STOP) |
|
111 |
{ orbit_fsm(); } |
|
112 |
bfs_state = BFS_STOP; // move to stop state |
|
107 | 113 |
} |
108 | 114 |
|
109 | 115 |
// evaluate state |
... | ... | |
124 | 130 |
move(BFS_STRAIGHT_SPEED,bfs_pControl); |
125 | 131 |
break; |
126 | 132 |
|
127 |
case(BFS_ORBIT): orb_set_color(BLUE); |
|
133 |
case(BFS_ORBIT): //orb_set_color(BLUE);
|
|
128 | 134 |
// orbit a robot |
129 | 135 |
//orbit_fsm(); |
130 |
move(0,0); |
|
136 |
//move(0,0);
|
|
131 | 137 |
break; |
132 | 138 |
|
133 | 139 |
case(BFS_STOP): orb_set_color(YELLOW); |
... | ... | |
135 | 141 |
move(0,0); |
136 | 142 |
break; |
137 | 143 |
|
138 |
default: orb_set_color(YELLOW);
|
|
144 |
default: orb_set_color(MAGENTA);
|
|
139 | 145 |
/*Should never get here, so stop.*/ |
140 | 146 |
move(0,0); |
141 | 147 |
break; |
branches/orbit/code/behaviors/bfs_fsm/bfs_fsm.h | ||
---|---|---|
15 | 15 |
|
16 | 16 |
#define BFS_ORBIT_DISTANCE 120 /* distance to start orbit around robot */ |
17 | 17 |
|
18 |
#define BFS_STOP_DISTANCE 100 /* distance to stop for object */ |
|
19 |
|
|
18 | 20 |
#define BFS_MAX_ROBOTS 20 /* max id of robot in project */ |
19 | 21 |
|
20 | 22 |
#define BFS_NO_VAL 255 |
branches/orbit/code/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_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/3,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/orbit/code/behaviors/bfs_fsm/smart_run_around_fsm.c | ||
---|---|---|
1 |
#include "dragonfly_lib.h"
|
|
1 |
#include <dragonfly_lib.h>
|
|
2 | 2 |
#include "smart_run_around_fsm.h" |
3 | 3 |
|
4 | 4 |
/*A simple behavior for navigating in an environment, i.e. avoiding walls and getting stuck. |
branches/orbit/code/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 250 // orbit distance as measured by rangefinders |
|
21 |
|
|
22 |
|
|
23 |
int orbit_state; /*State machine variable.*/ |
|
24 |
|
|
25 |
int orbit_otherRobot; /* must be set prior to running FSM */ |
|
26 |
|
|
27 |
|
|
28 |
int orbit_pControl; /*Proportional control variable, determines turn direction.*/ |
|
29 |
int orbit_d1,orbit_d2,orbit_d3,orbit_d4,orbit_d5; /*The five distances taken in by IR.*/ |
|
30 |
int orbit_bom; /* bom data */ |
|
31 |
int orbit_theta; /* number of degrees traveled in orbit */ |
|
32 |
int orbit_theta_stop; /* stop condition in number of degrees traveled */ |
|
33 |
|
|
34 |
/* orbit_init |
|
35 |
argument: robot_id that you want to orbit |
|
36 |
notes: must call before orbit_fsm |
|
37 |
*/ |
|
38 |
void orbit_init(int robot); |
|
39 |
|
|
40 |
/* orbit_init_theta |
|
41 |
argument: robot_id that you want to orbit |
|
42 |
theta that you want to stop orbiting at |
|
43 |
notes: must call before orbit_fsm |
|
44 |
*/ |
|
45 |
void orbit_init_theta(int robot,int theta); |
|
46 |
|
|
47 |
/* orbit_fsm |
|
48 |
argument: none |
|
49 |
notes: call in a while loop to perform FSM action |
|
50 |
*/ |
|
51 |
void orbit_fsm(void); |
|
52 |
|
|
53 |
#endif |
Also available in: Unified diff